Merge branch 'new-atomic-motor-api' (3f25ec44) · Commits · Wenxi XU / Mambo

drivers/chassis/chassis.c

+24 −27

Original line number	Diff line number	Diff line
		@@ -9,7 +9,7 @@
		#include <zephyr/devicetree.h>
		#include <zephyr/drivers/chassis.h>
		#include <zephyr/drivers/pid.h>
		#include "zephyr/drivers/wheel.h"
		#include <zephyr/drivers/wheel.h>
		#include <zephyr/logging/log.h>
		#include "chassis.h"
		#include "zephyr/kernel.h"
		@@ -31,11 +31,9 @@ LOG_MODULE_REGISTER(chassis, CONFIG_MOTOR_LOG_LEVEL);
		#endif

		void chassis_timer_cb(struct k_timer *timer);
		void chassis_thread(void arg1, void arg2, void *arg3);

		k_tid_t chassis_tid;
		struct k_thread chassis_thread_data;
		K_THREAD_STACK_DEFINE(chassis_stack_area, CHASSIS_STACK_SIZE);
		// struct k_thread chassis_thread_data;
		// K_THREAD_STACK_DEFINE(chassis_stack_area, CHASSIS_STACK_SIZE);

		K_TIMER_DEFINE(chassis_timer, chassis_timer_cb, NULL);

		@@ -54,14 +52,14 @@ int cchassis_init(const struct device *dev)
		&data->distance_to_center[idx]);
		idx++;
		}
		data->currTime = k_uptime_get_32();
		data->currTime = k_cycle_get_32();
		data->prevTime = data->currTime;

		k_thread_create(&chassis_thread_data, chassis_stack_area,
		K_THREAD_STACK_SIZEOF(chassis_stack_area), chassis_thread, (void *)dev,
		NULL, NULL, K_HIGHEST_THREAD_PRIO, 0, K_NO_WAIT);
		// k_thread_create(&chassis_thread_data, chassis_stack_area,
		// K_THREAD_STACK_SIZEOF(chassis_stack_area), chassis_thread, (void *)dev,
		// NULL, NULL, -2, 0, K_NO_WAIT);
		k_timer_user_data_set(&chassis_timer, (void *)dev);
		k_timer_start(&chassis_timer, K_MSEC(100), K_MSEC(2));
		k_timer_start(&chassis_timer, K_MSEC(100), K_MSEC(1));
		return 0;
		}

		@@ -96,8 +94,7 @@ void cchassis_resolve(chassis_data_t data, const chassis_cfg_t cfg)
		{
		// data->targetRollSpeed = 0;

		int idx = 0;
		while (cfg->wheels[idx] != NULL) {
		for (int idx = 0; idx < CHASSIS_WHEEL_COUNT; idx++) {
		float rollSpeedX = cfg->pos_Y_offset[idx] * data->set_status.gyro;
		float rollSpeedY = -cfg->pos_X_offset[idx] * data->set_status.gyro;
		float speedX, speedY;
		@@ -123,15 +120,11 @@ void cchassis_resolve(chassis_data_t data, const chassis_cfg_t cfg)
		if (fabsf(steerwheel_speed) > 0.1f \|\| fabsf(data->set_status.gyro) > 0.15f) {
		wheel_set_speed(cfg->wheels[idx], steerwheel_speed, steerwheel_angle);
		} else {
		int err = wheel_set_static(cfg->wheels[idx],
		data->angle_to_center[idx] + 90.0f);
		int err = wheel_set_static(cfg->wheels[idx], data->angle_to_center[idx]);
		if (err < 0) {
		wheel_set_speed(cfg->wheels[idx], 0,
		data->angle_to_center[idx] + 90.0f);
		wheel_set_speed(cfg->wheels[idx], 0, data->angle_to_center[idx]);
		}
		}

		idx++;
		}
		}

		@@ -145,11 +138,13 @@ void chassis_timer_cb(struct k_timer *timer)
		}
		}

		void chassis_thread(void arg1, void arg2, void *arg3)
		void chassis_thread_entry(void arg1, void arg2, void *arg3)
		{
		ARG_UNUSED(arg2);
		ARG_UNUSED(arg3);

		k_thread_name_set(k_current_get(), "chassis");

		const struct device dev = (const struct device )arg1;
		if (dev == NULL) {
		return;
		@@ -174,7 +169,7 @@ void chassis_thread(void arg1, void arg2, void *arg3)
		if ((data->chassis_sensor_data.accel[2] - 9.8f) > 0.4f) {
		// We are in the air
		}
		float deltaTimeUs = k_cyc_to_us_near32(data->currTime - data->prevTime);
		int32_t deltaTimeUs = k_cyc_to_us_floor32(data->currTime - data->prevTime);
		float error = 0;

		if (!isnan(data->chassis_sensor_data.Yaw) && data->angleControl) {
		@@ -223,13 +218,12 @@ void chassis_thread(void arg1, void arg2, void *arg3)
		float delta_speed =
		sqrtf(delta_speed_X * delta_speed_X + delta_speed_Y * delta_speed_Y);
		float delta_speed_angle = atan2f(delta_speed_Y, delta_speed_X);
		delta_speed = delta_speed > (cfg->max_lin_accel * deltaTimeUs * 0.001f)
		? (cfg->max_lin_accel * deltaTimeUs * 0.001f)
		: delta_speed < -(cfg->max_lin_accel * deltaTimeUs * 0.001f)
		? -(cfg->max_lin_accel * deltaTimeUs * 0.001f)
		delta_speed = (delta_speed > (cfg->max_lin_accel * deltaTimeUs) * 0.000001f)
		? (cfg->max_lin_accel * deltaTimeUs) * 0.000001f
		: delta_speed;
		data->set_status.speedX += delta_speed * cosf(delta_speed_angle);
		data->set_status.speedY += delta_speed * sinf(delta_speed_angle);

		// float currentSpeed = sqrtf(data->chassis_status.speedX *
		// data->chassis_status.speedX +
		// data->chassis_status.speedY * data->chassis_status.speedY);
		@@ -240,15 +234,18 @@ void chassis_thread(void arg1, void arg2, void *arg3)
		data->set_status.gyro = pid_calc_in(cfg->angle_pid, error, deltaTimeUs);
		}

		data->set_status.gyro = data->target_status.gyro > cfg->max_gyro ? cfg->max_gyro
		: data->target_status.gyro < -cfg->max_gyro
		data->set_status.gyro = data->set_status.gyro > cfg->max_gyro ? cfg->max_gyro
		: data->set_status.gyro < -cfg->max_gyro
		? -cfg->max_gyro
		: data->target_status.gyro;
		: data->set_status.gyro;

		cchassis_resolve(data, cfg);
		}
		}

		K_THREAD_DEFINE(chassis_thread, CHASSIS_STACK_SIZE, chassis_thread_entry,
		DEVICE_DT_GET(DT_DRV_INST(0)), NULL, NULL, 0, 0, 0);

		struct chassis_driver_api chassis_driver_api = {
		.set_angle = cchassis_set_angle,
		.set_speed = cchassis_set_speed,

drivers/chassis/chassis.h

+1 −2

Original line number	Diff line number	Diff line
		@@ -40,8 +40,7 @@
		.pos_X_offset = WHEELS_FOREACH(inst, GET_WHEEL_X_OFFSET), \
		.pos_Y_offset = WHEELS_FOREACH(inst, GET_WHEEL_Y_OFFSET), \
		.max_gyro = DT_STRING_UNQUOTED_OR(DT_DRV_INST(inst), max_gyro, 10), \
		.max_lin_accel = \
		DT_STRING_UNQUOTED_OR(DT_DRV_INST(inst), max_linear_accel, 10) / 1000.0f, \
		.max_lin_accel = DT_STRING_UNQUOTED_OR(DT_DRV_INST(inst), max_linear_accel, 10), \
		};

		#define CHASSIS_DEFINE_INST(inst) \

drivers/motor/dji/motor_dji.c

+21 −18

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,
		};

Admin message