ArduPilot开源代码之AP_WheelEncoder

1. 源由

最近调试ArduRover，出现一些意外状况，编码器无法正常工作。从整个过程了解下来，应该和代码实现有一定关系。

从另外的角度而言，编码器暂时的兼容性可能是存在讨论的地方。

• Internal Errors 0x2000000 with WheelEncoder – ArduRover 4.5.2

2. 工作原理

2.1 正交输出波形

增量编码器通常有两个输出信号，称为A相和B相。这两个信号是方波，并且它们之间的相位差为90度。正因为这种相位关系，这种输出被称为正交输出。

当旋转编码器轴旋转时，A相和B相信号交替变化，通过比较这两个信号，可以确定旋转的方向和步进数。

• 顺时针旋转：A相在B相之前上升
• 逆时针旋转：B相在A相之前上升

通过检测A相和B相的变化顺序，电子设备可以准确地判断旋转方向和角度。增量编码器的这种正交输出方式使得它们在需要精确位置和速度控制的应用中非常有用，如数控机床、机器人和旋转控制系统。

2.2 过滤抖动噪音

通常机械的编码器都有接触噪音。常见的抖动噪音如下所示：

处理编码器信号时，接触噪音是一个主要问题。它们会导致错误的方向和旋转检测，并使使用编码器变得困难。我们可以通过在软件中滤除接触噪音或使用一些额外的滤波电路来消除接触噪音。

在单片机软件中滤除噪音是一种选择，但它也有一些缺点。您需要编写更复杂的代码来处理噪音。滤波会占用处理时间，并且会延迟您的工作流程。您可能需要设置定时器来忽略嘈杂的间隔。

通过使用额外的硬件来滤除噪音更容易，它可以在源头上停止噪音。您需要的是一个一阶 RC 滤波器。您可以看到在使用 RC 滤波器后信号的样子。
RC滤波器减慢了下降时间和上升时间，并提供了硬件去抖动。在选择电阻和电容对时，您应该考虑旋转的最大频率。否则，编码器的预期响应也会被滤波。

因此采用光电编码器使用日益频繁，相对来说没有机械噪音。

2.3 规格参数

• PPR：Pulse Per Revolution，电机每转脉冲数
• CPR：Counts Per Revolution，轮子每转脉冲计数
• GR：Gear Ratio，减速比

$$Distance=\frac{Counts}{CPR}\ast \pi \ast Diameter$$

2.4 参数设置

举例 - 编码器规格

• GR：1:30
• PPR：1024 PPR
• 75mm rubber tire

正规计算方法：
$$CPR=PPR\ast GR\ast 4$$
$$Distance=\frac{Counts}{PPR\ast GR\ast 4}\ast \pi \ast Diameter=\frac{Counts}{PPR\ast GR}\ast \pi \ast \frac{Diameter}{4}$$

---

Deceptive AP_WheelEncoder Strategy(APM CPR是INT16变量，最大不能超过32768)

$$VCPR=PPR\ast GR=1024\ast 30=30720$$
$$VDiameter=\frac{Diameter}{4}=\frac{75mm}{4}=18.75mm$$
$$VRadius=\frac{Diameter}{4\ast 2}=\frac{75mm}{4\ast 2}=9.375mm=0.009375m$$

注：V代表Virtual，虚拟的直接，虚拟的半径，虚拟的CPR值。

3. AP_WheelEncoder类

该应用类，重要的入口函数主要如下：

• init：初始化
• Log_Write：日志记录
• update：front-end数据更新

3.1 编码器种类

其实就两种模拟和QUADRATURE编码器

    // WheelEncoder driver types
    enum WheelEncoder_Type : uint8_t {
        WheelEncoder_TYPE_NONE             =   0,
        WheelEncoder_TYPE_QUADRATURE       =   1,
        WheelEncoder_TYPE_SITL_QUADRATURE  =  10,
    };

3.2 后台状态数据

    // The WheelEncoder_State structure is filled in by the backend driver
    struct WheelEncoder_State {
        uint8_t                instance;        // the instance number of this WheelEncoder
        int32_t                distance_count;  // cumulative number of forward + backwards events received from wheel encoder
        float                  distance;        // total distance measured in meters
        uint32_t               total_count;     // total number of successful readings from sensor (used for sensor quality calcs)
        uint32_t               error_count;     // total number of errors reading from sensor (used for sensor quality calcs)
        uint32_t               last_reading_ms; // time of last reading
        int32_t                dist_count_change; // distance count change during the last update (used to calculating rate)
        uint32_t               dt_ms;             // time change (in milliseconds) for the previous period (used to calculating rate)
    };

3.3. 重要应用方法

3.3.1 AP_WheelEncoder::init

