ArduPilot开源代码之AP_OpticalFlow_UPFLOW

1. 源由

AP_OpticalFlow_UPFLOW也是一种光流计，与前面AP_OpticalFlow_CXOF类似。
而且其波特率也是19200 bps，采取同样的pulling方式。唯一的差异就是两者帧格式略有差异。
2. Library计划

装备相干的紧张函数：

• init
  
• update
  
• detect
  

1. class AP_OpticalFlow_UPFLOW : public OpticalFlow_backend
2. {
3. public:
4.     /// 构造函数
5.     AP_OpticalFlow_UPFLOW(AP_OpticalFlow &_frontend, AP_HAL::UARTDriver *uart);
7.     // 初始化传感器
8.     void init() override;
10.     // 从传感器读取最新的值并填充 x, y 和总量
11.     void update(void) override;
13.     // 检测传感器是否可用
14.     static AP_OpticalFlow_UPFLOW *detect(AP_OpticalFlow &_frontend);
16. private:
17.     // 数据结构：光流数据
18.     struct PACKED UpixelsOpticalFlow {
19.         int16_t flow_x_integral;        // 单位：10^-4 弧度，乘以 10^-4 得到弧度值
20.         int16_t flow_y_integral;        // 单位：10^-4 弧度，乘以 10^-4 得到弧度值
21.         uint16_t integration_timespan;  // 时间间隔，单位：微秒
22.         uint16_t ground_distance;       // 保留字段，总是为 999
23.         uint8_t quality;                // 数据质量，0 表示无效，245 表示有效
24.         uint8_t version;                // 版本号
25.     };
27.     AP_HAL::UARTDriver *uart;               // 连接到光流传感器的 UART
28.     struct UpixelsOpticalFlow updata;       // 接收的数据结构
29.     uint16_t recv_count;                    // 接收到的字节数
30.     uint8_t sum;                            // 校验和
31.     Vector2f gyro_sum;                      // 自上次从光流传感器接收到数据以来陀螺仪传感器值的总和
32.     uint16_t gyro_sum_count;                // 陀螺仪传感器值的数量
33. };

复制代码

3. 紧张例程

3.1 AP_OpticalFlow_UPFLOW::init

模块默认波特率19200 bps。

1. // initialise the sensor
2. void AP_OpticalFlow_UPFLOW::init()
3. {
4.     // sanity check uart
5.     if (uart == nullptr) {
6.         return;
7.     }
8.     // open serial port with baud rate of 19200
9.     uart->begin(19200);
10. }

复制代码

3.2 AP_OpticalFlow_UPFLOW::update

