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.
2026-06-02 16:27:07 +08:00
parent 88702e2832
commit da6acdcc89
8 changed files with 30 additions and 30 deletions
@@ -69,7 +69,7 @@ main()
 │                                                      │
 │  AUTO:   上位机 0xA5 帧 → error_x/y → KF → vision PID│
 │          → 角度增量 → 累计 angle ref                  │
-│  MANUAL: 上位机 0xA6 帧 → yaw_ref/pitch_ref 直接设定 │
+│  MANUAL: 上位机 0xA6 帧 → yaw/pitch 角速度 → 角度增量积分  │
 │  DEBUG:  上位机 0xA8 帧 → 实时更新 PID 参数           │
 │                                                      │
 │  ProcessCommand() 处理模式切换 / 激光控制            │
@@ -108,7 +108,7 @@ main()
 | 帧头 | 长度 | 用途 |
 |------|------|------|
 | 0xA5 | 10 | 自动/视觉追踪数据帧 (error_x, error_y) |
-| 0xA6 | 11 | 手动控制数据帧 (zero_force, yaw_ref, pitch_ref) |
+| 0xA6 | 11 | 手动控制数据帧 (zero_force, yaw_velocity, pitch_velocity) |
 | 0xA7 | 3 | 命令帧 (模式切换, 激光) |
 | 0xA8 | 50 | 调试调参帧 (12 个 float PID 参数) |

@@ -127,7 +127,7 @@ main()
 | 模式 | 值 | 说明 |
 |------|-----|------|
 | AUTO | 0 | 视觉误差 → KF → vision PID → 角度增量 |
-| MANUAL | 1 | 上位机直接下发角度参考值 |
+| MANUAL | 1 | 上位机下发角速度，固件积分累加角度参考 |
 | DEBUG | 2 | 失能电机，接收 PID 参数实时调参 |

 ### 数据流
@@ -135,14 +135,14 @@ main()
 ```
 上位机 USART10 → DecodeVision() → vision_data (全局结构体)
  ├── 0xA5 → auto_updated=1, error_x/y
-  ├── 0xA6 → manual_updated=1, zero_force, yaw_ref/pitch_ref
+  ├── 0xA6 → manual_updated=1, zero_force, yaw/pitch velocity
  ├── 0xA7 → cmd_pending=1, cmd
  └── 0xA8 → debug_updated=1, 12 个 PID 参数

 robot_cmd 每轮:
  ProcessCommand() → 模式切换 / 激光
  AUTO:  vision_data.error_x/y → KF → vision PID → 角度增量
-  MANUAL: vision_data.yaw_ref/pitch_ref → 直接设定
+  MANUAL: vision_data.yaw/pitch velocity → 角度增量积分
  DEBUG: vision_data PID 参数 → 直接写入 vision PID, 发布 motor_pid 话题
  PubPushMessage(gimbal_cmd) → gimbal 执行
 ```
@@ -256,7 +256,7 @@ DMMotorControl() @ 1kHz:
              │      RobotCMD          │
              │   ProcessCommand()     │ ← 模式切换/激光
              │   VisionTracking()     │ ← KF → vision PID → 角度增量
-              │   ManualMode()         │ ← 直接角度参考
+              │   ManualMode()         │ ← 角速度积分
              │   DebugMode()          │ ← 实时 PID 调参
              │   PubPushMessage()     │
              └───────┬───────┬────────┘
@@ -324,7 +324,7 @@ typedef struct {
    float error_x, error_y;           // 自动模式: 归一化误差
    uint8_t auto_updated;
    uint8_t zero_force;               // 手动模式: 零力标志
-    float yaw_ref, pitch_ref;         // 手动模式: 角度参考
+    float yaw_degree_velocity, pitch_degree_velocity;  // 手动模式: 角速度
    uint8_t manual_updated;
    // 调试模式数据
    float vision_yaw_Kp, vision_yaw_Ki, vision_yaw_Kd;
@@ -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)
@@ -5,6 +5,9 @@
 #include "ins_task.h"
 #include "stdint.h"

+// 机器人核心任务的延时时间(ms)
+#define ROBOT_CMD_DELAY 2
+
 /* 开发板类型定义 */
 #define ONE_BOARD // 单板控制整车

