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

boards/damiao/dm_mc02/dm_mc02_defconfig

+3 −0

Original line number	Diff line number	Diff line
		@@ -4,6 +4,9 @@
		# Enable MPU
		CONFIG_ARM_MPU=n

		CONFIG_THREAD_ANALYZER=y
		CONFIG_SCHED_THREAD_USAGE=y

		# LOG CONFIG
		# Enable UART console
		CONFIG_SERIAL=y

drivers/chassis/chassis.c

+61 −16

Original line number	Diff line number	Diff line
		@@ -84,6 +84,12 @@ void cchassis_set_gyro(const struct device *dev, float gyro)
		data->angleControl = false;
		}

		void cchassis_set_static(const struct device *dev, bool static_angle)
		{
		chassis_data_t *data = dev->data;
		data->static_angle = static_angle;
		}

		chassis_status_t cchassis_get_status(const struct device dev)
		{
		chassis_data_t *data = dev->data;
		@@ -98,6 +104,13 @@ void cchassis_resolve(chassis_data_t data, const chassis_cfg_t cfg)
		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;
		if (data->static_angle) {
		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]);
		}
		continue;
		}
		if (!data->track_angle) {
		speedX = rollSpeedX + data->set_status.speedX;
		speedY = rollSpeedY + data->set_status.speedY;
		@@ -117,14 +130,7 @@ void cchassis_resolve(chassis_data_t data, const chassis_cfg_t cfg)

		steerwheel_angle = -RAD2DEG(steerwheel_angle) + 90.0f;

		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]);
		if (err < 0) {
		wheel_set_speed(cfg->wheels[idx], 0, data->angle_to_center[idx]);
		}
		}
		}
		}

		@@ -215,14 +221,52 @@ void chassis_thread_entry(void arg1, void arg2, void *arg3)

		float delta_speed_X = data->target_status.speedX - data->set_status.speedX;
		float delta_speed_Y = data->target_status.speedY - data->set_status.speedY;
		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.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 deltaTime = (float)deltaTimeUs * 0.000001f;

		// Calculate ground acceleration, including centripetal acceleration, expressed in
		// chassis frame
		float ax_d = delta_speed_X / deltaTime; // desired linear acceleration x
		float ay_d = delta_speed_Y / deltaTime; // desired linear acceleration y

		float vx = data->set_status.speedX;
		float vy = data->set_status.speedY;
		float omega = data->target_status.gyro;

		// Centripetal acceleration component: a_c = omega x v
		// In 2D, with v=(vx, vy) and omega being a scalar rotating around z-axis,
		// the vector is (-omegavy, omegavx)
		float ax_c = -omega * vy;
		float ay_c = omega * vx;

		// Total ground acceleration (in chassis frame)
		float ax_total = ax_d + ax_c;
		float ay_total = ay_d + ay_c;
		float a_total_mag = sqrtf(ax_total * ax_total + ay_total * ay_total);

		if (a_total_mag > cfg->max_lin_accel) {
		// Limit the total acceleration
		// We can only affect the linear acceleration part (ax_d, ay_d)
		// The desired linear acceleration needs to be adjusted

		// Vector from centripetal accel to max_lin_accel circle center (-ax_c,
		// -ay_c) to desired total accel (ax_d, ay_d) is (ax_d - (-ax_c)), (ay_d -
		// (-ay_c)) = (ax_total, ay_total). We need to scale this vector to be on
		// the circle of radius max_lin_accel
		float scale = cfg->max_lin_accel / a_total_mag;
		ax_total *= scale;
		ay_total *= scale;

		// The new limited desired linear acceleration
		ax_d = ax_total - ax_c;
		ay_d = ay_total - ay_c;

		// Update delta_speed
		delta_speed_X = ax_d * deltaTime;
		delta_speed_Y = ay_d * deltaTime;
		}

		data->set_status.speedX += delta_speed_X;
		data->set_status.speedY += delta_speed_Y;

		// float currentSpeed = sqrtf(data->chassis_status.speedX *
		// data->chassis_status.speedX +
		@@ -251,6 +295,7 @@ struct chassis_driver_api chassis_driver_api = {
		.set_speed = cchassis_set_speed,
		.set_gyro = cchassis_set_gyro,
		.get_status = cchassis_get_status,
		.set_static = cchassis_set_static,
		};

		DT_INST_FOREACH_STATUS_OKAY(CHASSIS_INIT)
		No newline at end of file

drivers/chassis/chassis.h

+1 −0

Original line number	Diff line number	Diff line
		@@ -20,6 +20,7 @@
		.chassis_sensor_data = {0}, \
		.enabled = true, \
		.track_angle = DT_PROP(DT_DRV_INST(inst), track_angle), \
		.static_angle = false, \
		};

		#define WHEELS_FOREACH(inst, fn) {DT_FOREACH_PROP_ELEM_SEP(DT_DRV_INST(inst), wheels, fn, (,)) }

