TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 03:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Codex generated changes:

Browse Source 
1.stool-mode
2.INS direction changes
not tested,not ok
This commit is contained in:
TuxMonkey
2026-07-14 01:07:48 +08:00
parent a4ebc4b452
commit 8a13444e48
7 changed files with 286 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
modules/algorithm/QuaternionEKF.c
View File

@@ -199,8 +199,8 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
// 利用四元数反解欧拉角
QEKF_INS.Yaw = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f;
QEKF_INS.Pitch = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f;
QEKF_INS.Roll = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
QEKF_INS.Pitch = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
QEKF_INS.Roll = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f;
// get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast > 180.0f)

8
modules/motor/DMmotor/dmmotor.c
View File

@@ -65,8 +65,7 @@ static void DMMotorDecode(CANInstance *motor_can)
static void DMMotorLostCallback(void *motor_ptr)
{
DMMotorInstance *motor = (DMMotorInstance *)motor_ptr;
DMMotorEnable(motor);
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
DMMotorStop(motor);
}
void DMMotorCaliEncoder(DMMotorInstance *motor)
@@ -131,17 +130,16 @@ void DMMotorTask(void const *argument)
Motor_Control_Setting_s *setting;
DMMotor_Send_s motor_send_mailbox;
portTickType currentTime;
const portTickType xFrequency = pdMS_TO_TICKS(4);
while (1)
{
currentTime = xTaskGetTickCount();
set = motor->pid_ref;
measure = &motor->measure;
setting = &motor->motor_settings;
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
set *= -1;
measure = &motor->measure;
setting = &motor->motor_settings;
if ((setting->close_loop_type & ANGLE_LOOP) && (setting->outer_loop_type & ANGLE_LOOP))
{
if (setting->angle_feedback_source == OTHER_FEED)

14
modules/referee/referee_task.c
View File

@@ -38,7 +38,7 @@ static void DeterminRobotID()
static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data);
static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data); // 模式切换检测
static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data); // 测试用函数，实现模式自动变化
static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) __attribute__((unused)); // 测试用函数，实现模式自动变化
referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Interactive_info_t *UI_data)
{
@@ -225,6 +225,15 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
case CHASSIS_FOLLOW_GIMBAL_YAW:
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow ");
break;
case CHASSIS_STOOL_MODE:
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "stool ");
break;
case CHASSIS_FREE_DEBUG:
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "debug ");
break;
case CHASSIS_ROTATE_REVERSE:
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "revrot ");
break;
}
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]);
_Interactive_data->Referee_Interactive_Flag.chassis_flag = 0;
@@ -263,6 +272,9 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
{
switch (_Interactive_data->loader_mode)
{
case LOAD_REVERSE:
UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_Change, 8, UI_Color_Purplish_red, 15, 2, 270, 600, "rev ");
break;
case LOAD_1_BULLET:
UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_Change, 8, UI_Color_Cyan, 15, 2, 270, 600, "1 bull");
break;