串口数据采取pull的方式读取，相对来说更占用CPU资源。
[AP_OpticalFlow_MSP](https://blog.csdn.net/lida2003/article/details/140328944)采取Interrupt方式比力节流资源，而且Matek光流计采取了115200 bps的波特率，速率更快，暂用CPU的时间更短。

1. update()
2. ├── 检查UART是否有效
3. │   └── if (uart == nullptr)
4. │       └── return
5. ├── 记录陀螺仪数值
6. │   └── if (gyro_sum_count >= 1000)
7. │       ├── gyro_sum.zero()  // 将陀螺仪累积值清零
8. │       └── gyro_sum_count = 0  // 重置累积计数
9. │   └── const Vector3f& gyro = AP::ahrs().get_gyro()  // 获取陀螺仪数据
10. │   └── gyro_sum.x += gyro.x  // 累积陀螺仪x轴数据
11. │   └── gyro_sum.y += gyro.y  // 累积陀螺仪y轴数据
12. │   └── gyro_sum_count++  // 累积计数加一
13. ├── 读取串口数据
14. │   └── uint32_t nbytes = MIN(uart->available(), 1024u)  // 获取可用数据字节数
15. │   └── while (nbytes-- > 0)  // 循环读取每个字节数据
16. │       └── int16_t r = uart->read()  // 读取一个字节数据
17. │       └── if (r < 0)
18. │           └── break
19. │       └── uint8_t c = (uint8_t)r  // 转换为无符号字节
20. │       └── if (recv_count == 0)
21. │           └── if (c == UPFLOW_HEADER0)
22. │               └── recv_count++  // 接收计数加一
23. │               └── sum = 0  // 校验和清零
24. │       └── else if (recv_count == 1)
25. │           └── if (c != UPFLOW_HEADER1)
26. │               └── recv_count = 0  // 重置接收计数
27. │           └── else
28. │               └── recv_count++  // 接收计数加一
29. │       └── else if (recv_count < 12)
30. │           └── ((uint8_t*)&updata)[recv_count - 2] = c  // 存储接收到的数据
31. │           └── sum ^= c  // 更新校验和
32. │           └── recv_count++  // 接收计数加一
33. │       └── else if (recv_count == 12)
34. │           └── if (sum != c)
35. │               └── recv_count = 0  // 校验和错误，重置接收计数
36. │           └── else
37. │               └── recv_count++  // 接收计数加一
38. │       └── else
39. │           └── if (c == UPFLOW_FOOTER)
40. │               └── phrased = true  // 标记解析完成
41. │           └── recv_count = 0  // 重置接收计数
42. ├── 如果没有有效数据，则返回
43. │   └── if (phrased == false)
44. │       └── return
45. ├── 处理接收到的数据
46. │   └── struct AP_OpticalFlow::OpticalFlow_state state {}  // 定义光流状态结构体
47. │   └── state.surface_quality = updata.quality  // 设置表面质量
48. │   └── float dt = updata.integration_timespan * 1.0e-6  // 计算时间间隔
49. │   └── if (is_positive(dt) && (dt < UPFLOW_TIMEOUT_SEC))  // 如果时间间隔有效
50. │       ├── 计算流速数值
51. │       ├── const Vector2f flowScaler = _flowScaler()  // 获取流速缩放器
52. │       ├── Vector2f flowScaleFactor = Vector2f(1.0f, 1.0f) + flowScaler * 0.001f  // 计算流速缩放因子
53. │       ├── state.flowRate = Vector2f(-updata.flow_x_integral * flowScaleFactor.x, -updata.flow_y_integral * flowScaleFactor.y)  // 计算流速
54. │       ├── state.flowRate *= UPFLOW_PIXEL_SCALING / dt  // 根据时间间隔调整流速
55. │       ├── state.bodyRate = Vector2f(gyro_sum.x / gyro_sum_count, gyro_sum.y / gyro_sum_count)  // 计算体速度
56. │       └── _applyYaw(state.flowRate)  // 应用偏航角修正流速
57. │   └── else
58. │       └── state.flowRate.zero()  // 时间间隔无效时流速置零
59. │       └── state.bodyRate.zero()  // 时间间隔无效时体速度置零
60. └── 更新前端状态
61.     └── _update_frontend(state)  // 更新前端状态信息
62. └── 重置陀螺仪累积值
63.     └── gyro_sum.zero()  // 陀螺仪累积值清零
64.     └── gyro_sum_count = 0  // 重置陀螺仪累积计数

复制代码

3.3 AP_OpticalFlow_UPFLOW::detect

AP_OpticalFlow初始化的内容直接放到了detect函数实现。

1. // detect the device
2. AP_OpticalFlow_UPFLOW *AP_OpticalFlow_UPFLOW::detect(AP_OpticalFlow &_frontend)
3. {
4.     AP_SerialManager *serial_manager = AP::serialmanager().get_singleton();
5.     if (serial_manager == nullptr) {
6.         return nullptr;
7.     }
9.     // look for first serial driver with protocol defined as OpticalFlow
10.     AP_HAL::UARTDriver *uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_OpticalFlow, 0);
11.     if (uart == nullptr) {
12.         return nullptr;
13.     }
15.     // we have found a serial port so use it
16.     AP_OpticalFlow_UPFLOW *sensor = new AP_OpticalFlow_UPFLOW(_frontend, uart);
17.     return sensor;
18. }

复制代码

4. 总结

UPFLOW和CXOF类似，从协议利用的方式来看，服从偏低。
5. 参考资料

【1】ArduPilot开源飞控体系之简朴先容
【2】ArduPilot之开源代码Task先容
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches计划
【5】ArduPilot之开源代码Sensor Drivers计划
【6】ArduPilot开源飞控之AP_OpticalFlow

免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！qidao123.com:ToB企服之家，中国第一个企服评测及软件市场,开放入驻,技术点评得现金