Some little changes (b5edc208) · Commits · Wenxi XU / Mambo

drivers/chassis/Kconfig

+0 −5

Original line number	Diff line number	Diff line
		@@ -15,11 +15,6 @@ module = CHASSIS
		module-str = CHASSIS
		source "subsys/logging/Kconfig.template.log_config"

		config CHASSIS_MAX_STEERWHHEL_COUNT
		int "CHASSIS MAX STEERWHHEL COUNT"
		help
		Add support for dji motors

		config CHASSIS_LOG_LEVEL
		int "Motor log level"
		default 4

drivers/chassis/chassis.c

+35 −26

Original line number	Diff line number	Diff line
		@@ -31,10 +31,19 @@ ZBUS_MSG_SUBSCRIBER_DEFINE(chassis_sensor_msg_suscriber);
		#define M_PI_2 1.57079632679489661923f
		#endif

		ZBUS_CHAN_DEFINE(chassis_sensor_zbus, /* Name */
		struct pos_data, /* Message type */
		NULL, /* Validator */
		NULL, /* User Data */
		ZBUS_OBSERVERS(chassis_sensor_msg_suscriber), /* observers */
		ZBUS_MSG_INIT(.Yaw = 0, .accel = {0}) /* Initial value */
		);

		int cchassis_init(const struct device *dev)
		{
		chassis_data_t *data = dev->data;
		const chassis_cfg_t *cfg = dev->config;
		data->chassis_sensor_zbus = (struct zbus_channel *)&chassis_sensor_zbus;
		int idx = 0;
		while (cfg->wheels[idx] != NULL) {
		float arc;
		@@ -97,8 +106,8 @@ void cchassis_resolve(chassis_data_t data, const chassis_cfg_t cfg)
		// data->targetRollSpeed = 0;

		int idx = 0;
		float currentSpeedX[CONFIG_CHASSIS_MAX_STEERWHHEL_COUNT] = {0};
		float currentSpeedY[CONFIG_CHASSIS_MAX_STEERWHHEL_COUNT] = {0};
		float currentSpeedX[CHASSIS_WHEEL_COUNT] = {0};
		float currentSpeedY[CHASSIS_WHEEL_COUNT] = {0};
		float currentGyro = 0;
		while (cfg->wheels[idx] != NULL) {
		// 由于我们的轮电机使用的是速度环PID控制
		@@ -165,28 +174,26 @@ void cchassis_main_thread(const struct device dev, void ptr2, void *ptr3)
		chassis_data_t *data = dev->data;
		const chassis_cfg_t *cfg = dev->config;

		for (int i = 0; i < CONFIG_CHASSIS_MAX_STEERWHHEL_COUNT; i++) {
		for (int i = 0; i < CHASSIS_WHEEL_COUNT; i++) {
		if (cfg->wheels[i] == NULL) {
		break;
		}
		wheel_set_static(cfg->wheels[i], 90.0f);
		}

		k_msleep(1000);

		struct pos_data pos = {0};

		while (true) {
		if (data->chassis_sensor_zbus != NULL) {
		zbus_sub_wait_msg(&chassis_sensor_msg_suscriber, &chan, &pos, K_MSEC(1));
		zbus_sub_wait_msg(&chassis_sensor_msg_suscriber, &chan, &pos, K_FOREVER);
		if (data->chassis_sensor_zbus != chan) {
		continue;
		}
		} else {
		k_msleep(1);
		}

		if (!isnan(pos.Yaw)) {
		data->currentYaw = pos.Yaw;
		}

		if (data->angleControl) {
		float delta_Yaw = data->targetYaw - data->currentYaw;
		delta_Yaw = fmodf(delta_Yaw, 360.0f);
		if (delta_Yaw > 180) {
		@@ -199,9 +206,11 @@ void cchassis_main_thread(const struct device dev, void ptr2, void *ptr3)
		} else {
		data->pid_input = 0;
		}
		}

		printk("Yaw: %f, targetYaw: %f, delta_Yaw: %f\n", data->currentYaw, data->targetYaw,
		delta_Yaw);
		// printk("Yaw: %f, targetYaw: %f, delta_Yaw: %f\n", data->currentYaw,
		// data->targetYaw,
		// delta_Yaw);
		if ((pos.accel[2] - 9.8f) > 0.4f) {
		// We are in the air
		}

include/zephyr/drivers/chassis.h

+9 −7

Original line number	Diff line number	Diff line
		@@ -30,6 +30,8 @@ extern "C" {
		#define CHASSIS_DEVICE_DT_DEFINE(node_id, init_fn, pm, data, config, level, prio, api, ...) \
		DEVICE_DT_DEFINE(node_id, init_fn, pm, data, config, level, prio, api, __VA_ARGS__)

		#define CHASSIS_WHEEL_COUNT DT_PROP_LEN(DT_INST(0, ares_chassis), wheels)

		typedef struct {
		float speedX;
		float speedY;
		@@ -60,8 +62,8 @@ typedef struct {

		bool angleControl;

		float angle_to_center[CONFIG_CHASSIS_MAX_STEERWHHEL_COUNT];
		float distance_to_center[CONFIG_CHASSIS_MAX_STEERWHHEL_COUNT];
		float angle_to_center[CHASSIS_WHEEL_COUNT];
		float distance_to_center[CHASSIS_WHEEL_COUNT];

		chassis_status_t chassis_status;

		@@ -73,9 +75,9 @@ typedef struct {

		typedef struct {
		struct pid_data *angle_pid;
		const struct device *wheels[CONFIG_CHASSIS_MAX_STEERWHHEL_COUNT];
		float pos_X_offset[CONFIG_CHASSIS_MAX_STEERWHHEL_COUNT];
		float pos_Y_offset[CONFIG_CHASSIS_MAX_STEERWHHEL_COUNT];
		const struct device *wheels[CHASSIS_WHEEL_COUNT];
		float pos_X_offset[CHASSIS_WHEEL_COUNT];
		float pos_Y_offset[CHASSIS_WHEEL_COUNT];
		} chassis_cfg_t;

		/**
		@@ -143,10 +145,10 @@ static inline chassis_status_t z_impl_chassis_get_status(const struct device d
		return NULL;
		}

		static void chassis_set_sensor_zbus(const struct device dev, const struct zbus_channel chan)
		static void chassis_update_sensor(const struct device dev, struct pos_data pos_data)
		{
		chassis_data_t *data = dev->data;
		data->chassis_sensor_zbus = (struct zbus_channel *)chan;
		zbus_chan_pub(data->chassis_sensor_zbus, &pos_data, K_MSEC(5));
		}

		#ifdef __cplusplus

Admin message