Merge branch 'master' of https://github.com/ttwards/motor (939f1db7) · Commits · Wenxi XU / Mambo

drivers/motor/mi/motor_mi.c

+139 −86

Original line number	Diff line number	Diff line
		@@ -2,6 +2,7 @@
		#include "zephyr/device.h"
		#include "zephyr/drivers/can.h"
		#include "zephyr/types.h"
		#include <stdbool.h>
		#include <stdint.h>
		#include <string.h>
		#include <sys/_stdint.h>
		@@ -24,12 +25,13 @@ static float uint16_to_float(uint16_t x, float x_min, float x_max, int bits) {
		int float_to_uint(float x, float x_min, float x_max, int bits) {
		float span = x_max - x_min;
		float offset = x_min;
		if(x > x_max) x=x_max;
		else if(x < x_min) x= x_min;
		if (x > x_max)
		x = x_max;
		else if (x < x_min)
		x = x_min;
		return (int)((x - offset) * ((float)((1 << bits) - 1)) / span);
		}


		static int get_can_id(const struct device *dev) {
		const struct mi_motor_cfg *cfg = dev->config;
		for (int i = 0; i < CAN_COUNT; i++) {
		@@ -53,6 +55,7 @@ int mi_init(const struct device *dev) {
		int can_id = get_can_id(dev);
		k_sem_init(&tx_queue_sem[can_id], 3, 3); // 初始化信号量
		if (!device_is_ready(cfg->common.phy)) {
		LOG_ERR("CAN device not ready");
		return -1;
		}
		if (k_work_busy_get(&mi_init_work) != 0) {
		@@ -61,7 +64,7 @@ int mi_init(const struct device *dev) {
		k_work_queue_init(&mi_work_queue);

		mi_tx_timer.expiry_fn = mi_isr_init_handler;
		k_timer_start(&mi_tx_timer, K_MSEC(600), K_MSEC(1));
		k_timer_start(&mi_tx_timer, K_MSEC(600), K_MSEC(2));
		k_timer_user_data_set(&mi_tx_timer, &mi_init_work);
		return 0;
		}
		@@ -125,18 +128,53 @@ void mi_motor_control(const struct device *dev, enum motor_cmd cmd) {
		int can_id = get_can_id(dev);
		struct mi_can_id mi_can_id = (struct mi_can_id )&(frame.id);
		mi_can_id->id = cfg->common.id;
		int err = 0;
		switch (cmd) {
		case ENABLE_MOTOR:
		data->online = true;

		mi_can_id->mi_msg_mode = Communication_Type_MotorEnable;
		if (k_sem_take(&tx_queue_sem[can_id], K_NO_WAIT) == 0) {
		err = can_send(cfg->common.phy, &frame, K_NO_WAIT, can_tx_callback,
		&tx_queue_sem[can_id]);
		} else {
		LOG_ERR("motor control fail to get sem");
		}
		if (err != 0) {
		LOG_ERR("Failed to send CAN frame: %d", err);
		} else {
		data->online = true;
		data->enabled = true;
		}
		break;
		case DISABLE_MOTOR:
		data->online = false;

		mi_can_id->mi_msg_mode = Communication_Type_MotorStop;
		if (k_sem_take(&tx_queue_sem[can_id], K_NO_WAIT) == 0) {
		err = can_send(cfg->common.phy, &frame, K_NO_WAIT, can_tx_callback,
		&tx_queue_sem[can_id]);
		} else {
		LOG_ERR("motor control fail to get sem");
		}
		if (err != 0) {
		LOG_ERR("Failed to send CAN frame: %d", err);
		} else {
		data->online = false;
		data->enabled = false;
		}
		break;
		case SET_ZERO_OFFSET:
		mi_can_id->mi_msg_mode = Communication_Type_SetPosZero;
		frame.data[0] = 0x01;
		if (k_sem_take(&tx_queue_sem[can_id], K_NO_WAIT) == 0) {
		err = can_send(cfg->common.phy, &frame, K_NO_WAIT, can_tx_callback,
		&tx_queue_sem[can_id]);
		} else {
		LOG_ERR("motor control fail to get sem");
		}
		if (err != 0) {
		LOG_ERR("Failed to send CAN frame: %d", err);
		} else {
		}
		break;

		case CLEAR_PID:
		@@ -148,14 +186,6 @@ void mi_motor_control(const struct device *dev, enum motor_cmd cmd) {
		LOG_ERR("Unsupport motor command: %d", cmd);
		return;
		}
		int err = 0;
		if (k_sem_take(&tx_queue_sem[can_id], K_NO_WAIT) == 0) {
		err = can_send(cfg->common.phy, &frame, K_NO_WAIT, can_tx_callback,
		&tx_queue_sem[can_id]);
		}
		if (err != 0) {
		LOG_ERR("Failed to send CAN frame: %d", err);
		}
		}

		static void mi_motor_pack(const struct device dev, struct can_frame frame) {
		@@ -183,7 +213,7 @@ static void mi_motor_pack(const struct device dev, struct can_frame frame) {

		pos_tmp = float_to_uint(data->target_pos, P_MIN, P_MAX, 16);
		vel_tmp = float_to_uint(data->target_radps, V_MIN, V_MAX, 16);
		kp_tmp = float_to_uint(data->params.k_d, KP_MIN, KP_MAX, 16);
		kp_tmp = float_to_uint(data->params.k_p, KP_MIN, KP_MAX, 16);
		kd_tmp = float_to_uint(data->params.k_d, KD_MIN, KD_MAX, 16);
		tor_tmp = float_to_uint(data->target_torque, T_MIN, T_MAX, 16);
		mi_can_id->data = tor_tmp;
		@@ -206,7 +236,6 @@ static void mi_motor_pack(const struct device dev, struct can_frame frame) {
		frame->data[3] = 0;
		memcpy(&frame->data[4], &data->target_radps, 4);


		mi_can_id_fol->mi_msg_mode = Communication_Type_SetSingleParameter;
		frame_follow->dlc = 8;

		@@ -228,7 +257,6 @@ static void mi_motor_pack(const struct device dev, struct can_frame frame) {
		frame->data[3] = 0;
		memcpy(&frame->data[4], &data->limit_cur, 4);


		mi_can_id_fol->mi_msg_mode = Communication_Type_SetSingleParameter;

		frame_follow->dlc = 8;
		@@ -303,6 +331,8 @@ mi_can_id->id = cfg->common.id;
		if (k_sem_take(&tx_queue_sem[can_id], K_NO_WAIT) == 0) {
		err = can_send(cfg->common.phy, &frame, K_NO_WAIT, can_tx_callback,
		&tx_queue_sem[can_id]);
		} else {
		LOG_ERR("motor set mode fail to get sem");
		}
		if (err != 0) {
		LOG_ERR("Failed to send CAN frame: %d", err);
		@@ -375,15 +405,15 @@ static void mi_can_rx_handler(const struct device *can_dev,
		(struct mi_motor_data *)(motor_devices[id]->data);

		if (data->missed_times > 0) {
		if (data->missed_times > 5 && data->online == true) {

		data->missed_times = 0;
		LOG_ERR("Motor %d is back online", id);
		}
		data->missed_times--;
		}
		struct mi_can_id can_id = (struct mi_can_id )&(frame->id);
		if (can_id->mi_msg_mode == Communication_Type_MotorFeedback) {
		data->err = ((can_id->data) >> 8) & 0x1f;
		if (data->err) {
		LOG_ERR("id:%d err:%d", id, data->err);
		}
		data->update = true;
		data->RAWangle = (frame->data[0] << 8) \| (frame->data[1]);
		data->RAWrpm = (frame->data[2] << 8) \| (frame->data[3]);
		@@ -444,15 +474,42 @@ void mi_tx_data_handler(struct k_work *work) {
		// LOG_DBG("mi_tx_data_handler");
		struct can_frame tx_frame[2] = {0};


		tx_frame[0].flags = CAN_FRAME_IDE;
		tx_frame[1].flags = CAN_FRAME_IDE;
		for (int i = 0; i < MOTOR_COUNT; i++) {
		// LOG_INF("begin");
		struct mi_motor_data *data = motor_devices[i]->data;
		const struct mi_motor_cfg *cfg = motor_devices[i]->config;
		if (data->online && data->missed_times <= 5) {
		if (data->online) {
		int can_id = get_can_id(motor_devices[i]);
		if (data->missed_times > 4 && data->enabled == true) {
		LOG_ERR("Motor %d is not responding, missed %d times, setting it "
		"to "
		"offline...",
		i, data->missed_times);

		struct can_frame frame = {0};
		frame.flags = CAN_FRAME_IDE;
		frame.dlc = 8;
		struct mi_can_id mi_can_id = (struct mi_can_id )&(frame.id);
		mi_can_id->id = cfg->common.id;
		int err = 0;
		mi_can_id->mi_msg_mode = Communication_Type_MotorEnable;
		if (k_sem_take(&tx_queue_sem[can_id], K_NO_WAIT) == 0) {
		err = can_send(cfg->common.phy, &frame, K_NO_WAIT, can_tx_callback,
		&tx_queue_sem[can_id]);

		} else {
		LOG_ERR("motor control fail to get sem");
		}
		if (err != 0) {
		LOG_ERR("Failed to send CAN frame: %d", err);
		} else {
		data->online = true;
		data->enabled = true;
		data->missed_times = 0;
		}
		} else {
		mi_motor_pack(motor_devices[i], tx_frame);
		struct tx_frame queued_frame = {
		.can_dev = cfg->common.phy,
		@@ -475,17 +532,12 @@ void mi_tx_data_handler(struct k_work *work) {
		mi_queue_proceed(&mi_can_tx_msgq);
		}
		data->missed_times++;
		if (data->missed_times > 5) {
		LOG_ERR("Motor %d is not responding, missed %d times, setting it "
		"to "
		"offline...",
		i, data->missed_times);
		continue;
		}
		// if (data->missed_times > 3 && data->err > 1) {
		// mi_motor_control(motor_devices[i], CLEAR_ERROR);
		// }
		}
		if(i%2==1){
		k_usleep(500);
		}
		// LOG_INF("end");
		}
		}

		@@ -528,9 +580,10 @@ for (int i = 0; i < MOTOR_COUNT; i++) {

		for (int i = 0; i < MOTOR_COUNT; i++) {
		mi_motor_control(motor_devices[i], ENABLE_MOTOR);
		k_sleep(K_MSEC(2));
		}

		k_timer_start(&mi_tx_timer, K_NO_WAIT, K_MSEC(1));
		k_timer_start(&mi_tx_timer, K_NO_WAIT, K_MSEC(2));
		k_timer_user_data_set(&mi_tx_timer, &mi_tx_data_handle);
		}

drivers/motor/mi/motor_mi.h

+2 −1

Original line number	Diff line number	Diff line
		@@ -72,7 +72,7 @@
		#define Motor_Error 0x00
		#define Motor_OK 0x01

		#define CAN_SEND_STACK_SIZE 2048
		#define CAN_SEND_STACK_SIZE 4096
		#define CAN_SEND_PRIORITY -1

		enum CONTROL_MODE //控制模式定义
		@@ -124,6 +124,7 @@ struct mi_motor_data {
		uint8_t error_code;
		bool online;
		bool update;
		bool enabled;
		struct pid_config params;

		} ;

include/ares/interboard/interboard.c

+100 −137

Original line number	Diff line number	Diff line
		@@ -3,33 +3,26 @@
		#include <stdbool.h>
		#include <stdint.h>
		#include <string.h>
		#include <zephyr/drivers/dma.h>
		#include <zephyr/drivers/spi.h>
		#include <zephyr/kernel.h>
		#include <zephyr/kernel/thread.h>
		#include <zephyr/sys/crc.h>

		LOG_MODULE_REGISTER(interboard, LOG_LEVEL_DBG);
		#define SPI_BUF_SIZE 24
		#define MSG_QUEUE_SIZE 10
		#define RECEIVE_ACK 0x0A
		// #define CONFIG_MASTER_BOARD
		// #define CONFIG_SLAVE_BOARD
		K_MSGQ_DEFINE(interboardtx_msgq, sizeof(Frame_Data), MSG_QUEUE_SIZE, 4);

		K_MSGQ_DEFINE(interboardrxcan_msgq, sizeof(Frame_Data), MSG_QUEUE_SIZE, 4);
		K_MSGQ_DEFINE(interboardrximu_msgq, sizeof(Frame_Data), MSG_QUEUE_SIZE, 4);
		#ifdef CONFIG_MASTER_BOARD
		#define SPI_DEV DT_NODELABEL(spi4)
		#define DMA_DEV DT_NODELABEL(dma2)
		#endif
		#ifdef CONFIG_SLAVE_BOARD
		#define SPI_DEV DT_NODELABEL(spi2)
		#define DMA_DEV DT_NODELABEL(dma1)
		#endif
		const struct device *spi_dev;
		const struct device *dma_dev;
		K_MSGQ_DEFINE(interboardtx_msgq, sizeof(Frame_Data), MSG_QUEUE_SIZE, 4);


		K_THREAD_STACK_DEFINE(interboard_work_queue_stack, SPI_SEND_STACK_SIZE);
		K_WORK_DEFINE(interboard_rx_data_handle, interboard_rx_data_handler);
		K_WORK_DEFINE(interboard_tx_data_handle, interboard_tx_data_handler);
		K_TIMER_DEFINE(interboard_timer, interboard_tx_isr_handler, NULL);


		struct spi_config spi_cfg = {
		.frequency = 10000000,
		.frequency = 10500000,
		#ifdef CONFIG_MASTER_BOARD
		.operation = (spi_operation_t)(SPI_OP_MODE_MASTER \| SPI_LINES_DUAL \|
		SPI_WORD_SET(8) \| SPI_TRANSFER_LSB \|
		@@ -43,24 +36,17 @@ struct spi_config spi_cfg = {
		#endif
		.slave = 0,
		};
		uint8_t msgcounter = 0;
		bool received = true;
		void spi_init(void) {
		spi_dev = DEVICE_DT_GET(SPI_DEV);
		dma_dev = DEVICE_DT_GET(DMA_DEV);
		if (!device_is_ready(spi_dev)) {
		LOG_ERR("SPI device not ready\n");
		return;
		}

		if (!device_is_ready(dma_dev)) {
		LOG_ERR("DMA device not ready\n");
		return;
		}
		}
		struct k_work_q interboard_work_queue;

		const struct device *spi;

		uint16_t calculate_crc16(const uint8_t *data, size_t length) {
		return crc16(0x8005, 0xFFFF, data, length);
		}
		uint8_t msgcounter = 0;
		bool received = true;

		void build_txbuf(uint8_t tx_buffer, InterboardMsgType msg_type, uint8_t data,
		bool is_retransmit) {
		uint8_t is_master = 1;
		@@ -101,68 +87,6 @@ void build_txbuf(uint8_t tx_buffer, InterboardMsgType msg_type, uint8_t data,
		// 设置帧尾
		(uint16_t )ptr = 0x0D0A; // 帧尾 (0x0D0A)
		}

		void tx_thread_func(void p1, void p2, void *p3) {
		Frame_Data tx_data;
		// 预分配发送/接收缓冲
		static uint8_t __aligned(8) tx_buffer_local[SPI_BUF_SIZE];
		static uint8_t __aligned(8) tx_buffer_local_copy[SPI_BUF_SIZE];
		static uint8_t __aligned(8) rx_buffer_local[SPI_BUF_SIZE];

		// 配置 spi_buf
		struct spi_buf tx_buf = {
		.buf = tx_buffer_local,
		.len = SPI_BUF_SIZE,
		};
		struct spi_buf tx_buf_copy = {
		.buf = tx_buffer_local_copy,
		.len = SPI_BUF_SIZE,
		};
		struct spi_buf rx_buf = {
		.buf = rx_buffer_local,
		.len = SPI_BUF_SIZE,
		};
		struct spi_buf_set tx_bufs = {
		.buffers = &tx_buf,
		.count = 1,
		};
		struct spi_buf_set tx_bufs_copy = {
		.buffers = &tx_buf_copy,
		.count = 1,
		};
		struct spi_buf_set rx_bufs = {
		.buffers = &rx_buf,
		.count = 1,
		};
		while (1) {
		if (!received) {
		(tx_buffer_local_copy + 1) = ((tx_buffer_local_copy + 1)) \| 0x40;
		#ifdef CONFIG_MASTER_BOARD
		k_msleep(1);
		#endif
		int error = spi_transceive_cb(spi_dev, &spi_cfg, &tx_bufs_copy, &rx_bufs,
		interboardrx_callback, &rx_bufs);
		if (error != 0) {
		LOG_ERR("re_spi_errorcode: %d", error);
		}
		msgcounter++;
		}
		if (k_msgq_get(&interboardtx_msgq, &tx_data, K_FOREVER) == 0) {

		build_txbuf(tx_buffer_local, tx_data.msg_type, tx_data.data, false);
		int error = spi_transceive_cb(spi_dev, &spi_cfg, &tx_bufs, &rx_bufs,
		interboardrx_callback, rx_buffer_local);
		if (error != 0) {
		LOG_ERR("spi_errorcode: %d", error);
		k_msgq_purge(&interboardtx_msgq);
		}
		msgcounter++;
		memcpy(tx_buffer_local_copy, tx_buffer_local, SPI_BUF_SIZE);
		}
		}
		}
		K_THREAD_DEFINE(interboardtx_thread, 2048, tx_thread_func, NULL, NULL, NULL, 7,
		0, 0);
		void process_rxbuf(uint8_t *rx_buffer, InterboardMsgType msg_type,
		uint8_t *data, bool is_retransmit) {
		uint8_t *ptr = rx_buffer;
		@@ -214,18 +138,31 @@ void process_rxbuf(uint8_t *rx_buffer, InterboardMsgType msg_type,

		memcpy(data, data_local, datalen);
		}
		Frame_Data tx_data_out; // 定时发送的数据
		Frame_Data *tx_data_out_p=&tx_data_out; //定时发送的数据来源指针
		static uint8_t __aligned(8) tx_buffer_local[SPI_BUF_SIZE] = {0};
		static uint8_t __aligned(8) rx_buffer_local[SPI_BUF_SIZE] = {0};
		struct spi_buf tx_buf = {
		.buf = &tx_buffer_local, // Initialize with tx_buffer_local
		.len = SPI_BUF_SIZE,
		};
		struct spi_buf_set tx_bufs = {
		.buffers = &tx_buf,
		.count = 1,
		};
		struct spi_buf rx_buf = {
		.buf = &rx_buffer_local, // Initialize with NULL
		.len = SPI_BUF_SIZE,
		};
		struct spi_buf_set rx_bufs = {
		.buffers = &rx_buf,
		.count = 1,
		};

		K_SEM_DEFINE(rx_thread_sem, 0, 1);

		uint8_t *rx_buf;
		void rx_thread_func(void p1, void p2, void *p3) {
		while (1) {
		// 等待信号量
		k_sem_take(&rx_thread_sem, K_FOREVER);
		// 处理接收到的数据
		void interboard_rx_data_handler(struct k_work *work) {
		Frame_Data rx_data = {0};
		bool is_retransmit = false;
		process_rxbuf(rx_buf, rx_data.msg_type, rx_data.data, is_retransmit);
		process_rxbuf(rx_buffer_local, rx_data.msg_type, rx_data.data, is_retransmit);
		switch (rx_data.msg_type) {
		case INTERBOARD_MSG_CAN:
		k_msgq_put(&interboardrxcan_msgq, &rx_data, K_NO_WAIT);
		@@ -235,12 +172,59 @@ void rx_thread_func(void p1, void p2, void *p3) {
		break;
		}
		}

		void interboardrx_callback(const struct device *spi_dev, int error_code,
		void *data) {
		if (error_code == 0) {
		k_work_submit_to_queue(&interboard_work_queue, &interboard_rx_data_handle);
		#ifdef CONFIG_SLAVE_BOARD
		k_work_submit_to_queue(&interboard_work_queue, &interboard_tx_data_handle);
		#endif
		} else if (error_code != 0) {
		LOG_ERR(" transfer met an error\n");
		//释放spi
		spi_release(spi_dev, &spi_cfg);
		}
		}
		K_THREAD_DEFINE(interboardrx_thread, 2048, rx_thread_func, NULL, NULL, NULL, 7,
		0, 0);

		void interboard_tx_data_handler(struct k_work *work) {
		if (k_msgq_get(&interboardtx_msgq, tx_data_out_p, K_NO_WAIT) == 0) {
		build_txbuf(tx_buffer_local, tx_data_out_p->msg_type, tx_data_out_p->data,
		false);
		msgcounter++;
		}
		int error = spi_transceive_cb(spi, &spi_cfg, &tx_bufs, &rx_bufs,
		interboardrx_callback, rx_buf.buf);
		if (error != 0) {
		LOG_ERR("spi_errorcode: %d", error);
		}
		}

		void interboard_tx_isr_handler(struct k_timer *dummy) {
		#ifdef CONFIG_MASTER_BOARD
		k_work_submit_to_queue(&interboard_work_queue, &interboard_tx_data_handle);
		#endif
		}

		//初始化timer，设置定时器回调函数，设置定时器数据（要提交的工作）
		void interboard_init(struct device *spi_dev) {
		spi = spi_dev;
		if (!device_is_ready(spi_dev)) {
		LOG_ERR("SPI device not ready\n");
		return;
		}
		k_work_queue_init(&interboard_work_queue);
		k_work_queue_start(&interboard_work_queue, interboard_work_queue_stack,
		SPI_SEND_STACK_SIZE, SPI_SEND_PRIORITY, NULL);
		k_timer_start(&interboard_timer, K_MSEC(1), K_MSEC(1));
		#ifdef CONFIG_SLAVE_BOARD
		k_sleep(K_MSEC(1000));
		k_work_submit_to_queue(&interboard_work_queue, &interboard_tx_data_handle);
		#endif
		}

		int interboard_transceive(Frame_Data tx_data, Frame_Data rx_data) {
		k_msgq_put(&interboardtx_msgq, tx_data, K_NO_WAIT);

		switch (tx_data->msg_type) {
		case INTERBOARD_MSG_CAN:
		k_msgq_get(&interboardrxcan_msgq, rx_data, K_FOREVER);
		@@ -251,24 +235,3 @@ int interboard_transceive(Frame_Data tx_data, Frame_Data rx_data) {
		}
		return 0;
		}

		void spi_error_handler(int error_code) {
		LOG_ERR("Error code: %d", error_code);

		//释放spi
		spi_release(spi_dev, &spi_cfg);

		//重新初始化
		spi_init();
		}
		void interboardrx_callback(const struct device *spi_dev, int error_code,
		void *data) {
		if (error_code == 0) {
		// LOG_INF(" transfer done");
		k_sem_give(&rx_thread_sem);
		rx_buf = (uint8_t *)data;
		} else {
		LOG_ERR(" transfer met an error\n");
		spi_error_handler(error_code);
		}
		}
		No newline at end of file

include/ares/interboard/interboard.h

+19 −3

Original line number	Diff line number	Diff line
		@@ -8,6 +8,11 @@
		extern "C" {
		#endif
		#define datalen 16
		#define RECEIVE_ACK 0x0A
		#define SPI_BUF_SIZE 24
		#define MSG_QUEUE_SIZE 10
		#define SPI_SEND_STACK_SIZE 2048
		#define SPI_SEND_PRIORITY -1
		typedef struct {
		uint8_t sync; // 同步头0x5A（固定值）
		uint8_t ctrl; // [7]主1从0 [6]是否为重发
		@@ -39,12 +44,23 @@ typedef struct{
		uint8_t *rxdata;
		} Interboard_Context ;

		uint16_t calculate_crc16(const uint8_t *data, size_t length);
		void build_txbuf(uint8_t tx_buffer, InterboardMsgType msg_type, uint8_t data,
		bool is_retransmit);
		void process_rxbuf(uint8_t *rx_buffer, InterboardMsgType msg_type,
		uint8_t *data, bool is_retransmit) ;

		void tx_thread_func(void p1, void p2, void *p3);
		void interboard_rx_data_handler(struct k_work *work);

		void interboardrx_callback(const struct device spi_dev, int error_code, void data);

		void spi_init(void);
		void interboardrx_callback(const struct device *spi_dev, int error_code,
		void *data);

		void interboard_tx_data_handler(struct k_work *work);
		void interboard_tx_isr_handler(struct k_timer *dummy) ;


		void interboard_init(struct device *spi_dev) ;
		int interboard_transceive(Frame_Data tx_data, Frame_Data rx_data);

		#ifdef __cplusplus

Admin message