修复了若干问题 (8c567e3d) · Commits · Wenxi XU / Mambo

drivers/motor/dji/motor_dji.c

+24 −21

Original line number	Diff line number	Diff line
		@@ -332,9 +332,7 @@ int dji_init(const struct device *dev)
		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);
		data->ctrl_struct->mask[frame_id] \|= 1 << motor_id(follow_dev);
		}
		if (data->ctrl_struct->rx_ids[id]) {
		LOG_ERR("Conflicting motor id: %d, dev name: %s", id + 1, dev->name);
		@@ -351,6 +349,7 @@ int dji_init(const struct device *dev)
		pid_reg_output(cfg->common.pid_datas[i - 1],
		&data->target_torque);
		}
		data->current_mode_index = i;
		break;
		}
		if (strcmp(cfg->common.capabilities[i], "speed") == 0) {
		@@ -377,7 +376,6 @@ int dji_init(const struct device *dev)
		pid_reg_time(cfg->common.pid_datas[i], &(data->curr_time),
		&(data->prev_time));
		}
		data->current_mode_index = 0;
		data->ctrl_struct->motor_devs[id] = (struct device *)dev;
		data->prev_time = 0;
		data->ctrl_struct->flags = 0;
		@@ -428,7 +426,11 @@ void can_rx_callback(const struct device can_dev, struct can_frame frame, void
		const struct dji_motor_config *motor_cfg =
		(const struct dji_motor_config *)dev->config;
		int8_t frame_id = frameID_to_index(motor_cfg->common.tx_id);
		if (motor_cfg->follow) {
		data->ctrl_struct->mask[frame_id] \|= 1 << motor_id(motor_cfg->follow);
		} else {
		data->ctrl_struct->mask[frame_id] \|= 1 << id;
		}
		LOG_ERR("Motor \"%s\" on canbus \"%s\" is responding again.", dev->name,
		motor_cfg->common.phy->name);
		} else if (data->missed_times > 0) {
		@@ -458,18 +460,17 @@ void can_rx_callback(const struct device can_dev, struct can_frame frame, void
		goto exit;
		}

		bool full = false;
		uint8_t clear_flag = 0u;
		for (int i = 0; i < CAN_TX_ID_CNT; i++) {
		uint8_t combined = data->ctrl_struct->mask[i] & data->ctrl_struct->flags;
		if ((combined & 0xF) == (data->ctrl_struct->mask[i] & 0xF) &&
		data->ctrl_struct->mask[i]) {
		data->ctrl_struct->flags &= ~(data->ctrl_struct->mask[i]);
		if (combined == data->ctrl_struct->mask[i] && data->ctrl_struct->mask[i]) {
		clear_flag \|= data->ctrl_struct->mask[i];
		data->ctrl_struct->full[i] = true;
		full = true;
		}
		}

		if (full && !k_work_is_pending(&data->ctrl_struct->full_handle)) {
		data->ctrl_struct->flags &= ~clear_flag;
		if (clear_flag && !k_work_is_pending(&data->ctrl_struct->full_handle)) {
		k_work_submit_to_queue(&dji_work_queue, &data->ctrl_struct->full_handle);
		}

		@@ -542,8 +543,13 @@ static void dji_timeout_handle(const struct device *dev, uint32_t curr_time)
		if (data->missed_times > 3) {
		LOG_ERR("Motor \"%s\" on canbus \"%s\" is not responding", dev->name,
		cfg->common.phy->name);
		if (!cfg->follow) {
		data->ctrl_struct->mask[frameID_to_index(cfg->common.tx_id)] &=
		~(1 << motor_id(dev));
		} else {
		data->ctrl_struct->mask[frameID_to_index(cfg->common.tx_id)] &=
		~(1 << motor_id(cfg->follow));
		}
		data->online = false;
		}
		}
		@@ -569,8 +575,8 @@ static void motor_calc(const struct device *dev)
		convert[data->convert_num][CURRENT2TORQUE] *
		config->gear_ratio;
		} else {
		data->common.torque =
		data->RAWcurrent * config->dm_torque_ratio * config->gear_ratio * 0.001f;
		data->common.torque = ((float)data->RAWcurrent / 16384.0f) * config->dm_i_max *
		config->dm_torque_ratio * config->gear_ratio;
		}

		if (config->follow) {
		@@ -601,7 +607,7 @@ torque2current:
		data->target_current = data->target_torque / config->gear_ratio *
		convert[data->convert_num][TORQUE2CURRENT];
		} else {
		data->target_current = data->target_torque * 10000 /
		data->target_current = data->target_torque * 16384.0f /
		(config->dm_torque_ratio * config->dm_i_max *
		config->gear_ratio);
		}
		@@ -627,8 +633,6 @@ torque2current:
		k_spin_unlock(&data->data_input_lock, key);
		}

		struct can_frame txframe;

		void dji_miss_isr_handler(struct k_timer *dummy)
		{
		ARG_UNUSED(dummy);
		@@ -688,8 +692,8 @@ void dji_tx_handler(struct k_work *work)
		ctrl_struct->full[i] = false;

		int8_t id_temp = -1;
		uint8_t frame_data[8] = {0};
		bool packed = false;
		struct can_frame txframe = {0};

		for (int j = 0; j < 4; j++) {
		id_temp = ctrl_struct->mapping[i][j];
		@@ -703,7 +707,7 @@ void dji_tx_handler(struct k_work *work)
		motor_calc(ctrl_struct->motor_devs[id_temp]);
		data->calculated = true;
		}
		can_pack_add(frame_data, ctrl_struct->motor_devs[id_temp],
		can_pack_add(txframe.data, ctrl_struct->motor_devs[id_temp],
		j);
		packed = true;
		}
		@@ -712,7 +716,6 @@ 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(txframe.data));
		const struct device *can_dev = ctrl_struct->can_dev;
		can_send_queued(can_dev, &txframe);
		}