• 默认最大支持2个编码器
• 启动流程源自setup流程
• 初始化就是对象实例化

#define WHEELENCODER_MAX_INSTANCES      2

AP_Vehicle::setup
 └──> Rover::init_ardupilot
     └──> AP_WheelEncoder::init


AP_WheelEncoder::init
 ├──> <num_instances != 0>
 │   └──> return // init called a 2nd time?
 └──> for (uint8_t i=0; i<WHEELENCODER_MAX_INSTANCES; i++)
     ├──> switch ((WheelEncoder_Type)_type[i].get()) {
     │   ├──> <WheelEncoder_TYPE_QUADRATURE><CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS>
     │   │   ├──> drivers[i] = new AP_WheelEncoder_Quadrature(*this, i, state[i]);
     │   │   └──> break;
     │   ├──> <WheelEncoder_TYPE_SITL_QUADRATURE><CONFIG_HAL_BOARD == HAL_BOARD_SITL>
     │   │   ├──> drivers[i] = new AP_WheelEncoder_SITL_Quadrature(*this, i, state[i]);
     │   │   └──> break;
     │   └──> <WheelEncoder_TYPE_NONE>
     │       └──> break;
     └──> <drivers[i] != nullptr>
         │  // we loaded a driver for this instance, so it must be
         │  // present (although it may not be healthy)
         └──> num_instances = i+1;  // num_instances is a high-water-mark

3.3.2 AP_WheelEncoder::update

从AP_WheelEncoder类角度，通过定时任务从后台将数据复制到前台应用。

SCHED_TASK(update_wheel_encoder,   50,    200,  36),
 └──> Rover::update_wheel_encoder
     └──> AP_WheelEncoder::update


// update WheelEncoder state for all instances. This should be called by main loop
AP_WheelEncoder::update
 └──> for (uint8_t i=0; i<num_instances; i++)
     └──> <drivers[i] != nullptr && _type[i] != WheelEncoder_TYPE_NONE>
         └──> drivers[i]->update();

3.3.3 AP_WheelEncoder::Log_Write

定期打包记录前台应用数据。

SCHED_TASK(update_logging2,        10,    200,  48),
 └──> Rover::update_logging2
     └──> AP_WheelEncoder::Log_Write


// log wheel encoder information
AP_WheelEncoder::Log_Write
 ├──> <!enabled(0) && !enabled(1)>
 │   └──> return; // return immediately if no wheel encoders are enabled
 │
 │  struct log_WheelEncoder pkt = {
 │      LOG_PACKET_HEADER_INIT(LOG_WHEELENCODER_MSG),
 │      time_us     : AP_HAL::micros64(),
 │      distance_0  : get_distance(0),
 │      quality_0   : (uint8_t)get_signal_quality(0),
 │      distance_1  : get_distance(1),
 │      quality_1   : (uint8_t)get_signal_quality(1),
 │  };
 └──> AP::logger().WriteBlock(&pkt, sizeof(pkt));

4. AP_WheelEncoder_Quadrature类

该应用类，重要的入口函数主要如下：

• update
• update_pin
• irq_handler
• update_phase_and_error_count
• pin_ab_to_phase

4.1 AP_WheelEncoder_Quadrature::update

整个更新过程就是一个后台驱动数据复制过程，该过程执行期间，中断被屏蔽。

• 在AP_WheelEncoder::update中被调用
• 定期更新pin脚定义
• 去使能中断；复制后台状态数据；使能中断

AP_WheelEncoder_Quadrature::update()
│
├── Call update_pin for last_pin_a
│   └── update_pin(last_pin_a, get_pin_a(), last_pin_a_value)
│       └── get_pin_a()
│
├── Call update_pin for last_pin_b
│   └── update_pin(last_pin_b, get_pin_b(), last_pin_b_value)
│       └── get_pin_b()
│
├── Disable interrupts to prevent race with irq_handler
│   └── void *irqstate = hal.scheduler->disable_interrupts_save()
│
├── Copy distance and error count to front end
│   └── copy_state_to_frontend(irq_state.distance_count,
│                              irq_state.total_count,
│                              irq_state.error_count,
│                              irq_state.last_reading_ms)
│
└── Restore interrupts
    └── hal.scheduler->restore_interrupts(irqstate)

4.2 AP_WheelEncoder_Quadrature::update_pin

• 引脚无变化则返回
• 引脚变化，首先将原来的中断钩子函数脱钩
• 挂上新引脚的中断钩子函数
• 读取当前IO引脚电平高低状态

AP_WheelEncoder_Quadrature::update_pin(uint8_t &pin, uint8_t new_pin, uint8_t &pin_value)
│
├── Check if new_pin is the same as current pin
│   └── if (new_pin == pin) { return; }
│
├── Remove old GPIO event callback if present
│   ├── if (pin != (uint8_t)-1 && !hal.gpio->detach_interrupt(pin))
│   │   ├── Send warning message: "WEnc: Failed to detach from pin %u"
│   │   └── // Ignore the failure
│   └── pin = new_pin
│
└── Install interrupt handler if new_pin is valid
    └── if (new_pin != (uint8_t)-1)
        ├── Set pin mode to input: hal.gpio->pinMode(pin, HAL_GPIO_INPUT)
        └── Attach interrupt handler
            ├── if (!hal.gpio->attach_interrupt(pin, FUNCTOR_BIND_MEMBER(&AP_WheelEncoder_Quadrature::irq_handler, void, uint8_t, bool, uint32_t), AP_HAL::GPIO::INTERRUPT_BOTH))
            │   └── Send warning message: "WEnc: Failed to attach to pin %u"
            └── Read initial pin value: pin_value = hal.gpio->read(pin)

4.3 AP_WheelEncoder_Quadrature::irq_handler

• 中断执行函数
• 保存当前状态、执行时间
• 更新当前状态

AP_WheelEncoder_Quadrature::irq_handler(uint8_t pin, bool pin_value, uint32_t timestamp)
│
├── Sanity Check: if (last_pin_a == 0 || last_pin_b == 0) { return; }
│
├── Update Distance and Error Counts:
│   │
│   ├── if (pin == last_pin_a)
│   │   └── last_pin_a_value = pin_value;
│   │
│   ├── else if (pin == last_pin_b)
│   │   └── last_pin_b_value = pin_value;
│   │
│   └── else { return; }
│
├── Call to update_phase_and_error_count()
│
└── Record Update Time:
    └── irq_state.last_reading_ms = timestamp * 1e-3f;

4.4 AP_WheelEncoder_Quadrature::update_phase_and_error_count

• 获取当前正交引脚状态
• 结合之前引脚状态判断转向
• 更新距离脉冲计数

AP_WheelEncoder_Quadrature::update_phase_and_error_count()
│
├── Convert pin state before and after to phases
│   └── uint8_t phase_after = pin_ab_to_phase(last_pin_a_value, last_pin_b_value)
│
├── Look for invalid changes
│   ├── Determine step forward and step back
│   │   ├── uint8_t step_forward = irq_state.phase < 3 ? irq_state.phase + 1 : 0
│   │   └── uint8_t step_back = irq_state.phase > 0 ? irq_state.phase - 1 : 3
│   │
│   ├── If phase_after is step_forward
│   │   ├── irq_state.phase = phase_after
│   │   └── irq_state.distance_count++
│   │
│   ├── Else if phase_after is step_back
│   │   ├── irq_state.phase = phase_after
│   │   └── irq_state.distance_count--
│   │
│   └── Else
│       └── irq_state.error_count++
│
└── Increment total count
    └── irq_state.total_count++

4.5 AP_WheelEncoder_Quadrature::pin_ab_to_phase

• 判断当前正交引脚传感信号电平
• 结合之前信号可以判断转向
  • 顺时针：1 ⇒ 0 ⇒ 3 ⇒ 2
  • 逆时针：3 ⇒ 0 ⇒ 1 ⇒ 2

AP_WheelEncoder_Quadrature::pin_ab_to_phase(bool pin_a, bool pin_b)
│
├── If pin_a is false
│   ├── If pin_b is false
│   │   └── return 0  // A = 0, B = 0
│   └── Else
│       └── return 1  // A = 0, B = 1
│
└── Else
    ├── If pin_b is false
    │   └── return 3  // A = 1, B = 0
    └── Else
        └── return 2  // A = 1, B = 1

Return default value (should be unreachable)
└── return (uint8_t)pin_a << 1 | (uint8_t)pin_b

5. AP_WheelEncoder_SITL_Quadrature类

TBD. //SITL仿真编码器，详见代码

6. 总结

结合Internal Errors 0x2000000 with WheelEncoder – ArduRover 4.5.2遇到的问题。

AP_WheelEncoder在Ardupilot开源代码之Rover上路计划的调测过程，主要存在以下问题：

• CPR参数值定义INT16，最大不能超过32768
• 对于高精度编码器的应用，会直接导致MCU占用率增加

本文针对AP_WheelEncoder进行代码级别的梳理和研读，同时也提出了对于高精度1024线编码器，高减速比应用的情况做了适当的兼容处理。使得APM代码在上述场景，依然可以使用。

注：同时，可以通过适当软缩减编码器精度，或者增加状态变量字节长度的方法，将上述应用类进行兼容性扩展。这样对于高精度要求的机器人应用上，APM将会更加适应其精度要求。

7. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码框架