drivers/motor/dji/motor_dji.c

+20 −23

Original line number	Diff line number	Diff line
		@@ -33,20 +33,10 @@ const struct device *motor_devices[] = {DT_INST_FOREACH_STATUS_OKAY(DJI_DEVICE_P

		#define CTRL_STRUCT_DATA(i, _) \
		{ \
		.can_dev = NULL, \
		.flags = 0, \
		.full = {false}, \
		.mask = {0}, \
		.mapping = \
		{ \
		{-1, -1, -1, -1}, \
		{-1, -1, -1, -1}, \
		{-1, -1, -1, -1}, \
		{-1, -1, -1, -1}, \
		{-1, -1, -1, -1}, \
		{-1, -1, -1, -1}, \
		{-1, -1, -1, -1}, \
		{-1, -1, -1, -1}, \
		.can_dev = NULL, .flags = 0, .full = {false}, .mask = {0}, \
		.mapping = { \
		{-1, -1, -1, -1}, {-1, -1, -1, -1}, {-1, -1, -1, -1}, {-1, -1, -1, -1}, \
		{-1, -1, -1, -1}, {-1, -1, -1, -1}, {-1, -1, -1, -1}, {-1, -1, -1, -1}, \
		}, \
		}

		@@ -328,11 +318,13 @@ 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 dji_motor_config *follow_cfg =
		(const struct dji_motor_config *)cfg->follow->config;
		if (cfg->follow && cfg->common.phy == follow_cfg->common.phy) {
		const struct device *follow_dev = cfg->follow;
		data->ctrl_struct->mask[frame_id] \|= 1 << motor_id(follow_dev);
		} else {
		data->ctrl_struct->mask[frame_id] \|= 1 << id;
		}
		if (data->ctrl_struct->rx_ids[id]) {
		LOG_ERR("Conflicting motor id: %d, dev name: %s", id + 1, dev->name);
		@@ -426,7 +418,9 @@ 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) {
		const struct dji_motor_config *follow_cfg =
		(const struct dji_motor_config *)motor_cfg->follow->config;
		if (motor_cfg->follow && motor_cfg->common.phy == follow_cfg->common.phy) {
		data->ctrl_struct->mask[frame_id] \|= 1 << motor_id(motor_cfg->follow);
		} else {
		data->ctrl_struct->mask[frame_id] \|= 1 << id;
		@@ -456,7 +450,8 @@ void can_rx_callback(const struct device can_dev, struct can_frame frame, void
		data->curr_time = curr_time;
		data->calculated = false;

		if (cfg->follow) {
		struct dji_motor_config follow_cfg = (struct dji_motor_config )cfg->follow->config;
		if (cfg->follow && cfg->common.phy == follow_cfg->common.phy) {
		goto exit;
		}

		@@ -543,12 +538,14 @@ 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) {
		const struct dji_motor_config *follow_cfg =
		(const struct dji_motor_config *)cfg->follow->config;
		if (cfg->follow && cfg->common.phy == follow_cfg->common.phy) {
		data->ctrl_struct->mask[frameID_to_index(cfg->common.tx_id)] &=
		~(1 << motor_id(dev));
		~(1 << motor_id(cfg->follow));
		} else {
		data->ctrl_struct->mask[frameID_to_index(cfg->common.tx_id)] &=
		~(1 << motor_id(cfg->follow));
		~(1 << motor_id(dev));
		}
		data->online = false;
		}

drivers/motor/mi/motor_mi.c

+3 −3

Original line number	Diff line number	Diff line
		@@ -341,7 +341,7 @@ void mi_tx_data_handler(struct k_work *work)
		const struct mi_motor_cfg *cfg = motor_devices[i]->config;
		if (data->online) {
		int can_id = get_can_id(motor_devices[i]);
		if (data->missed_times > 20 && data->enabled == true) {
		if (data->missed_times > 300 && data->enabled == true) {
		LOG_ERR("Motor %s is not responding, setting it to offline.",
		motor_devices[i]->name);
		data->missed_times = 0;
		@@ -384,8 +384,8 @@ void mi_init_handler(struct k_work *work)

		reg_can_dev(cfg->common.phy);
		filters[i].flags = CAN_FILTER_IDE;
		filters[i].mask = 0x1F0000FF;
		filters[i].id = 0x1F000000 \| (cfg->common.id & 0xFF);
		filters[i].mask = 0x1F000000;
		filters[i].id = 0x02000000 \| (cfg->common.id & 0xFF);
		int err = can_add_rx_filter(cfg->common.phy, mi_can_rx_handler, 0, &filters[i]);
		if (err < 0) {
		LOG_ERR("Error adding CAN filter (err %d)", err);

Admin message