robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

云台好了

Browse Source
This commit is contained in:
Robofish 2026-01-12 01:19:16 +08:00
parent 8c44c6a274
commit 39adf0a2b6
9 changed files with 8591 additions and 6396 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
MDK-ARM/CtrBoard-H7_ALL.uvoptx
View File

@ -157,18 +157,34 @@
<Bp> <Bp>
<Number>0</Number> <Number>0</Number>
<Type>0</Type> <Type>0</Type>
<LineNumber>56</LineNumber> <LineNumber>0</LineNumber>
<EnabledFlag>1</EnabledFlag> <EnabledFlag>1</EnabledFlag>
<Address>134292314</Address> <Address>128</Address>
<ByteObject>0</ByteObject> <ByteObject>0</ByteObject>
<HtxType>0</HtxType> <HtxType>0</HtxType>
<ManyObjects>0</ManyObjects> <ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject> <SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess> <BreakByAccess>0</BreakByAccess>
<BreakIfRCount>1</BreakIfRCount> <BreakIfRCount>1</BreakIfRCount>
<Filename>..\User\task\ctrl_gimbal.c</Filename> <Filename></Filename>
<ExecCommand></ExecCommand> <ExecCommand></ExecCommand>
<Expression>\\CtrBoard_H7_ALL\../User/task/ctrl_gimbal.c\56</Expression> <Expression>0x00000080</Expression>
</Bp>
<Bp>
<Number>1</Number>
<Type>0</Type>
<LineNumber>0</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>134</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>1</BreakIfRCount>
<Filename></Filename>
<ExecCommand></ExecCommand>
<Expression>0x00000086</Expression>
</Bp> </Bp>
</Breakpoint> </Breakpoint>
<WatchWindow1> <WatchWindow1>
@ -197,6 +213,41 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>GIMBAL_IMU_Update</ItemText> <ItemText>GIMBAL_IMU_Update</ItemText>
</Ww> </Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>rx_msg</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>count</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>rx_msg</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>can2_debug</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>fdcan2_init_debug</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>rx_debug</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_for_gimbal</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
@ -1056,7 +1107,7 @@
<Group> <Group>
<GroupName>User/component</GroupName> <GroupName>User/component</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -1184,7 +1235,7 @@
<Group> <Group>
<GroupName>User/device</GroupName> <GroupName>User/device</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>

2
MDK-ARM/CtrBoard-H7_ALL.uvprojx
View File

@ -314,7 +314,7 @@
</ArmAdsMisc> </ArmAdsMisc>
<Cads> <Cads>
<interw>1</interw> <interw>1</interw>
<Optim>4</Optim> <Optim>1</Optim>
<oTime>0</oTime> <oTime>0</oTime>
<SplitLS>0</SplitLS> <SplitLS>0</SplitLS>
<OneElfS>1</OneElfS> <OneElfS>1</OneElfS>

BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
View File

Binary file not shown.

14797
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
View File

File diff suppressed because it is too large Load Diff

47
User/device/gimbal_imu.c
View File

