对不同减速比做了不同适配；dji驱动中添加对dm电机一拖四模式的兼容 (3fd6ff3d) · Commits · Wenxi XU / Mambo

drivers/motor/dji/dji_macros.h

+27 −14

Original line number	Diff line number	Diff line
		@@ -7,7 +7,7 @@
		#include <zephyr/devicetree.h>
		#include <zephyr/drivers/motor.h>

		#define CAN_SEND_STACK_SIZE 1536
		#define CAN_SEND_STACK_SIZE 2048
		#define CAN_SEND_PRIORITY -1

		#define HIGH_BYTE(x) ((x) >> 8)
		@@ -55,8 +55,11 @@

		#define DT_DRIVER_GET_CANBUS_ID(inst) DT_NODE_CHILD_IDX(DT_DRIVER_INST_GET_CANBUS_IDT(inst))

		#define DMOTOR_DATA_INST(inst) \
		static struct dji_motor_data dji_motor_data_##inst = { \
		#define DT_MOTOR_NAME(node) DT_NODE_FULL_NAME_UNQUOTED(node)
		#define DT_MOTOR_NAME_INST(inst) DT_MOTOR_NAME(DT_DRV_INST(inst))

		#define DMOTOR_DATA(inst, node, name) \
		static struct dji_motor_data DT_CAT(dji_motor_data_, name) = { \
		.common = MOTOR_DT_DRIVER_DATA_INST_GET(inst), \
		.canbus_id = 0, \
		.ctrl_struct = NULL, \
		@@ -77,26 +80,36 @@
		.pid_ref_input = 0, \
		};

		#define DMOTOR_CONFIG_INST(inst) \
		static const struct dji_motor_config dji_motor_cfg_##inst = { \
		#define CONFIG_GET_FOLLOW(node) DT_PHANDLE(node, follow)

		#define DMOTOR_CONFIG(inst, node, name) \
		static const struct dji_motor_config DT_CAT(dji_motor_cfg_, name) = { \
		.common = MOTOR_DT_DRIVER_CONFIG_INST_GET(inst), \
		.gear_ratio = (float)(DT_PROP(DT_DRV_INST(inst), is_gm6020) ? 1.0f : 4.0f) * \
		(float)DT_STRING_UNQUOTED(DT_DRV_INST(inst), gear_ratio), \
		.is_gm6020 = DT_PROP(DT_DRV_INST(inst), is_gm6020), \
		.is_m3508 = DT_PROP(DT_DRV_INST(inst), is_m3508), \
		.is_m2006 = DT_PROP(DT_DRV_INST(inst), is_m2006), \
		.gear_ratio = (float)DT_STRING_UNQUOTED(node, gear_ratio), \
		.is_gm6020 = DT_PROP(node, is_gm6020), \
		.is_m3508 = DT_PROP(node, is_m3508), \
		.is_m2006 = DT_PROP(node, is_m2006), \
		.is_dm_motor = DT_PROP(node, is_dm_motor), \
		.dm_i_max = DT_STRING_UNQUOTED_OR(node, dm_i_max, 0.0f), \
		.dm_torque_ratio = DT_STRING_UNQUOTED_OR(node, dm_torque_ratio, 0.0f), \
		.follow = DEVICE_DT_GET_OR_NULL(CONFIG_GET_FOLLOW(node)), \
		};

		#define DMOTOR_DATA_INST(inst) DMOTOR_DATA(inst, DT_DRV_INST(inst), DT_MOTOR_NAME_INST(inst))
		#define DMOTOR_CONFIG_INST(inst) DMOTOR_CONFIG(inst, DT_DRV_INST(inst), DT_MOTOR_NAME_INST(inst))

		#define MOTOR_DEVICE_DT_DEFINE(node_id, init_fn, pm, data, config, level, prio, api, ...) \
		DEVICE_DT_DEFINE(node_id, init_fn, pm, data, config, level, prio, api, __VA_ARGS__)

		#define MOTOR_DEVICE_DT_INST_DEFINE(inst, ...) \
		MOTOR_DEVICE_DT_DEFINE(DT_DRV_INST(inst), __VA_ARGS__)

		#define DMOTOR_DEFINE_INST(inst) \
		MOTOR_DEVICE_DT_INST_DEFINE(inst, dji_init, NULL, &dji_motor_data_##inst, \
		&dji_motor_cfg_##inst, POST_KERNEL, \
		CONFIG_MOTOR_INIT_PRIORITY, &motor_api_funcs);
		#define DMOTOR_DEFINE(inst, name) \
		MOTOR_DEVICE_DT_INST_DEFINE(inst, dji_init, NULL, &DT_CAT(dji_motor_data_, name), \
		&DT_CAT(dji_motor_cfg_, name), POST_KERNEL, \
		CONFIG_MOTOR_INIT_PRIORITY, &motor_api_funcs)

		#define DMOTOR_DEFINE_INST(inst) DMOTOR_DEFINE(inst, DT_MOTOR_NAME_INST(inst))

		#define DMOTOR_INST(inst) \
		MOTOR_DT_DRIVER_PID_DEFINE(DT_DRV_INST(inst)) \

