重构电机驱动逻辑，优化消息队列处理，更新电机状态管理，简化代码结构 (47deb3a1) · Commits · Wenxi XU / Mambo

drivers/motor/common/common.c

0 → 100644

+129 −0

Original line number	Diff line number	Diff line
		#include <stdint.h>
		#include <zephyr/kernel.h>
		#include <zephyr/device.h>
		#include <zephyr/drivers/can.h>
		#include <zephyr/drivers/motor.h>
		#include <zephyr/logging/log.h>

		LOG_MODULE_REGISTER(motor_common, CONFIG_MOTOR_LOG_LEVEL);

		#include "common.h"

		static struct k_sem tx_queue_sem[CONFIG_CAN_COUNT];
		static struct device *can_devices[CONFIG_CAN_COUNT];

		K_MSGQ_DEFINE(can_tx_msgq, sizeof(struct tx_frame), 4 * CONFIG_CAN_COUNT, 4);

		static void can_tx_entry(void arg1, void arg2, void *arg3);
		K_THREAD_DEFINE(can_tx_thread, 1024, can_tx_entry, NULL, NULL, NULL, 0, 0, 0);

		static bool initialized = false;
		int can_work_init(void)
		{
		k_thread_start(can_tx_thread);
		initialized = true;
		return 0;
		}

		int8_t reg_can_dev(const struct device *dev)
		{
		if (!initialized) {
		can_work_init();
		initialized = true;
		}
		int8_t can_id = get_can_id(dev);
		if (can_id != -1) {
		return can_id;
		}
		for (int i = 0; i < CONFIG_CAN_COUNT; i++) {
		if (can_devices[i] == NULL) {
		can_devices[i] = (struct device *)dev;
		k_sem_init(&tx_queue_sem[i], 3, 3);
		can_start(can_devices[i]);
		return i;
		}
		}
		return -1;
		}

		int8_t get_can_id(const struct device *dev)
		{
		for (int i = 0; i < CONFIG_CAN_COUNT; i++) {
		if (can_devices[i] == dev) {
		return i;
		}
		}
		return -1;
		}

		static void can_tx_callback(const struct device can_dev, int error, void user_data)
		{
		struct k_sem *queue_sem = user_data;
		if (!error) {
		k_sem_give(queue_sem);
		}
		}

		int can_send_queued(const struct device can_dev, struct can_frame frame)
		{
		if (!initialized) {
		return -ENOSYS;
		}

		int err = k_sem_take(&tx_queue_sem[get_can_id(can_dev)], K_NO_WAIT);
		if (err == 0) {
		err = can_send(can_dev, frame, K_NO_WAIT, can_tx_callback,
		&tx_queue_sem[get_can_id(can_dev)]);
		// LOG_ERR("Send CAN frame: %d", err);
		if (err) {
		LOG_ERR("TX queue full, will be put into msgq: %d", err);
		}
		} else if (err < 0) {
		// LOG_ERR("CAN hardware TX queue is full. (err %d)", err);
		struct tx_frame q_frame = {
		.can_dev = can_dev,
		.sem = &tx_queue_sem[get_can_id(can_dev)],
		.frame = *frame,
		};
		err = k_msgq_put(&can_tx_msgq, &q_frame, K_NO_WAIT);
		}
		// int err = can_send(can_dev, frame, K_NO_WAIT, can_tx_callback,
		// &tx_queue_sem[get_can_id(can_dev)]);
		// if (err) {
		// LOG_ERR("Failed to send CAN frame: %d", err);
		// }
		return err;
		}

		void can_tx_entry(void arg1, void arg2, void *arg3)
		{
		struct tx_frame frame;
		int err = 0;
		uint32_t last_time = 0;
		uint16_t failed_times = 0;
		while (!k_msgq_get(&can_tx_msgq, &frame, K_FOREVER)) {
		// LOG_ERR("Get CAN frame from msgq");
		err = k_sem_take(frame.sem, K_NO_WAIT);
		if (err == 0) {
		err = can_send(frame.can_dev, &(frame.frame), K_USEC(100), can_tx_callback,
		frame.sem);
		if (err && k_uptime_get() - last_time > 400) {
		LOG_ERR("Failed to send CAN frame: %d", err);
		last_time = k_uptime_get();
		}
		k_msgq_purge(&can_tx_msgq);
		} else {
		if (failed_times > 127) {
		k_msgq_purge(&can_tx_msgq);
		LOG_ERR("Failed too many times, purge msgq");
		k_sem_give(frame.sem);
		failed_times = 0;
		break;
		}
		k_sleep(K_USEC(50));
		failed_times++;
		}
		}
		}

		SYS_INIT(can_work_init, APPLICATION, CONFIG_MOTOR_INIT_PRIORITY);
		No newline at end of file

drivers/motor/common/common.h

0 → 100644

+15 −0

