various changes (38e4a0b0) · Commits · Wenxi XU / Mambo

drivers/chassis/chassis.c

+3 −2

Original line number	Diff line number	Diff line
		@@ -169,7 +169,8 @@ void chassis_thread_entry(void arg1, void arg2, void *arg3)
		}
		}

		while (!k_sem_take(&chassis_sem, K_FOREVER)) {
		while (1) {
		k_sem_take(&chassis_sem, K_MSEC(1));
		data->prevTime = data->currTime;
		data->currTime = k_cycle_get_32();

		@@ -292,7 +293,7 @@ void chassis_thread_entry(void arg1, void arg2, void *arg3)
		}

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

		struct chassis_driver_api chassis_driver_api = {
		.set_angle = cchassis_set_angle,

drivers/motor/dm/motor_dm.c

+7 −5

Original line number	Diff line number	Diff line
		@@ -184,9 +184,10 @@ static void dm_rx_handler(const struct device can_dev, struct can_frame frame,
		{
		const struct device *dev = user_data;
		struct dm_motor_data *data = dev->data;
		const struct dm_motor_config *cfg = dev->config;

		data->err = frame->data[0] >> 4;
		data->enabled = data->err & 0b1;
		data->enabled = data->err != 0;
		data->online = true;
		data->RAWangle = (frame->data[1] << 8) \| (frame->data[2]);
		data->RAWrpm = (frame->data[3] << 4) \| (frame->data[4] >> 4);
		@@ -194,7 +195,7 @@ static void dm_rx_handler(const struct device can_dev, struct can_frame frame,
		data->update = true;

		uint64_t now = k_uptime_get();
		if (now - data->prev_recv_time > 100 && data->enabled && data->enable) {
		if (now - data->prev_recv_time > 4000 / cfg->freq && data->enable) {
		LOG_ERR("motor %s is back online", dev->name);
		}
		data->prev_recv_time = now;
		@@ -261,7 +262,7 @@ void dm_motor_set_mode(const struct device *dev, enum motor_mode mode)
		break;
		}
		if (strcmp(cfg->common.capabilities[i], mode_str) == 0) {
		struct pid_config params;
		struct pid_config params = {0};
		pid_get_params(cfg->common.pid_datas[i], &params);

		data->common.mode = mode;
		@@ -355,6 +356,7 @@ void dm_tx_data_handler(struct k_work *work)
		dm_motor_pack(motor_devices[i], &tx_frame);
		can_send_queued(cfg->common.phy, &tx_frame);
		data->last_tx_time = now;
		data->tx_cnt++;
		}

		if (data->online && now - data->prev_recv_time <= 5000 / cfg->freq &&
		@@ -370,9 +372,9 @@ void dm_tx_data_handler(struct k_work *work)
		data->online = false;
		data->enabled = false;
		}
		if (((!data->online && data->enable) \|\| (data->enable && !data->enabled)) &&
		now - data->prev_recv_time <= 5000 / cfg->freq) {
		if (((data->online && data->enable && !data->enabled)) && data->tx_cnt == 3) {
		dm_control(motor_devices[i], ENABLE_MOTOR);
		data->tx_cnt = 0;
		}
		}
		}

drivers/motor/dm/motor_dm.h

+1 −1

Original line number	Diff line number	Diff line
		@@ -51,8 +51,8 @@ struct dm_motor_data {
		bool online;
		bool enable;
		bool enabled;

		bool update;
		uint8_t tx_cnt;

		uint64_t prev_recv_time;
		int8_t err;

Admin message