refactor: manual mode from absolute setpoint to velocity control, remove auto-enable at motor init

Manual mode now receives angular velocity (degrees/s) via 0xA6 frame instead of
absolute angle reference. The firmware integrates velocity * dt into gimbal angle ref.
Motors are no longer auto-enabled at DMMotorInit — they stay disabled until a mode
switch out of DEBUG. Extract ROBOT_CMD_DELAY macro to replace magic number.
Fix "volity" typo → "velocity" and update docs/protocol comments accordingly.
This commit is contained in:
2026-06-02 16:27:07 +08:00
parent 88702e2832
commit da6acdcc89
8 changed files with 30 additions and 30 deletions
+6 -6
View File
@@ -247,11 +247,11 @@ static void VisionTracking()
Kalman_Filter_Update(&pitch_error_kf);
filtered_pitch_error = pitch_error_kf.FilteredValue[0];
float yaw_increment = PIDCalculate(&vision_yaw_pid, filtered_yaw_error, 0);
float pitch_increment = PIDCalculate(&vision_pitch_pid, filtered_pitch_error, 0);
float yaw_degree_velocity = PIDCalculate(&vision_yaw_pid, filtered_yaw_error, 0);
float pitch_degree_velocity = PIDCalculate(&vision_pitch_pid, filtered_pitch_error, 0);
gimbal_yaw_ref += 0.001f * yaw_increment;
gimbal_pitch_ref += 0.001f * pitch_increment;
gimbal_yaw_ref += 0.001f *ROBOT_CMD_DELAY * yaw_degree_velocity;
gimbal_pitch_ref += 0.001f *ROBOT_CMD_DELAY * pitch_degree_velocity;
}
if (gimbal_pitch_ref > 30.0f)
@@ -269,8 +269,8 @@ static void ManualMode()
if (vision_data.manual_updated)
{
vision_data.manual_updated = 0;
gimbal_yaw_ref = vision_data.yaw_ref;
gimbal_pitch_ref = vision_data.pitch_ref;
gimbal_yaw_ref += 0.001f *ROBOT_CMD_DELAY * vision_data.yaw_degree_velocity;
gimbal_pitch_ref += 0.001f *ROBOT_CMD_DELAY * vision_data.pitch_degree_velocity;
}
if (gimbal_pitch_ref > 30.0f)
+3
View File
@@ -5,6 +5,9 @@
#include "ins_task.h"
#include "stdint.h"
// 机器人核心任务的延时时间(ms)
#define ROBOT_CMD_DELAY 2
/* 开发板类型定义 */
#define ONE_BOARD // 单板控制整车
+1 -1
View File
@@ -113,6 +113,6 @@ __attribute__((noreturn)) void StartROBOTTASK(void *argument)
robot_dt = DWT_GetTimeline_ms() - robot_start;
if (robot_dt > 5)
LOGERROR("[freeRTOS] ROBOT core Task DELAY dt=%.3f", robot_dt);
osDelay(2);
osDelay(ROBOT_CMD_DELAY);
}
}