@ -21,7 +21,7 @@
* @param bits 位数 * @param bits 位数
* @return float 转换后的浮点数 * @return float 转换后的浮点数
*/ */
static float uint_to_float(uint16_t x_int, float x_min, float x_max, int bits) { static float uint_to_float(uint32_t x_int, float x_min, float x_max, int bits) {
float span = x_max - x_min; float span = x_max - x_min;
float offset = x_min; float offset = x_min;
return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset; return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
@ -37,37 +37,34 @@ static void GIMBAL_IMU_ParseMessage(GIMBAL_IMU_t *gimbal_imu, uint32_t id, const
/* 判断是哪个ID的数据 */ /* 判断是哪个ID的数据 */
if (id == gimbal_imu->param.accl_id) { if (id == gimbal_imu->param.accl_id) {
/* 解析加速度计数据 */ /* 解析加速度计数据 - 21位精度解包 */
uint16_t acc_x = (uint16_t)data[2] | ((uint16_t)data[3] << 8); uint32_t acc_x = ((uint32_t)data[0] << 13) | ((uint32_t)data[1] << 5) | ((uint32_t)data[2] >> 3);
uint16_t acc_y = (uint16_t)data[4] | ((uint16_t)data[5] << 8); uint32_t acc_y = (((uint32_t)data[2] & 0x07) << 18) | ((uint32_t)data[3] << 10) | ((uint32_t)data[4] << 2) | ((uint32_t)data[5] >> 6);
uint16_t acc_z = (uint16_t)data[6] | ((uint16_t)data[7] << 8); uint32_t acc_z = (((uint32_t)data[5] & 0x3F) << 15) | ((uint32_t)data[6] << 7) | ((uint32_t)data[7] >> 1);
gimbal_imu->data.accl.x = uint_to_float(acc_x, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 16); gimbal_imu->data.accl.x = uint_to_float(acc_x, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 21);
gimbal_imu->data.accl.y = uint_to_float(acc_y, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 16); gimbal_imu->data.accl.y = uint_to_float(acc_y, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 21);
gimbal_imu->data.accl.z = uint_to_float(acc_z, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 16); gimbal_imu->data.accl.z = uint_to_float(acc_z, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 21);
/* 温度数据 */
gimbal_imu->data.temp = (float)data[1];
} else if (id == gimbal_imu->param.gyro_id) { } else if (id == gimbal_imu->param.gyro_id) {
/* 解析陀螺仪数据 */ /* 解析陀螺仪数据 - 21位精度解包 */
uint16_t gyro_x = (uint16_t)data[2] | ((uint16_t)data[3] << 8); uint32_t gyro_x = ((uint32_t)data[0] << 13) | ((uint32_t)data[1] << 5) | ((uint32_t)data[2] >> 3);
uint16_t gyro_y = (uint16_t)data[4] | ((uint16_t)data[5] << 8); uint32_t gyro_y = (((uint32_t)data[2] & 0x07) << 18) | ((uint32_t)data[3] << 10) | ((uint32_t)data[4] << 2) | ((uint32_t)data[5] >> 6);
uint16_t gyro_z = (uint16_t)data[6] | ((uint16_t)data[7] << 8); uint32_t gyro_z = (((uint32_t)data[5] & 0x3F) << 15) | ((uint32_t)data[6] << 7) | ((uint32_t)data[7] >> 1);
gimbal_imu->data.gyro.x = uint_to_float(gyro_x, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 16); gimbal_imu->data.gyro.x = uint_to_float(gyro_x, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 21);
gimbal_imu->data.gyro.y = uint_to_float(gyro_y, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 16); gimbal_imu->data.gyro.y = uint_to_float(gyro_y, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 21);
gimbal_imu->data.gyro.z = uint_to_float(gyro_z, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 16); gimbal_imu->data.gyro.z = uint_to_float(gyro_z, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 21);
} else if (id == gimbal_imu->param.eulr_id) { } else if (id == gimbal_imu->param.eulr_id) {
/* 解析欧拉角数据 */ /* 解析欧拉角数据 - 21位精度解包 */
uint16_t pit = (uint16_t)data[2] | ((uint16_t)data[3] << 8); uint32_t pit = ((uint32_t)data[0] << 13) | ((uint32_t)data[1] << 5) | ((uint32_t)data[2] >> 3);
uint16_t yaw = (uint16_t)data[4] | ((uint16_t)data[5] << 8); uint32_t yaw = (((uint32_t)data[2] & 0x07) << 18) | ((uint32_t)data[3] << 10) | ((uint32_t)data[4] << 2) | ((uint32_t)data[5] >> 6);
uint16_t rol = (uint16_t)data[6] | ((uint16_t)data[7] << 8); uint32_t rol = (((uint32_t)data[5] & 0x3F) << 15) | ((uint32_t)data[6] << 7) | ((uint32_t)data[7] >> 1);
gimbal_imu->data.eulr.pit = uint_to_float(pit, GIMBAL_IMU_PITCH_MIN, GIMBAL_IMU_PITCH_MAX, 16); gimbal_imu->data.eulr.pit = uint_to_float(pit, GIMBAL_IMU_PITCH_MIN, GIMBAL_IMU_PITCH_MAX, 21);
gimbal_imu->data.eulr.yaw = uint_to_float(yaw, GIMBAL_IMU_YAW_MIN, GIMBAL_IMU_YAW_MAX, 16); gimbal_imu->data.eulr.yaw = uint_to_float(yaw, GIMBAL_IMU_YAW_MIN, GIMBAL_IMU_YAW_MAX, 21);
gimbal_imu->data.eulr.rol = uint_to_float(rol, GIMBAL_IMU_ROLL_MIN, GIMBAL_IMU_ROLL_MAX, 16); gimbal_imu->data.eulr.rol = uint_to_float(rol, GIMBAL_IMU_ROLL_MIN, GIMBAL_IMU_ROLL_MAX, 21);
} else if (id == gimbal_imu->param.quat_id) { } else if (id == gimbal_imu->param.quat_id) {
/* 解析四元数数据 - 使用14位精度解包 */ /* 解析四元数数据 - 使用14位精度解包 */

12
User/module/config.c
View File

@ -66,11 +66,11 @@ Config_RobotParam_t robot_config = {
}, },
.mech_zero = { .mech_zero = {
.yaw = 0.0f, .yaw = 0.0f,
.pit = 1.77f, .pit = 2.12f,
}, },
.travel = { .travel = {
.yaw = -1.0f, .yaw = -1.0f,
.pit = 0.8f, .pit = 0.9f,
}, },
.low_pass_cutoff_freq = { .low_pass_cutoff_freq = {
.out = -1.0f, .out = -1.0f,
@ -92,10 +92,10 @@ Config_RobotParam_t robot_config = {
}, },
.imu = { .imu = {
.can = BSP_CAN_2, .can = BSP_CAN_2,
.accl_id = 0x64, // 100 .accl_id = 0x100, // 加速度计 (十进制256)
.gyro_id = 0x65, // 101 .gyro_id = 0x101, // 陀螺仪 (十进制257)
.eulr_id = 0x66, // 102 .eulr_id = 0x102, // 欧拉角 (十进制258)
.quat_id = 0x67 // 103 .quat_id = 0x103 // 四元数 (十进制259)
}, },
}, },

10
User/task/ctrl_gimbal.c
View File

@ -19,7 +19,7 @@
/* USER STRUCT BEGIN */ /* USER STRUCT BEGIN */
Gimbal_t gimbal; Gimbal_t gimbal;
Gimbal_CMD_t gimbal_cmd; Gimbal_CMD_t gimbal_cmd;
BSP_FDCAN_StdDataFrame_t can_frame; // BSP_FDCAN_StdDataFrame_t can_frame;
/* USER STRUCT END */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -42,9 +42,9 @@ void Task_ctrl_gimbal(void *argument) {
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */ /* USER CODE BEGIN */
can_frame.id = 0x200; // can_frame.id = 0x200;
can_frame.dlc = 8; // can_frame.dlc = 8;
BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame); // BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame);
Gimbal_UpdateIMU(&gimbal); Gimbal_UpdateIMU(&gimbal);
osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0); osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);
@ -53,7 +53,7 @@ void Task_ctrl_gimbal(void *argument) {
Gimbal_UpdateFeedback(&gimbal); Gimbal_UpdateFeedback(&gimbal);
// osMessageQueueReset(task_runtime.msgq.chassis_yaw); // osMessageQueueReset(task_runtime.msgq.chassis_yaw);
osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0); // osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0);
Gimbal_Control(&gimbal, &gimbal_cmd); Gimbal_Control(&gimbal, &gimbal_cmd);

4
User/task/init.c
View File

@ -9,6 +9,7 @@
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
#include "module/shoot.h" #include "module/shoot.h"
#include "module/balance_chassis.h" #include "module/balance_chassis.h"
#include "module/gimbal.h"
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -33,7 +34,7 @@ void Task_Init(void *argument) {
/* 创建任务线程 */ /* 创建任务线程 */
task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc); task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
task_runtime.thread.atti_esit = osThreadNew(Task_atti_esit, NULL, &attr_atti_esit); task_runtime.thread.atti_esit = osThreadNew(Task_atti_esit, NULL, &attr_atti_esit);
task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis); // task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal); task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal);
task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor); task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor);
task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink); task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
@ -46,6 +47,7 @@ void Task_Init(void *argument) {
task_runtime.msgq.chassis.imu = osMessageQueueNew(2u, sizeof(Chassis_IMU_t), NULL); task_runtime.msgq.chassis.imu = osMessageQueueNew(2u, sizeof(Chassis_IMU_t), NULL);
task_runtime.msgq.chassis.cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL); task_runtime.msgq.chassis.cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
task_runtime.msgq.chassis.yaw = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL); task_runtime.msgq.chassis.yaw = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL);
task_runtime.msgq.gimbal.cmd = osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
/* USER MESSAGE END */ /* USER MESSAGE END */
osKernelUnlock(); // 解锁内核 osKernelUnlock(); // 解锁内核