drivers/motor/dji/dji_ratios.h

+26 −17

Original line number	Diff line number	Diff line
		@@ -5,10 +5,10 @@
		*/
		#define M3508_CURRENT2TORQUE 0.00002200f
		#define M3508_TORQUE2CURRENT 45000.0422411f
		#define M3508_SPEED2RPM 1.0f / 4.0f
		#define M3508_RPM2SPEED 4.0f
		#define M3508_ANGLE2DEGREE 0.043945312f / 4.0f
		#define M3508_DEGREE2ANGLE 22.75277778f / 4.0f
		#define M3508_SPEED2RPM 1.0f
		#define M3508_RPM2SPEED 1.0f
		#define M3508_ANGLE2DEGREE 0.043945312f
		#define M3508_DEGREE2ANGLE 22.75277778f

		#define GM6020_CURRENT2TORQUE 0.00010986f
		#define GM6020_TORQUE2CURRENT 9102.22222f
		@@ -19,14 +19,19 @@

		#define M2006_CURRENT2TORQUE 0.0000089606f
		#define M2006_TORQUE2CURRENT 111600.0f
		#define M2006_SPEED2RPM 1.0f / 4.0f
		#define M2006_RPM2SPEED 4.0f
		#define M2006_ANGLE2DEGREE 0.043950678f / 4.0f
		#define M2006_DEGREE2ANGLE 22.75277778f / 4.0f
		#define M2006_SPEED2RPM 1.0f
		#define M2006_RPM2SPEED 1.0f
		#define M2006_ANGLE2DEGREE 0.043950678f
		#define M2006_DEGREE2ANGLE 22.75277778f

		#define DM_MOTOR_SPEED2RPM 0.001f
		#define DM_MOTOR_RPM2SPEED 1.0f
		#define DM_MOTOR_ANGLE2DEGREE 0.001f

		#define M3508_CONVERT_NUM 0
		#define GM6020_CONVERT_NUM 1
		#define M2006_CONVERT_NUM 2
		#define DM_MOTOR_CONVERT_NUM 3

		#define CURRENT2TORQUE 0
		#define TORQUE2CURRENT 1
		@@ -35,11 +40,15 @@
		#define ANGLE2DEGREE 4
		#define DEGREE2ANGLE 5

		const float convert[3][6] = {{M3508_CURRENT2TORQUE, M3508_TORQUE2CURRENT, M3508_SPEED2RPM,
		M3508_RPM2SPEED, M3508_ANGLE2DEGREE, M3508_DEGREE2ANGLE},
		const float convert[4][6] = {
		{M3508_CURRENT2TORQUE, M3508_TORQUE2CURRENT, M3508_SPEED2RPM, M3508_RPM2SPEED,
		M3508_ANGLE2DEGREE, M3508_DEGREE2ANGLE},

		{GM6020_CURRENT2TORQUE, GM6020_TORQUE2CURRENT, GM6020_SPEED2RPM, GM6020_RPM2SPEED,
		GM6020_ANGLE2DEGREE, GM6020_DEGREE2ANGLE},

		{GM6020_CURRENT2TORQUE, GM6020_TORQUE2CURRENT, GM6020_SPEED2RPM,
		GM6020_RPM2SPEED, GM6020_ANGLE2DEGREE, GM6020_DEGREE2ANGLE},
		{M2006_CURRENT2TORQUE, M2006_TORQUE2CURRENT, M2006_SPEED2RPM, M2006_RPM2SPEED,
		M2006_ANGLE2DEGREE, M2006_DEGREE2ANGLE},

		{M2006_CURRENT2TORQUE, M2006_TORQUE2CURRENT, M2006_SPEED2RPM,
		M2006_RPM2SPEED, M2006_ANGLE2DEGREE, M2006_DEGREE2ANGLE}};
		{0.0f, 0.0f, DM_MOTOR_SPEED2RPM, DM_MOTOR_RPM2SPEED, DM_MOTOR_ANGLE2DEGREE, 0.0},
		};