drivers/motor/dm/motor_dm.c

+29 −8

Original line number	Diff line number	Diff line
		@@ -149,7 +149,6 @@ static void dm_motor_pack(const struct device dev, struct can_frame frame)
		pbuf = (uint8_t *)&data->target_angle;
		vbuf = (uint8_t *)&data->target_radps;

		frame->data[0] = *pbuf;
		memcpy(frame->data, pbuf, 4);
		memcpy(frame->data + 4, vbuf, 4);
		break;
		@@ -203,7 +202,25 @@ static void dm_rx_handler(const struct device can_dev, struct can_frame frame,
		k_work_submit_to_queue(&dm_work_queue, &dm_rx_data_handle);
		}

		int dm_motor_set_mode(const struct device *dev, enum motor_mode mode)
		static void dm_edit_reg_value(const struct device *dev, uint16_t can_id, uint8_t reg_addr,
		uint32_t reg_value)
		{
		struct can_frame frame;
		frame.id = 0x7FF;
		frame.dlc = 8;
		frame.flags = 0;
		frame.data[CANID_L] = can_id & 0xFF;
		frame.data[CANID_H] = can_id >> 8;
		frame.data[2] = 0x55;
		frame.data[RID] = reg_addr;
		frame.data[4] = reg_value & 0xFF;
		frame.data[5] = reg_value >> 8;
		frame.data[6] = reg_value >> 16;
		frame.data[7] = reg_value >> 24;
		can_send_queued(dev, &frame);
		}

		void dm_motor_set_mode(const struct device *dev, enum motor_mode mode)
		{
		struct dm_motor_data *data = dev->data;
		const struct dm_motor_config *cfg = dev->config;
		@@ -215,19 +232,26 @@ int dm_motor_set_mode(const struct device *dev, enum motor_mode mode)
		case MIT:
		strcpy(mode_str, "mit");
		data->tx_offset = 0x0;
		dm_edit_reg_value(cfg->common.phy, cfg->common.tx_id, 0x0A, 0x01);
		break;
		case PV:
		strcpy(mode_str, "pv");
		data->tx_offset = 0x100;
		dm_edit_reg_value(cfg->common.phy, cfg->common.tx_id, 0x0A, 0x02);
		break;
		case VO:
		strcpy(mode_str, "vo");
		data->tx_offset = 0x200;
		dm_edit_reg_value(cfg->common.phy, cfg->common.tx_id, 0x0A, 0x03);
		break;
		case HYBRID:
		strcpy(mode_str, "hybrid");
		data->tx_offset = 0x300;
		dm_edit_reg_value(cfg->common.phy, cfg->common.tx_id, 0x0A, 0x04);
		break;
		default:
		data->online = false;
		dm_control(dev, DISABLE_MOTOR);
		return -ENOSYS;
		}

		if (mode != VO) {
		@@ -251,11 +275,8 @@ int dm_motor_set_mode(const struct device *dev, enum motor_mode mode)
		LOG_ERR("Mode %s not found", mode_str);
		dm_control(dev, DISABLE_MOTOR);
		data->enable = false;
		return -ENOSYS;
		}
		}

		return 0;
		}

		int dm_set(const struct device dev, motor_status_t status)
		@@ -281,7 +302,7 @@ int dm_set(const struct device dev, motor_status_t status)
		return -ENOSYS;
		}

		return dm_motor_set_mode(dev, status->mode);
		return 0;
		}

		void dm_rx_data_handler(struct k_work *work)
		@@ -342,7 +363,7 @@ void dm_tx_data_handler(struct k_work *work)
		dm_control(motor_devices[i], CLEAR_ERROR);
		}
		}
		if (now - data->prev_recv_time > 150000 / cfg->freq && data->online &&
		if (now - data->prev_recv_time > 250000 / cfg->freq && data->online &&
		data->enable) {
		LOG_ERR("motor %s is not responding, setting it to offline",
		motor_devices[i]->name);

drivers/motor/dm/motor_dm.h

+6 −0

Original line number	Diff line number	Diff line
		@@ -27,6 +27,10 @@
		#define RAD2ROUND 1.0f / (2 * PI)
		#define RAD2DEG 180.0f / PI

		#define CANID_L 0u
		#define CANID_H 1u
		#define RID 3u

		static const uint8_t enable_frame[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC};
		static const uint8_t disable_frame[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD};
		static const uint8_t set_zero_frame[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE};
		@@ -87,6 +91,7 @@ struct k_work_q dm_work_queue;
		int dm_set(const struct device dev, motor_status_t status);
		void dm_control(const struct device *dev, enum motor_cmd cmd);
		int dm_get(const struct device dev, motor_status_t status);
		void dm_motor_set_mode(const struct device *dev, enum motor_mode mode);

		void dm_rx_data_handler(struct k_work *work);

		@@ -99,6 +104,7 @@ void dm_init_handler(struct k_work *work);
		static const struct motor_driver_api motor_api_funcs = {
		.motor_get = dm_get,
		.motor_set = dm_set,
		.motor_set_mode = dm_motor_set_mode,
		.motor_control = dm_control,
		};

include/zephyr/drivers/motor.h

+49 −21

Original line number	Diff line number	Diff line
		@@ -47,46 +47,44 @@ extern "C" {
		motor_set(dev, &(motor_status_t){.torque = _torque, .mode = ML_TORQUE})
		#define motor_set_speed(dev, _speed) \
		motor_set(dev, &(motor_status_t){.rpm = _speed, .mode = ML_SPEED})
		#define motor_set_mode(dev, _mode) motor_set(dev, &(motor_status_t){.mode = _mode})
		#define motor_set_mit(dev, _speed, _angle, _torque) \
		motor_set(dev, \
		&(motor_status_t){ \
		.speed = _speed, .angle = _angle, .torque = _torque, .mode = ML_MIT})
		motor_set(dev, &(motor_status_t){ \
		.rpm = _speed, .angle = _angle, .torque = _torque, .mode = MIT})
		#define motor_set_speed_limit(dev, _min, _max) \
		motor_set(dev, &(motor_status_t){.speed_limit = {_min, _max}})
		#define motor_set_torque_limit(dev, _min, _max) \
		motor_set(dev, &(motor_status_t){.torque_limit = {_min, _max}})

		#define motor_get_angle(dev) \
		{ \
		({ \
		motor_status_t status; \
		motor_get(dev, &status); \
		return status.angle; \
		}
		status.angle; \
		})
		#define motor_get_rpm(dev) \
		{ \
		({ \
		motor_status_t status; \
		motor_get(dev, &status); \
		return status.rpm; \
		}
		status.rpm; \
		})
		#define motor_get_torque(dev) \
		{ \
		({ \
		motor_status_t status; \
		motor_get(dev, &status); \
		return status.torque; \
		}
		status.torque; \
		})
		#define motor_get_speed(dev) \
		{ \
		({ \
		motor_status_t status; \
		motor_get(dev, &status); \
		return status.speed; \
		}
		status.rpm; \
		})
		#define motor_get_mode(dev) \
		{ \
		({ \
		motor_status_t status; \
		motor_get(dev, &status); \
		return status.mode; \
		}
		status.mode; \
		})

		/**
		* @brief 电机工作模式枚举
		@@ -103,6 +101,7 @@ enum motor_mode {
		ML_TORQUE = 3,
		ML_ANGLE = 4,
		ML_SPEED = 5,
		HYBRID = 6,
		};

		/**
		@@ -137,7 +136,7 @@ struct motor_driver_data {
		float rpm;
		float torque;
		float temperature; /* Cannot be set in target */
		int round_cnt;
		float sum_angle;

		float speed_limit[2];
		float torque_limit[2];
		@@ -166,6 +165,16 @@ typedef int (motor_api_stat_t)(const struct device dev, motor_status_t *status
		*/
		typedef int (motor_api_set_t)(const struct device dev, motor_status_t *status);

		/**
		* @typedef motor_api_set_mode_t
		* @brief 设置电机模式
		*
		* @param dev 指向电机设备的指针
		* @param mode 电机模式
		* @return void
		*/
		typedef void (motor_api_set_mode_t)(const struct device dev, enum motor_mode mode);

		/**
		* @typedef motor_api_ctrl_t
		* @brief 电机控制命令
		@@ -183,6 +192,7 @@ __subsystem struct motor_driver_api {
		motor_api_ctrl_t motor_control;
		motor_api_set_t motor_set;
		motor_api_stat_t motor_get;
		motor_api_set_mode_t motor_set_mode;
		};

		/**
		@@ -238,6 +248,24 @@ static inline void z_impl_motor_control(const struct device *dev, enum motor_cmd
		api->motor_control(dev, cmd);
		}

		/**
		* @brief 设置电机模式
		*
		* @param dev 电机设备指针
		* @param mode 电机模式
		* @return void
		*/
		__syscall void motor_set_mode(const struct device *dev, enum motor_mode mode);

		static inline void z_impl_motor_set_mode(const struct device *dev, enum motor_mode mode)
		{
		const struct motor_driver_api api = (const struct motor_driver_api )dev->api;
		if (api->motor_set_mode == NULL) {
		return;
		}
		api->motor_set_mode(dev, mode);
		}

		#define DT_GET_CANPHY(node_id) DEVICE_DT_GET(DT_PHANDLE(node_id, can_channel))

		#define NEW_PID_INSTANCE_STRUCT(node_id, prop, idx) \
		@@ -266,7 +294,7 @@ static inline void z_impl_motor_control(const struct device *dev, enum motor_cmd
		.rpm = 0, \
		.torque = 0, \
		.temperature = 0, \
		.round_cnt = 0, \
		.sum_angle = 0, \
		.speed_limit = {-99999, 99999}, \
		.torque_limit = {-99999, 99999}, \
		.mode = MIT, \

Admin message