Original line number	Diff line number	Diff line
		#include <zephyr/kernel.h>
		#include <zephyr/device.h>
		#include <zephyr/drivers/can.h>
		#include <zephyr/drivers/motor.h>

		struct tx_frame {
		const struct device *can_dev;
		struct k_sem *sem;
		struct can_frame frame;
		};

		int8_t get_can_id(const struct device *dev);
		int8_t reg_can_dev(const struct device *dev);

		int can_send_queued(const struct device can_dev, struct can_frame frame);
		No newline at end of file

drivers/motor/dji/dji_macros.h

+3 −4

Original line number	Diff line number	Diff line
		@@ -34,8 +34,7 @@
		DEVICE_DT_GET(DT_PHANDLE(DT_CHILD_BY_IDX(node_id, idx)))
		#define CANPHY_BY_IDX(idx) GET_CANPHY_POINTER_BY_IDX(CAN_BUS_PATH, idx)

		#define MOTOR_COUNT sizeof(motor_devices) / sizeof(motor_devices[0])
		#define CAN_COUNT DT_NUM_INST_STATUS_OKAY(vnd_canbus)
		#define DJI_MOTOR_COUNT sizeof(motor_devices) / sizeof(motor_devices[0])

		#define GET_CAN_CHANNEL_IDT(node_id) DT_PHANDLE(node_id, can_channel)
		#define GET_CAN_DEV(node_id) DEVICE_DT_GET(DT_PHANDLE(node_id, can_device))
		@@ -59,8 +58,8 @@
		#define DMOTOR_DATA_INST(inst) \
		static struct dji_motor_data dji_motor_data_##inst = { \
		.common = MOTOR_DT_DRIVER_DATA_INST_GET(inst), \
		.canbus_id = DT_DRIVER_GET_CANBUS_ID(inst), \
		.ctrl_struct = &ctrl_structs[DT_DRIVER_GET_CANBUS_ID(inst)], \
		.canbus_id = 0, \
		.ctrl_struct = NULL, \
		.online = false, \
		.convert_num = 0, \
		.current_mode_index = -1, \

drivers/motor/dji/motor_dji.c

+19 −56

