修复若干可能出现访问溢出的问题 (de18643c) · Commits · Wenxi XU / Mambo

drivers/chassis/chassis.c

+44 −43

Original line number	Diff line number	Diff line
		@@ -41,8 +41,7 @@ int cchassis_init(const struct device *dev)
		{
		chassis_data_t *data = dev->data;
		const chassis_cfg_t *cfg = dev->config;
		int idx = 0;
		while (cfg->wheels[idx] != NULL) {
		for (int idx = 0; idx < CHASSIS_WHEEL_COUNT; idx++) {
		float arc;
		arm_atan2_f32(cfg->pos_Y_offset[idx], cfg->pos_X_offset[idx], &arc);

		@@ -50,7 +49,6 @@ int cchassis_init(const struct device *dev)
		arm_sqrt_f32(cfg->pos_X_offset[idx] * cfg->pos_X_offset[idx] +
		cfg->pos_Y_offset[idx] * cfg->pos_Y_offset[idx],
		&data->distance_to_center[idx]);
		idx++;
		}
		data->currTime = k_cycle_get_32();
		data->prevTime = data->currTime - 100;
		@@ -228,8 +226,9 @@ void chassis_thread_entry(void arg1, void arg2, void *arg3)
		float delta_speed_Y = data->target_status.speedY - data->set_status.speedY;
		float deltaTime = (float)deltaTimeUs * 0.000001f;

		// Calculate ground acceleration, including centripetal acceleration, expressed in
		// chassis frame
		if (deltaTime > 1e-6f) {
		// 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

		@@ -253,10 +252,11 @@ void chassis_thread_entry(void arg1, void arg2, void *arg3)
		// 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
		// 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;
		@@ -269,6 +269,7 @@ void chassis_thread_entry(void arg1, void arg2, void *arg3)
		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;

drivers/chassis/chassis.h

+0 −1

Original line number	Diff line number	Diff line
		@@ -11,7 +11,6 @@
		static chassis_data_t chassis_data_##inst = { \
		.chassis_status = {0.0f}, \
		.set_status = {0.0f}, \
		.set_status = {0.0f}, \
		.currTime = 0ULL, \
		.prevTime = 0ULL, \
		.angleControl = true, \

drivers/motor/dm/motor_dm.c

+21 −16

Original line number	Diff line number	Diff line
		@@ -8,6 +8,7 @@
		#include <string.h>
		#include <zephyr/device.h>
		#include <zephyr/devicetree.h>
		#include <sys/_stdint.h>
		#include "../common/common.h"
		#include "motor_dm.h"
		#include "syscalls/kernel.h"
		@@ -188,17 +189,17 @@ static void dm_rx_handler(const struct device can_dev, struct can_frame frame,

		data->err = frame->data[0] >> 4;
		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);
		data->RAWtorque = (frame->data[4] & 0xF) << 8;
		data->update = true;

		uint64_t now = k_uptime_get();
		if (now - data->prev_recv_time > 4000 / cfg->freq && data->enable) {
		if (data->enable && !data->online) {
		LOG_ERR("motor %s is back online", dev->name);
		}
		data->prev_recv_time = now;
		data->online = true;

		k_work_submit_to_queue(&dm_work_queue, &dm_rx_data_handle);
		}
		@@ -231,22 +232,22 @@ void dm_motor_set_mode(const struct device *dev, enum motor_mode mode)

		switch (mode) {
		case MIT:
		strcpy(mode_str, "mit");
		snprintf(mode_str, sizeof(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");
		snprintf(mode_str, sizeof(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");
		snprintf(mode_str, sizeof(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");
		snprintf(mode_str, sizeof(mode_str), "hybrid");
		data->tx_offset = 0x300;
		dm_edit_reg_value(cfg->common.phy, cfg->common.tx_id, 0x0A, 0x04);
		break;
		@@ -283,6 +284,7 @@ void dm_motor_set_mode(const struct device *dev, enum motor_mode mode)
		int dm_set(const struct device dev, motor_status_t status)
		{
		struct dm_motor_data *data = dev->data;
		const struct dm_motor_config *cfg = dev->config;

		if (status->mode == MIT) {
		data->target_angle = RAD2DEG * status->angle;
		@@ -303,6 +305,11 @@ int dm_set(const struct device dev, motor_status_t status)
		return -ENOSYS;
		}

		struct can_frame tx_frame;
		dm_motor_pack(dev, &tx_frame);
		can_send_queued(cfg->common.phy, &tx_frame);
		data->last_tx_time = k_uptime_get();

		return 0;
		}

		@@ -358,23 +365,21 @@ void dm_tx_data_handler(struct k_work *work)
		data->last_tx_time = now;
		data->tx_cnt++;
		}

		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 > 250000 / cfg->freq && data->online &&
		if (now - data->prev_recv_time > 10000 / cfg->freq && data->online &&
		data->enable) {
		LOG_ERR("motor %s is not responding, setting it to offline",
		motor_devices[i]->name);
		data->online = false;
		data->enabled = false;
		}
		if (((data->online && data->enable && !data->enabled)) && data->tx_cnt == 3) {
		if ((data->online && data->enable) && data->tx_cnt == 3) {
		if (!data->enabled) {
		dm_control(motor_devices[i], ENABLE_MOTOR);
		}
		data->tx_cnt = 0;
		if (data->err > 1) {
		dm_control(motor_devices[i], CLEAR_ERROR);
		}
		}
		}
		}

west.yml

+2 −0

Original line number	Diff line number	Diff line
		@@ -24,3 +24,5 @@ manifest:
		- hal_stm32 # required by the nucleo_f302r8 board (STM32 based)
		- mcuboot
		- segger
		- hal_nxp
		- hal_espressif
		No newline at end of file

Admin message