@@ -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);
    }
 }
@@ -55,28 +55,28 @@ def send_auto_tracking(ser, error_x: float, error_y: float):

 ## 2. 手动控制数据帧 0xA6 (11 字节)

-**用途**: 手动模式下发送角度目标值。
+**用途**: 手动模式下发送角速度目标值。

 ```
-[SOF:0xA6][zero_force:1][yaw_ref:4][pitch_ref:4][checksum:1]
+[SOF:0xA6][zero_force:1][yaw_velocity:4][pitch_velocity:4][checksum:1]
 ```

 | 偏移 | 长度 | 类型 | 含义 |
 |---|---|---|---|
 | 0 | 1 | uint8 | 帧头 0xA6 |
 | 1 | 1 | uint8 | 零力标志 (0=正常, 1=零力) |
-| 2 | 4 | float | Yaw 角度参考 (degrees) |
-| 6 | 4 | float | Pitch 角度参考 (degrees) |
+| 2 | 4 | float | Yaw 角速度 (degrees/s) |
+| 6 | 4 | float | Pitch 角速度 (degrees/s) |
 | 10 | 1 | uint8 | checksum |

 **Python 写入示例**:

 ```python
-def send_manual_control(ser, yaw_deg: float, pitch_deg: float, zero_force: int = 0):
+def send_manual_control(ser, yaw_deg_per_s: float, pitch_deg_per_s: float, zero_force: int = 0):
    data = bytearray()
    data.append(0xA6)
    data.append(0 if zero_force else 0)
-    data.extend(struct.pack('<ff', yaw_deg, pitch_deg))
+    data.extend(struct.pack('<ff', yaw_deg_per_s, pitch_deg_per_s))
    checksum = sum(data) & 0xFF
    data.append(checksum)
    ser.write(data)
@@ -38,8 +38,8 @@ static void DecodeVision()
        if (calc_checksum(buf, VISION_MANUAL_LEN) != buf[VISION_MANUAL_LEN - 1])
            return;
        vision_data.zero_force = buf[1];
-        memcpy(&vision_data.yaw_ref,   buf + 2, 4);
-        memcpy(&vision_data.pitch_ref, buf + 6, 4);
+        memcpy(&vision_data.yaw_degree_velocity,   buf + 2, 4);
+        memcpy(&vision_data.pitch_degree_velocity, buf + 6, 4);
        vision_data.manual_updated = 1;
    }
    else if (sof == VISION_SOF_DEBUG && len == VISION_DEBUG_LEN)
@@ -16,9 +16,9 @@
 *   error_x/y: float, 归一化误差, 视觉PID外环反馈值
 *
 * ---- 手动控制数据帧 0xA6 (11 字节) ----
- *   [SOF:0xA6][zero_force:1][yaw_ref:4][pitch_ref:4][checksum:1]
- *   zero_force: 0=正常角度控制, 1=零力/保持当前位置
- *   yaw_ref/pitch_ref: float, 角度参考值 (degrees)
+ *   [SOF:0xA6][zero_force:1][yaw_velocity:4][pitch_velocity:4][checksum:1]
+ *   zero_force: 0=正常速度控制, 1=零力/保持当前位置
+ *   yaw_velocity/pitch_velocity: float, 角速度 (degrees/s)
 *
 * ---- 命令帧 0xA7 (3 字节) ----
 *   [SOF:0xA7][cmd:1][checksum:1]
@@ -68,8 +68,8 @@ typedef struct

    /* 手动模式数据 */
    uint8_t zero_force;
-    float yaw_ref;
-    float pitch_ref;
+    float yaw_degree_velocity;
+    float pitch_degree_velocity;
    uint8_t manual_updated; // 收到新的手动数据帧标志

    /* 调试模式数据 */
@@ -191,9 +191,6 @@ DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config)
    };
    //motor->motor_daemon = DaemonRegister(&conf);

-    /* enable motor */
-    DMMotorSetMode(DM_CMD_ENABLE_MODE, motor);
-
    dm_motor_instance[idx++] = motor;
    return motor;
 }