52
User/task/rc.c
View File

@ -9,6 +9,7 @@
#include "device/dr16.h" #include "device/dr16.h"
#include "module/shoot.h" #include "module/shoot.h"
#include "module/balance_chassis.h" #include "module/balance_chassis.h"
#include "module/gimbal.h"
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -19,6 +20,7 @@
DR16_t dr16; DR16_t dr16;
Shoot_CMD_t for_shoot; Shoot_CMD_t for_shoot;
Chassis_CMD_t cmd_for_chassis; Chassis_CMD_t cmd_for_chassis;
Gimbal_CMD_t cmd_for_gimbal;
/* USER STRUCT END */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -74,32 +76,34 @@ void Task_rc(void *argument) {
// task_runtime.msgq.chassis.cmd); // 重置消息队列，防止阻塞 // task_runtime.msgq.chassis.cmd); // 重置消息队列，防止阻塞
// osMessageQueuePut(task_runtime.msgq.chassis.cmd, &cmd_for_chassis, 0, // osMessageQueuePut(task_runtime.msgq.chassis.cmd, &cmd_for_chassis, 0,
// 0); // 非阻塞发送底盘控制命令 // 0); // 非阻塞发送底盘控制命令
/************************* 云台命令 **************************************/
switch (dr16.data.sw_l) {
case DR16_SW_UP:
cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
cmd_for_gimbal.delta_yaw = 0.0f;
cmd_for_gimbal.delta_pit = 0.0f;
break;
case DR16_SW_MID:
cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 5.0f;
cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 5.0f;
break;
case DR16_SW_DOWN:
cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 5.0f;
cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 5.0f;
break;
default:
cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
cmd_for_gimbal.delta_yaw = 0.0f;
cmd_for_gimbal.delta_pit = 0.0f;
break;
}
// switch (dr16.data.sw_l) { osMessageQueueReset(task_runtime.msgq.gimbal.cmd);
// case DR16_SW_UP: osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0);
// cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
// cmd_for_gimbal.delta_yaw = 0.0f;
// cmd_for_gimbal.delta_pit = 0.0f;
// break;
// case DR16_SW_MID:
// cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
// cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 5.0f;
// cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 5.0f;
// break;
// case DR16_SW_DOWN:
// cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
// cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 5.0f;
// cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 5.0f;
// break;
// default:
// cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
// cmd_for_gimbal.delta_yaw = 0.0f;
// cmd_for_gimbal.delta_pit = 0.0f;
// break;
// }
// osMessageQueueReset(task_runtime.msgq.gimbal.cmd); /************************* 底盘命令 **************************************/
// osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0);
switch (dr16.data.sw_l) { switch (dr16.data.sw_l) {
case DR16_SW_UP: case DR16_SW_UP:
cmd_for_chassis.mode = CHASSIS_MODE_RELAX; cmd_for_chassis.mode = CHASSIS_MODE_RELAX;