diff --git a/doc/todo.md b/doc/todo.md deleted file mode 100644 index 3d45259..0000000 --- a/doc/todo.md +++ /dev/null @@ -1,7 +0,0 @@ -1. 实测轮距、轴距和最大可用转角 -修改了转角中位值的问题，保持直线行驶 SERVO_INIT 1590 - -2. 修正后轮半径公式 -3. 先限制转角，再计算左右轮速 -4. 重新标定舵机 PWM 与实际轮角关系 -5. 辨识后轮电机和调整 PI \ No newline at end of file diff --git a/doc/时间戳.md b/doc/时间戳.md deleted file mode 100644 index 0321bc3..0000000 --- a/doc/时间戳.md +++ /dev/null @@ -1,349 +0,0 @@ -针对现在的“FreeRTOS 下位机 + Linux/ROS 2 上位机 + 后续轮速/IMU/2D雷达融合”，建议直接制定一套统一的时间戳与通信协议。 - -最核心的原则是： - -[ -\boxed{ -\text{MCU原始单调时间戳永不修改} -\quad+\quad -\text{上位机维护时钟映射} -} -] - -即： - -[ -T_{\rm host}=a,t_{\rm mcu}+b -] - -## 1. 下位机统一时间基准 - -不要把 `xTaskGetTickCount()` 作为传感器主时间戳。 - -它适合： - -* `vTaskDelayUntil()` 周期调度； -* 超时判断； -* 任务延时； -* 毫秒级状态机。 - -它不适合： - -* IMU与编码器精确对齐； -* 200 Hz以上传感器采样； -* 分析通信延迟； -* 后续雷达、IMU、轮式里程计融合。 - -FreeRTOS Tick 的分辨率由 `configTICK_RATE_HZ` 决定；ISR 中若确实要读 Tick，应使用 `xTaskGetTickCountFromISR()`。[FreeRTOS Task Utilities](https://www.freertos.org/Documentation/02-Kernel/04-API-references/03-Task-utilities/00-Task-utilities) - -### 推荐时间源 - -使用一个32位硬件定时器： - -```text -计数频率：1 MHz -计数单位：1 us -自动重装值：0xFFFFFFFF -软件扩展：64 bit -时间含义：MCU自本次上电以来经过的微秒数 -``` - -例如 STM32 可以使用 TIM2 或 TIM5： - -[ -t_{\rm mcu}\in uint64_t,\qquad 单位=\mu s -] - -32位、1 MHz计数器约每71.58分钟回绕一次，所以用更新中断增加高32位。 - -```c -static volatile uint32_t g_time_high = 0; - -void TIM2_IRQHandler(void) -{ - if (__HAL_TIM_GET_FLAG(&htim2, TIM_FLAG_UPDATE) != RESET) { - __HAL_TIM_CLEAR_IT(&htim2, TIM_IT_UPDATE); - g_time_high++; - } -} - -uint64_t mcu_time_us(void) -{ - uint32_t high1; - uint32_t high2; - uint32_t low; - uint32_t status; - - do { - high1 = g_time_high; - __DMB(); - - low = TIM2->CNT; - status = TIM2->SR; - - __DMB(); - high2 = g_time_high; - } while (high1 != high2); - - /* - * 计数器已经回绕，但更新中断可能还没有得到执行。 - * low 小于半量程，说明读取发生在回绕之后。 - */ - if ((status & TIM_SR_UIF) && low < 0x80000000U) { - high1++; - } - - return ((uint64_t)high1 << 32) | low; -} -``` - -注意： - -* 定时器时钟必须确认是否受 APB 分频后的“定时器倍频”影响； -* 不要直接读取一个由中断写入的普通 `uint64_t`，32位MCU上未必原子； -* 如果启用了 STOP 模式或 Tickless Idle，要确认这个定时器休眠时是否继续工作； -* 时间戳只能递增，不能因为校时而向前或向后跳变。 - -## 2. 时间戳应该在哪里打 - -时间戳必须尽可能靠近数据真正产生的位置。 - -| 数据 | 推荐时间戳位置 | -| -------- | ------------- | -| IMU | DRDY中断发生时 | -| SPI读取IMU | 不要等SPI读取完成才打点 | -| 编码器 | 控制定时器锁存计数器时 | -| 电机控制量 | PWM寄存器更新时 | -| 控制周期 | 控制周期入口 | -| 上位机接收 | 完整帧接收完成时，单独记录 | - -例如： - -```c -typedef struct { - uint64_t sample_time_us; - int16_t gyro_raw[3]; - int16_t accel_raw[3]; -} ImuSample; -``` - -IMU中断中只做： - -1. 读取 `mcu_time_us()`； -2. 保存时间戳； -3. 通知采集任务； -4. 立即退出中断。 - -不要在中断中完成协议编码、CRC计算和串口发送。 - -如果传感器内部启用了低通滤波，DRDY时间仍然可能比真实物理测量晚一个固定群延迟。后续可以增加： - -```c -corrected_time_us = drdy_time_us - imu_filter_delay_us; -``` - -但必须在确定传感器滤波器延迟后再补偿。 - -## 3. FreeRTOS任务结构 - -建议使用： - -```mermaid -flowchart TD - A["IMU DRDY / 控制定时器 ISR"] --> B["采样环形缓冲区"] - B --> C["传感器处理任务"] - C --> D["协议发送队列"] - D --> E["唯一 UART TX 任务"] - F["UART DMA / IDLE ISR"] --> G["接收环形缓冲区"] - G --> H["协议解析任务"] -``` - -关键规则： - -* UART发送只能有一个所有者任务，避免多个任务发送的数据交叉； -* UART接收使用DMA、IDLE中断和环形缓冲区； -* ISR只负责搬运数据和通知任务； -* 二进制协议和 `printf()` 调试日志不要共用同一个UART； -* 如果ISR调用 FreeRTOS 的 `...FromISR()` 接口，中断优先级必须符合 `configMAX_SYSCALL_INTERRUPT_PRIORITY` 约束； -* 周期任务使用 `vTaskDelayUntil()`，它能避免普通相对延时产生的累计漂移，但实际执行时刻仍应调用硬件微秒时钟记录。[FreeRTOS `vTaskDelayUntil()`](https://freertos.org/xtaskdelayuntiltask-control.html) - -例如200 Hz低精度周期任务： - -```c -void SensorTask(void *argument) -{ - TickType_t last_wake = xTaskGetTickCount(); - const TickType_t period = pdMS_TO_TICKS(5); - - for (;;) { - vTaskDelayUntil(&last_wake, period); - - uint64_t actual_time_us = mcu_time_us(); - - /* 采集和处理 */ - } -} -``` - -如果要求更稳定的200 Hz采样，应由硬件定时器产生中断，再通知任务，而不是完全依赖 FreeRTOS Tick。 - -## 4. 推荐二进制帧规范 v1 - -假设使用 UART 或 USB CDC，建议采用： - -```text -COBS编码（协议头 + 负载 + CRC32） + 0x00帧分隔符 -``` - -COBS的好处是任意解析错误后，都可以在下一个 `0x00` 重新找到帧边界。 - -### 统一协议头：24字节 - -之前已有的基础上额外增加 -| session_id | uint32 | 4 | 本次MCU启动会话ID | -| sample_time_us | uint64 | 8 | MCU采样时间 | - - -但不要直接执行： - -```c -uart_send((uint8_t *)&header, sizeof(header)); // 不推荐 -``` - -因为不同编译器可能存在： - -* 结构体填充； -* 对齐差异； -* 大小端差异； -* 浮点格式差异。 - - | - - -## 7. 四时间戳同步协议 - -每次同步记录： - -* (T_1)：上位机发送请求； -* (t_2)：MCU收到并解析请求； -* (t_3)：MCU准备发送响应； -* (T_4)：上位机收到完整响应。 - -定义MCU时间减上位机时间为： - -[ -\theta= -\frac{(t_2-T_1)+(t_3-T_4)}{2} -] - -往返延迟： - -[ -d=(T_4-T_1)-(t_3-t_2) -] - -初始转换： - -[ -T_{\rm host}\approx t_{\rm mcu}-\theta -] - -运行中收集多组： - -[ -x_i=\frac{t_{2,i}+t_{3,i}}{2} -] - -[ -y_i=\frac{T_{1,i}+T_{4,i}}{2} -] - -拟合： - -[ -\boxed{T_{\rm host}=a,t_{\rm mcu}+b} -] - -推荐参数： - -* 启动时连续同步20～50次； -* 初始阶段先令 (a=1)，从最小RTT样本估计 (b)； -* 运行中每1秒同步一次； -* 保存最近60～120组； -* 丢弃RTT明显偏大的样本； -* 使用RTT最小的20%～30%拟合 (a,b)； -* MCU时间戳本身永远不被“校准”或重写。 - -上位机使用 `CLOCK_MONOTONIC_RAW` 或 `std::chrono::steady_clock`，不要使用会被NTP校时改变的墙上时间参与控制。 - -## 8. ROS 2时间戳转换 - -MCU的“上电微秒数”不能直接填进 ROS 消息的 `header.stamp`，否则无法和激光雷达时间对齐。 - -上位机先得到： - -[ -T_{\rm steady}=a,t_{\rm mcu}+b -] - -再在ROS桥接节点中保存一对锚点： - -[ -(T_{\rm steady,0},T_{\rm ros,0}) -] - -转换为： - -[ -T_{\rm ros,sample} -================== - -T_{\rm ros,0} -+ -(T_{\rm steady,sample}-T_{\rm steady,0}) -] - -然后将这个时间填入： - -* `sensor_msgs/Imu.header.stamp`； -* `nav_msgs/Odometry.header.stamp`； -* 编码器或轮速自定义消息时间戳。 - -这样轮速、IMU和激光雷达才能在同一ROS时间轴上融合。 - -## 9. 上位机命令不要依赖绝对时间 - -普通运动命令建议使用“接收后立即执行＋超时”： - -```c -typedef struct { - uint32_t command_sequence; - uint32_t valid_for_us; - float velocity_ref; - float yaw_rate_ref; -} MotionCommand; -``` - -MCU收到时记录： - -```c -last_command_rx_us = mcu_time_us(); -``` - -安全判断： - -```c -if (mcu_time_us() - last_command_rx_us > command.valid_for_us) { - enter_safe_deceleration(); -} -``` - -只有真正需要未来定时执行时，才让上位机通过逆映射计算： - -[ -t_{\rm mcu,execute} -=================== - -\frac{T_{\rm host,execute}-b}{a} -] -