Original line number	Diff line number	Diff line
		@@ -23,6 +23,7 @@
		#include <zephyr/kernel.h>
		#include <zephyr/logging/log.h>
		#include <zephyr/sys/time_units.h>
		#include "../common/common.h"

		#define DT_DRV_COMPAT dji_motor

		@@ -30,12 +31,9 @@ LOG_MODULE_REGISTER(motor_dji, CONFIG_MOTOR_LOG_LEVEL);

		const struct device *motor_devices[] = {DT_INST_FOREACH_STATUS_OKAY(DJI_DEVICE_POINTER)};

		const struct device *can_devices[] = {
		DT_FOREACH_CHILD_STATUS_OKAY_SEP(CAN_BUS_PATH, CAN_DEVICE_POINTER, (, ))};

		#define CTRL_STRUCT_DATA(node_id) \
		#define CTRL_STRUCT_DATA(i, _) \
		{ \
		.can_dev = DT_GET_CANPHY_BY_BUS(node_id), \
		.can_dev = NULL, \
		.flags = 0, \
		.full = {false}, \
		.mask = {0}, \
		@@ -99,18 +97,8 @@ static int16_t to16t(float value)
		}
		}

		struct motor_controller ctrl_structs[CAN_COUNT] = {
		DT_FOREACH_CHILD_STATUS_OKAY_SEP(CAN_BUS_PATH, CTRL_STRUCT_DATA, (, ))};

		static inline motor_id_t canbus_id(const struct device *dev)
		{
		for (int i = 0; i < CAN_COUNT; i++) {
		if (can_devices[i] == dev) {
		return i;
		}
		}
		return -1;
		}
		struct motor_controller ctrl_structs[CONFIG_CAN_COUNT] = {
		LISTIFY(CONFIG_CAN_COUNT, CTRL_STRUCT_DATA, (, ))};

		static inline motor_id_t motor_id(const struct device *dev)
		{
		@@ -302,6 +290,9 @@ int dji_init(const struct device *dev)
		if (dev) {
		const struct dji_motor_config *cfg = dev->config;
		struct dji_motor_data *data = dev->data;
		data->canbus_id = reg_can_dev(cfg->common.phy);
		data->ctrl_struct = &ctrl_structs[data->canbus_id];
		data->ctrl_struct->can_dev = (struct device *)cfg->common.phy;
		uint8_t frame_id = frameID_to_index(cfg->common.tx_id);
		uint8_t id = motor_id(dev);
		data->ctrl_struct->mask[frame_id] \|= 1 << id;
		@@ -383,7 +374,7 @@ void can_rx_callback(const struct device can_dev, struct can_frame frame, void

		struct dji_motor_data *data = dev->data;
		uint16_t id = motor_id(dev);
		uint8_t bus_id = canbus_id(can_dev);

		k_spinlock_key_t key;
		if (k_spin_trylock(&data->data_input_lock, &key) != 0) {
		return;
		@@ -399,8 +390,8 @@ void can_rx_callback(const struct device can_dev, struct can_frame frame, void
		(const struct dji_motor_config *)dev->config;
		int8_t frame_id = frameID_to_index(motor_cfg->common.tx_id);
		data->ctrl_struct->mask[frame_id] \|= 1 << id;
		LOG_ERR("Motor \"%s\" on canbus %d is responding again, resuming...", dev->name,
		bus_id);
		LOG_ERR("Motor \"%s\" on canbus \"%s\" is responding again, resuming...", dev->name,
		motor_cfg->common.phy->name);
		} else if (data->missed_times > 0) {
		data->missed_times--;
		}
		@@ -432,16 +423,6 @@ void can_rx_callback(const struct device can_dev, struct can_frame frame, void
		return;
		}

		static void can_tx_callback(const struct device can_dev, int error, void user_data)
		{
		struct k_sem *queue_sem = user_data;
		if (!error \|\| error == -EIO) {
		k_sem_give(queue_sem);
		} else {
		LOG_ERR("CAN TX error: %d on canbus %d", error, canbus_id(can_dev));
		}
		}

		static void proceed_delta_degree(const struct device *dev)
		{
		struct dji_motor_data *data = dev->data;
		@@ -588,18 +569,8 @@ void dji_miss_handler(struct k_work *work)
		{
		ARG_UNUSED(work);
		int curr_time = k_cycle_get_32();
		for (int i = 0; i < CAN_COUNT; i++) {
		if (k_sem_count_get(&ctrl_structs[i].tx_queue_sem) < 1) {
		// can_stop(ctrl_structs[i].can_dev);
		// can_start(ctrl_structs[i].can_dev);
		// k_sem_reset(&ctrl_structs[i].tx_queue_sem);
		}
		for (int j = 0; j < 8; j++) {
		if (ctrl_structs[i].motor_devs[j]) {
		dji_timeout_handle(ctrl_structs[i].motor_devs[j], curr_time,
		&ctrl_structs[i]);
		}
		}
		for (int i = 0; i < DJI_MOTOR_COUNT; i++) {
		dji_timeout_handle(motor_devices[i], curr_time, &ctrl_structs[i]);
		}
		}

		@@ -607,18 +578,14 @@ void dji_init_handler(struct k_work *work)
		{
		ARG_UNUSED(work);
		k_timer_stop(&dji_miss_handle_timer);
		struct device *can_dev = NULL;
		for (int i = 0; i < CAN_COUNT; i++) {
		k_sem_init(&ctrl_structs[i].tx_queue_sem, 3, 3); // 初始化信号量

		can_dev = (struct device *)ctrl_structs[i].can_dev;
		can_start(can_dev);
		}
		for (int i = 0; i < MOTOR_COUNT; i++) {
		for (int i = 0; i < DJI_MOTOR_COUNT; i++) {
		if (motor_devices[i]) {
		const struct dji_motor_config *cfg = motor_devices[i]->config;
		struct can_filter filter = {
		.id = cfg->common.rx_id, .mask = 0x3FF, .flags = 0};
		.id = cfg->common.rx_id,
		.mask = 0x7FF,
		.flags = 0,
		};
		int err = can_add_rx_filter(cfg->common.phy, can_rx_callback,
		(void *)motor_devices[i], &filter);
		if (err < 0) {
		@@ -666,11 +633,7 @@ void dji_tx_handler(struct k_work *work)
		txframe.flags = 0;
		memcpy(txframe.data, frame_data, sizeof(frame_data));
		const struct device *can_dev = ctrl_struct->can_dev;
		int err = k_sem_take(&ctrl_struct->tx_queue_sem, K_NO_WAIT);
		if (err == 0) {
		err = can_send(can_dev, &txframe, K_NO_WAIT,
		can_tx_callback, &ctrl_struct->tx_queue_sem);
		}
		int err = can_send_queued(can_dev, &txframe);
		if (err != 0 && err != -EAGAIN && err != -EBUSY) {
		LOG_ERR("Error sending CAN frame (err %d)", err);
		}

drivers/motor/dji/motor_dji.h

+1 −3

Original line number	Diff line number	Diff line
		@@ -34,7 +34,7 @@ typedef uint16_t motor_id_t;
		struct k_work_q dji_work_queue;

		struct motor_controller {
		const struct device *can_dev;
		struct device *can_dev;

		/*
		There are 4 tx addresses
		@@ -51,8 +51,6 @@ struct motor_controller {
		struct device *motor_devs[8];

		struct k_work full_handle;

		struct k_sem tx_queue_sem;
		};

		struct dji_motor_data {

Admin message