bugs in dji motor driver fixed (e725791a) · Commits · Wenxi XU / Mambo

.vscode/settings.json

+1 −1

Original line number	Diff line number	Diff line
		@@ -32,7 +32,7 @@
		},
		"C_Cpp.formatting": "clangFormat",
		"C_Cpp.errorSquiggles": "disabled",
		"cortex-debug.variableUseNaturalFormat": false,
		"cortex-debug.variableUseNaturalFormat": true,
		"C_Cpp.default.compileCommands": "${workspaceFolder}/.vscode/compile_commands.json",
		"cmake.configureOnOpen": false,

assembly.txt

+75034 −79740

File changed.

Preview size limit exceeded, changes collapsed.

drivers/chassis/swchassis.c

+7 −9

Original line number	Diff line number	Diff line
		@@ -110,7 +110,6 @@ void swchassis_resolve(chassis_data_t data, const chassis_cfg_t cfg)
		currentSpeedX[idx] = speed * cos_theta;
		currentSpeedY[idx] = speed * sin_theta;

		printk("steerwheel%d: angle=%f speed=%f\n", idx, (double)angle, (double)speed);
		idx++;
		}

		@@ -130,20 +129,19 @@ void swchassis_resolve(chassis_data_t data, const chassis_cfg_t cfg)
		float speedY = rollSpeedY + data->targetYSpeed;
		float steerwheel_speed;
		arm_sqrt_f32(speedX * speedX + speedY * speedY, &steerwheel_speed);
		float steerwheel_angle;
		float steerwheel_angle = 0;
		arm_atan2_f32(speedY, speedX, &steerwheel_angle);

		printk("steerwheel%d: angle=%f speed X=%f Y=%f ", idx, (double)steerwheel_angle,
		(double)speedX, (double)speedY);

		steerwheel_angle = -RAD2DEG(steerwheel_angle) + 90.0f;

		printk("deg: %f\n", (double)steerwheel_angle);

		if (fabsf(steerwheel_speed) > 0.05f) {
		if (fabsf(steerwheel_speed) > 0.06f \|\| fabsf(data->targetGyro) > 0.2f) {
		steerwheel_set_angle(cfg->steerwheels[idx], steerwheel_angle);
		}
		steerwheel_set_speed(cfg->steerwheels[idx], steerwheel_speed);
		} else {
		steerwheel_set_speed(cfg->steerwheels[idx], 0);
		}

		idx++;
		}
		@@ -162,12 +160,12 @@ void swchassis_main_thread(const struct device dev, void ptr2, void *ptr3)

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

drivers/chassis/swchassis.h

+8 −1

Original line number	Diff line number	Diff line
		#include "zephyr/drivers/pid.h"
		#include <zephyr/device.h>
		#include <zephyr/devicetree.h>
		#include <zephyr/drivers/chassis.h>
		@@ -39,7 +40,8 @@

		#define CHASSIS_CONFIG_INST(inst) \
		static const chassis_cfg_t chassis_cfg_##inst = { \
		.angle_pid = DEVICE_DT_GET(DT_PROP(DT_DRV_INST(inst), angle_pid)), \
		.angle_pid = &PID_INS_NAME(DT_PROP(DT_DRV_INST(inst), angle_pid), \
		DT_NODE_FULL_NAME_UNQUOTED(DT_DRV_INST(inst))), \
		.steerwheels = STEERWHEELS_FOREACH(inst, GET_SW_DEVICE), \
		.pos_X_offset = STEERWHEELS_FOREACH(inst, GET_SW_X_OFFSET), \
		.pos_Y_offset = STEERWHEELS_FOREACH(inst, GET_SW_Y_OFFSET), \
		@@ -50,7 +52,12 @@
		&chassis_cfg_##inst, POST_KERNEL, CONFIG_CHASSIS_INIT_PRIORITY, \
		&swchassis_driver_api);

		#define CHASSIS_PID_DEFINE(inst) \
		PID_NEW_INSTANCE(DT_PROP(DT_DRV_INST(inst), angle_pid), \
		DT_NODE_FULL_NAME_UNQUOTED(DT_DRV_INST(inst)))

		#define CHASSIS_INIT(inst) \
		CHASSIS_PID_DEFINE(inst) \
		CHASSIS_CONFIG_INST(inst) \
		CHASSIS_DATA_INST(inst) \
		CHASSIS_DEFINE_INST(inst)

drivers/motor/dji/dji_macros.h

+1 −1

Original line number	Diff line number	Diff line
		@@ -7,7 +7,7 @@
		#include <zephyr/devicetree.h>
		#include <zephyr/drivers/motor.h>

		#define CAN_SEND_STACK_SIZE 2048
		#define CAN_SEND_STACK_SIZE 1536
		#define CAN_SEND_PRIORITY -1

		#define HIGH_BYTE(x) ((x) >> 8)

Admin message