drivers/motor/dji/motor_dji.c

+165 −87

Original line number	Diff line number	Diff line
		@@ -44,6 +44,9 @@ const struct device *motor_devices[] = {DT_INST_FOREACH_STATUS_OKAY(DJI_DEVICE_P
		{-1, -1, -1, -1}, \
		{-1, -1, -1, -1}, \
		{-1, -1, -1, -1}, \
		{-1, -1, -1, -1}, \
		{-1, -1, -1, -1}, \
		{-1, -1, -1, -1}, \
		}, \
		}

		@@ -59,6 +62,12 @@ static int frameID_to_index(int tx_id)
		return 3;
		} else if (tx_id == 0x2FF) {
		return 4;
		} else if (tx_id == 0x300) {
		return 5;
		} else if (tx_id == 0x3FE) {
		return 6;
		} else if (tx_id == 0x4FE) {
		return 7;
		}
		return -1; // Return a default value if no match is found
		}
		@@ -75,6 +84,12 @@ static int index_to_frameID(int frames_id)
		return 0x2FE;
		} else if (frames_id == 4) {
		return 0x2FF;
		} else if (frames_id == 5) {
		return 0x300;
		} else if (frames_id == 6) {
		return 0x3FE;
		} else if (frames_id == 7) {
		return 0x4FE;
		}
		return -1;
		}
		@@ -181,13 +196,14 @@ int dji_set_mode(const struct device *dev, enum motor_mode mode)
		data->current_mode_index = -1;

		for (int i = 0; i < SIZE_OF_ARRAY(cfg->common.pid_datas); i++) {
		if (cfg->common.pid_datas[i]->pid_dev == NULL) {
		if (cfg->common.pid_datas[i]->pid_dev == NULL && mode == ML_TORQUE) {
		data->current_mode_index = i;
		break;
		}
		if (strcmp(cfg->common.capabilities[i], mode_str) == 0) {
		pid_calc(cfg->common.pid_datas[i]);
		data->current_mode_index = i;
		break;
		}
		}

		@@ -196,15 +212,19 @@ int dji_set_mode(const struct device *dev, enum motor_mode mode)
		return -ENOSYS;
		}

		data->common.mode = mode;

		return 0;
		}

		int dji_set(const struct device dev, motor_status_t status)
		{
		struct dji_motor_data *data = dev->data;

		if (status->mode == ML_TORQUE) {
		dji_set_torque(dev, status->torque);
		} else if (status->mode == ML_ANGLE) {
		dji_set_angle(dev, status->angle + 360.0f * (float)status->round_cnt);
		dji_set_angle(dev, status->angle);
		} else if (status->mode == ML_SPEED) {
		dji_set_speed(dev, status->rpm);
		} else {
		@@ -213,6 +233,7 @@ int dji_set(const struct device dev, motor_status_t status)
		}

		dji_set_mode(dev, status->mode);
		data->common.mode = status->mode;

		if (status->speed_limit[0] > 0 \|\| status->speed_limit[1] > 0) {
		dji_speed_limit(dev, status->speed_limit[1], status->speed_limit[0]);
		@@ -241,13 +262,33 @@ void dji_control(const struct device *dev, enum motor_cmd cmd)
		break;
		case SET_ZERO:
		data->angle_add = 0;
		data->common.angle = 0;
		data->angle_offset = data->common.angle;
		if (cfg->is_dm_motor) {
		frame.id = 0x7FF;
		frame.flags = 0;
		frame.dlc = 4;
		frame.data[0] = (cfg->common.rx_id - 0x200) & 0xFF;
		frame.data[1] = (cfg->common.rx_id - 0x200) >> 8;
		frame.data[2] = 0x55;
		frame.data[3] = 0x3C;
		can_send_queued(cfg->common.phy, &frame);
		}
		break;
		case CLEAR_PID:
		break;
		case CLEAR_ERROR:
		data->missed_times = 0;
		data->online = true;
		if (cfg->is_dm_motor) {
		frame.id = 0x7FF;
		frame.flags = 0;
		frame.dlc = 4;
		frame.data[0] = (cfg->common.rx_id - 0x200) & 0xFF;
		frame.data[1] = (cfg->common.rx_id - 0x200) >> 8;
		frame.data[2] = 0x55;
		frame.data[3] = 0x50;
		can_send_queued(cfg->common.phy, &frame);
		}
		break;
		}
		}
		@@ -267,15 +308,7 @@ int dji_get(const struct device dev, motor_status_t status)
		status->mode = MIT;
		}

		status->angle = fmodf(data->common.angle, 360.0f);
		status->rpm = data->common.rpm;
		status->torque = data->common.torque;
		status->round_cnt = data->common.round_cnt;

		status->speed_limit[0] = data->common.speed_limit[0];
		status->speed_limit[1] = data->common.speed_limit[1];
		status->torque_limit[0] = data->common.torque_limit[0];
		status->torque_limit[1] = data->common.torque_limit[1];
		memcpy(status, &data->common, sizeof(motor_status_t));

		return 0;
		}
		@@ -295,9 +328,16 @@ int dji_init(const struct device *dev)
		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);
		if (!cfg->follow) {
		data->ctrl_struct->mask[frame_id] \|= 1 << id;
		} else {
		const struct device *follow_dev = cfg->follow;
		const struct dji_motor_config *follow_cfg = follow_dev->config;
		uint8_t follow_frame_id = frameID_to_index(follow_cfg->common.tx_id);
		data->ctrl_struct->mask[follow_frame_id] \|= 1 << motor_id(follow_dev);
		}
		if (data->ctrl_struct->rx_ids[id]) {
		LOG_ERR("Conflicting motor id: %d, dev name: %s", id, dev->name);
		LOG_ERR("Conflicting motor id: %d, dev name: %s", id + 1, dev->name);
		}
		data->ctrl_struct->rx_ids[id] = cfg->common.rx_id;

		@@ -348,6 +388,8 @@ int dji_init(const struct device *dev)
		data->convert_num = M3508_CONVERT_NUM;
		} else if (cfg->is_m2006) {
		data->convert_num = M2006_CONVERT_NUM;
		} else if (cfg->is_dm_motor) {
		data->convert_num = DM_MOTOR_CONVERT_NUM;
		} else {
		LOG_ERR("Unsupported motor type");
		}
		@@ -373,16 +415,13 @@ void can_rx_callback(const struct device can_dev, struct can_frame frame, void
		struct can_frame rx_frame = *frame;

		struct dji_motor_data *data = dev->data;
		const struct dji_motor_config *cfg = dev->config;
		uint16_t id = motor_id(dev);

		k_spinlock_key_t key;
		if (k_spin_trylock(&data->data_input_lock, &key) != 0) {
		return;
		}

		if (!data) {
		return;
		}

		if (data->missed_times > 3) {
		data->missed_times = 0;
		data->online = true;
		@@ -390,26 +429,41 @@ 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 \"%s\" is responding again, resuming...", dev->name,
		LOG_ERR("Motor \"%s\" on canbus \"%s\" is responding again.", dev->name,
		motor_cfg->common.phy->name);
		} else if (data->missed_times > 0) {
		data->missed_times--;
		}

		// Store in RAW data. Process when API is called.
		// Using FPU in ISR is not recommended, since it requires actions on registers
		data->RAWprev_angle = data->RAWangle;
		data->RAWangle = COMBINE_HL8(rx_frame.data[0], rx_frame.data[1]);
		int delta = data->RAWangle - data->RAWprev_angle;
		if (data->RAWangle < 2048 && data->RAWprev_angle > 6144) {
		delta += 8192;
		} else if (data->RAWangle > 6144 && data->RAWprev_angle < 2048) {
		delta -= 8192;
		}
		data->angle_add += delta;
		data->RAWrpm = COMBINE_HL8(rx_frame.data[2], rx_frame.data[3]);
		data->RAWcurrent = COMBINE_HL8(rx_frame.data[4], rx_frame.data[5]);
		data->RAWtemp = rx_frame.data[6];
		data->ctrl_struct->flags \|= 1 << id;
		data->prev_time = (data->curr_time == 0) ? (curr_time - 1) : data->curr_time;
		data->curr_time = curr_time;
		data->calculated = false;

		if (cfg->follow) {
		goto exit;
		}

		bool full = false;
		for (int i = 0; i < 5; i++) {
		for (int i = 0; i < CAN_TX_ID_CNT; i++) {
		uint8_t combined = data->ctrl_struct->mask[i] & data->ctrl_struct->flags;
		if (combined == data->ctrl_struct->mask[i] && data->ctrl_struct->mask[i]) {
		data->ctrl_struct->flags = 0;
		if ((combined & 0xF) == (data->ctrl_struct->mask[i] & 0xF) &&
		data->ctrl_struct->mask[i]) {
		data->ctrl_struct->flags &= ~(data->ctrl_struct->mask[i]);
		data->ctrl_struct->full[i] = true;
		full = true;
		}
		@@ -418,51 +472,42 @@ void can_rx_callback(const struct device can_dev, struct can_frame frame, void
		if (full && !k_work_is_pending(&data->ctrl_struct->full_handle)) {
		k_work_submit_to_queue(&dji_work_queue, &data->ctrl_struct->full_handle);
		}
		// k_thread_resume(dji_motor_ctrl_thread);
		k_spin_unlock(&data->data_input_lock, key);

		exit:
		return;
		}

		static void proceed_delta_degree(const struct device *dev)
		{
		struct dji_motor_data *data = dev->data;
		const struct dji_motor_config *config_temp = dev->config;
		int delta = data->RAWangle - data->RAWprev_angle;
		if (data->RAWangle < 2048 && data->RAWprev_angle > 6144) {
		delta += 8192;
		data->common.round_cnt++;
		if (data->target_angle < 0 \|\| data->target_angle > 360) {
		data->target_angle -= 360;
		}
		} else if (data->RAWangle > 6144 && data->RAWprev_angle < 2048) {
		delta -= 8192;
		data->common.round_cnt--;
		if (data->target_angle < 0 \|\| data->target_angle > 360) {
		data->target_angle += 360;
		}
		}
		const struct dji_motor_config *config = dev->config;

		if (fabsf(config_temp->gear_ratio - 1) > 0.001f) {
		// I dont know why RAW_angle for M3508 is 4 times of angle
		data->angle_add += (float)(delta)*convert[data->convert_num][ANGLE2DEGREE] /
		(config_temp->gear_ratio);
		if (fabsf(config->gear_ratio - 1) > 0.001f) {
		data->common.sum_angle = (float)(data->angle_add) *
		convert[data->convert_num][ANGLE2DEGREE] /
		config->gear_ratio;

		data->common.angle = fmodf(data->angle_add, 360.0f);
		while (data->common.angle < 0) {
		data->common.angle = fmodf(data->common.sum_angle, 360.0f);
		if (data->common.angle < 0) {
		data->common.angle += 360.0f;
		}
		} else {
		data->common.sum_angle =
		(float)(data->angle_add) * convert[data->convert_num][ANGLE2DEGREE];
		data->common.angle =
		(float)(data->RAWangle) * convert[data->convert_num][ANGLE2DEGREE];
		(float)(data->RAWangle) * convert[data->convert_num][ANGLE2DEGREE] -
		data->angle_offset;
		}

		float delta_angle = data->common.angle - data->target_angle;
		float delta_angle = data->common.sum_angle - data->target_angle;

		if (fabsf(config->gear_ratio - 1) < 0.001f) {
		if (delta_angle > 180) {
		delta_angle -= 360.0f;
		} else if (delta_angle < -180) {
		delta_angle += 360.0f;
		}
		}

		data->pid_angle_input = delta_angle;
		}
		@@ -470,31 +515,36 @@ static void proceed_delta_degree(const struct device *dev)
		static void can_pack_add(uint8_t data, struct device motor_dev, uint8_t num)
		{
		struct dji_motor_data *motor_data = motor_dev->data;
		const struct dji_motor_config *cfg = motor_dev->config;

		int16_t value = to16t(motor_data->target_current);

		if (!cfg->is_dm_motor) {
		data[num * 2] = HIGH_BYTE(value);
		data[num * 2 + 1] = LOW_BYTE(value);
		} else {
		data[num * 2] = LOW_BYTE(value);
		data[num * 2 + 1] = HIGH_BYTE(value);
		}
		}

		static void dji_timeout_handle(const struct device *dev, uint32_t curr_time,
		struct motor_controller *ctrl_struct)
		static void dji_timeout_handle(const struct device *dev, uint32_t curr_time)
		{
		struct dji_motor_data motor_data = (struct dji_motor_data )dev->data;
		const struct dji_motor_config motor_cfg = (const struct dji_motor_config )dev->config;
		struct dji_motor_data data = (struct dji_motor_data )dev->data;
		const struct dji_motor_config cfg = (const struct dji_motor_config )dev->config;

		if (motor_data->online == false) {
		if (data->online == false) {
		return;
		}
		uint32_t prev_time = motor_data->curr_time;
		uint32_t prev_time = data->curr_time;
		if (k_cyc_to_us_near32(curr_time - prev_time) > 2000 \|\| curr_time - prev_time > 100000) {
		motor_data->missed_times++;
		if (motor_data->missed_times > 3) {
		LOG_ERR("Motor \"%s\" on canbus %d is not responding", dev->name,
		(int)(ctrl_struct - ctrl_structs));
		ctrl_struct->mask[frameID_to_index(motor_cfg->common.tx_id)] &=
		data->missed_times++;
		if (data->missed_times > 3) {
		LOG_ERR("Motor \"%s\" on canbus \"%s\" is not responding", dev->name,
		cfg->common.phy->name);
		data->ctrl_struct->mask[frameID_to_index(cfg->common.tx_id)] &=
		~(1 << motor_id(dev));
		motor_data->online = false;
		data->online = false;
		}
		}
		}
		@@ -507,32 +557,60 @@ static void motor_calc(const struct device *dev)
		if (k_spin_trylock(&data->data_input_lock, &key) != 0) {
		return;
		}
		const struct dji_motor_config *config_temp = dev->config;
		const struct dji_motor_config *config = dev->config;
		// Proceed the RAW data
		// Add up to avoid circular overflow
		proceed_delta_degree(dev);

		float rpm = data->RAWrpm * convert[data->convert_num][SPEED2RPM] / config_temp->gear_ratio;
		data->common.rpm = rpm;
		data->common.torque = data->RAWcurrent * convert[data->convert_num][CURRENT2TORQUE] *
		config_temp->gear_ratio;
		data->common.rpm =
		data->RAWrpm * convert[data->convert_num][SPEED2RPM] / config->gear_ratio;
		if (!config->is_dm_motor) {
		data->common.torque = data->RAWcurrent *
		convert[data->convert_num][CURRENT2TORQUE] *
		config->gear_ratio;
		} else {
		data->common.torque =
		data->RAWcurrent * config->dm_torque_ratio * config->gear_ratio * 0.001f;
		}

		if (config->follow) {
		const struct device *follow_dev = config->follow;
		struct dji_motor_data *follow_data = follow_dev->data;
		if (!follow_data->calculated) {
		motor_calc(follow_dev);
		follow_data->calculated = true;
		data->target_torque = follow_data->target_torque;
		} else if (follow_data->online) {
		data->target_torque = follow_data->target_torque;
		} else {
		data->target_torque = 0;
		}
		goto torque2current;
		}

		for (int i = data->current_mode_index; i < SIZE_OF_ARRAY(config_temp->common.capabilities);
		for (int i = data->current_mode_index; i < SIZE_OF_ARRAY(config->common.capabilities);
		i++) {
		if (config_temp->common.pid_datas[i]->pid_dev == NULL) {
		if (config->common.pid_datas[i]->pid_dev == NULL) {
		torque2current:
		if (data->target_torque > data->common.torque_limit[1]) {
		data->target_torque = data->common.torque_limit[1];
		} else if (data->target_torque < data->common.torque_limit[0]) {
		data->target_torque = data->common.torque_limit[0];
		}
		data->target_current = data->target_torque / config_temp->gear_ratio *
		if (!config->is_dm_motor) {
		data->target_current = data->target_torque / config->gear_ratio *
		convert[data->convert_num][TORQUE2CURRENT];
		} else {
		data->target_current = data->target_torque * 10000 /
		(config->dm_torque_ratio * config->dm_i_max *
		config->gear_ratio);
		}
		break;
		}

		pid_calc(config_temp->common.pid_datas[i]);
		pid_calc(config->common.pid_datas[i]);

		if (strcmp(config_temp->common.capabilities[i], "angle") == 0) {
		if (strcmp(config->common.capabilities[i], "angle") == 0) {
		if (data->target_rpm > data->common.speed_limit[1]) {
		data->target_rpm = data->common.speed_limit[1];
		} else if (data->target_rpm < data->common.speed_limit[0]) {
		@@ -540,9 +618,9 @@ static void motor_calc(const struct device *dev)
		}
		}

		if (strcmp(config_temp->common.capabilities[i], "torque") == 0) {
		if (strcmp(config->common.capabilities[i], "torque") == 0) {
		break;
		} else if (strcmp(config_temp->common.capabilities[i], "mit") == 0) {
		} else if (strcmp(config->common.capabilities[i], "mit") == 0) {
		break;
		}
		}
		@@ -570,7 +648,7 @@ void dji_miss_handler(struct k_work *work)
		ARG_UNUSED(work);
		int curr_time = k_cycle_get_32();
		for (int i = 0; i < DJI_MOTOR_COUNT; i++) {
		dji_timeout_handle(motor_devices[i], curr_time, &ctrl_structs[i]);
		dji_timeout_handle(motor_devices[i], curr_time);
		}
		}

		@@ -605,7 +683,7 @@ void dji_tx_handler(struct k_work *work)
		return;
		}

		for (int i = 0; i < 5; i++) { // For each frame
		for (int i = 0; i < CAN_TX_ID_CNT; i++) { // For each frame
		if (ctrl_struct->full[i]) {
		ctrl_struct->full[i] = false;

		@@ -621,7 +699,10 @@ void dji_tx_handler(struct k_work *work)
		const struct device *dev = ctrl_struct->motor_devs[id_temp];
		struct dji_motor_data *data = dev->data;
		if (id_temp < 8 && data->online) {
		if (!data->calculated) {
		motor_calc(ctrl_struct->motor_devs[id_temp]);
		data->calculated = true;
		}
		can_pack_add(frame_data, ctrl_struct->motor_devs[id_temp],
		j);
		packed = true;
		@@ -631,12 +712,9 @@ void dji_tx_handler(struct k_work *work)
		txframe.id = index_to_frameID(i);
		txframe.dlc = 8;
		txframe.flags = 0;
		memcpy(txframe.data, frame_data, sizeof(frame_data));
		memcpy(txframe.data, frame_data, sizeof(txframe.data));
		const struct device *can_dev = ctrl_struct->can_dev;
		int err = can_send_queued(can_dev, &txframe);
		if (err != 0 && err != -EAGAIN && err != -EBUSY) {
		LOG_ERR("Error sending CAN frame (err %d)", err);
		}
		can_send_queued(can_dev, &txframe);
		}
		}
		}

drivers/motor/dji/motor_dji.h

+13 −8

Original line number	Diff line number	Diff line
		@@ -18,6 +18,8 @@
		#include <zephyr/drivers/motor.h>
		#include <zephyr/drivers/pid.h>

		#define CAN_TX_ID_CNT 8

		/* canbus_id_t specifies the ID of the CAN bus the motor is on
		which is defined in motor_devices[] */
		typedef uint8_t canbus_id_t;
		@@ -40,14 +42,13 @@ struct motor_controller {
		There are 4 tx addresses
		0x1FF, 0x200 for M3508/M2006
		0x1FE, 0x2FE for GM6020 Current control.
		!!!! Voltage control for GM6020 is deprecated !!!!
		The 5 nums in full[] are: 0x200, 0x1FF, 0x1FE, 0x2FE, 0x2FF
		The 5 nums in full[] are: 0x200, 0x1FF, 0x1FE, 0x2FE, 0x2FF, 0x300
		*/
		int rx_ids[8];
		bool full[5];
		int8_t mapping[5][4];
		bool full[CAN_TX_ID_CNT];
		int8_t mapping[CAN_TX_ID_CNT][4];
		uint8_t flags;
		uint8_t mask[5];
		uint8_t mask[CAN_TX_ID_CNT];
		struct device *motor_devs[8];

		struct k_work full_handle;
		@@ -69,7 +70,7 @@ struct dji_motor_data {
		int16_t RAWcurrent;
		int16_t RAWrpm;
		int8_t RAWtemp;
		float angle_add;
		int32_t angle_add;

		uint32_t curr_time;
		uint32_t prev_time;
		@@ -82,13 +83,13 @@ struct dji_motor_data {

		struct k_spinlock data_input_lock;

		bool minorArc;

		// Target
		float target_angle;
		float target_rpm;
		float target_torque;
		float target_current;
		bool calculated;
		bool new_data;
		};

		struct dji_motor_config {
		@@ -98,6 +99,10 @@ struct dji_motor_config {
		bool is_gm6020;
		bool is_m3508;
		bool is_m2006;
		bool is_dm_motor;
		float dm_i_max;
		float dm_torque_ratio;
		const struct device *follow;
		};

		// 全局变量声明

drivers/motor/dm/motor_dm.c

+10 −12

Original line number	Diff line number	Diff line
		@@ -173,7 +173,7 @@ int dm_get(const struct device dev, motor_status_t status)
		status->torque = data->common.torque;

		status->mode = data->common.mode;
		status->round_cnt = (int)(data->common.angle / 360.0f);
		status->sum_angle = data->delta_deg_sum;
		status->speed_limit[0] = cfg->v_max;
		status->speed_limit[1] = cfg->v_max;
		status->torque_limit[0] = cfg->t_max;
		@@ -302,13 +302,6 @@ void dm_rx_data_handler(struct k_work *work)
		data->common.torque = uint_to_float(data->RAWtorque, -cfg->t_max, cfg->t_max, 12);

		data->delta_deg_sum += data->common.angle - prev_angle;
		if (data->delta_deg_sum > 360) {
		data->common.round_cnt++;
		data->delta_deg_sum -= 360.0f;
		} else if (data->delta_deg_sum < -360) {
		data->common.round_cnt--;
		data->delta_deg_sum += 360.0f;
		}

		data->update = false;
		}
		@@ -337,15 +330,20 @@ void dm_tx_data_handler(struct k_work *work)
		struct dm_motor_data *data = motor_devices[i]->data;
		const struct dm_motor_config *cfg = motor_devices[i]->config;

		if (now - data->last_tx_time >= 1000 / cfg->freq) {
		dm_motor_pack(motor_devices[i], &tx_frame);
		can_send_queued(cfg->common.phy, &tx_frame);
		data->last_tx_time = now;
		}

		if (data->online && now - data->prev_recv_time <= 5 && data->enable) {
		if (data->online && now - data->prev_recv_time <= 5000 / cfg->freq &&
		data->enable) {
		if (data->err > 1) {
		dm_control(motor_devices[i], CLEAR_ERROR);
		}
		}
		if (now - data->prev_recv_time > 150 && data->online && data->enable) {
		if (now - data->prev_recv_time > 150000 / cfg->freq && data->online &&
		data->enable) {
		LOG_ERR("motor %s is not responding, setting it to offline",
		motor_devices[i]->name);
		data->online = false;

Admin message