ArduPilot开源飞控之AP_Baro_DroneCAN

1. 源由

鉴于ArduPilot开源飞控之AP_Baro中涉及Sensor Driver有以下总线类型：

1. I2C
2. Serial UART
3. CAN
4. SITL //模拟传感器(暂时并列放在这里)

ArduPilot之开源代码Sensor Drivers设计的front-end / back-end分层设计思路，AP_Baro主要描述的是front-end。

为了更好的从整体理解气压计这个传感器的嵌入式应用，这里深入到back-end驱动层，针对基于CAN协议的气压计设备，进行一个研读和理解。

2. back-end抽象类

AP_Baro_Backend驱动层需实现方法：

• void update()
• static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev)

注：通常来说使用ChibiOS的都有定时器，如果没有定时器，可以使用void accumulate(void)来实现传感器的数据定时获取。

class AP_Baro_Backend
{
public:AP_Baro_Backend(AP_Baro &baro);virtual ~AP_Baro_Backend(void) {};// each driver must provide an update method to copy accumulated// data to the frontendvirtual void update() = 0;// accumulate function. This is used for backends that don't use a// timer, and need to be called regularly by the main code to// trigger them to read the sensorvirtual void accumulate(void) {}void backend_update(uint8_t instance);//  Check that the baro valid by using a mean filter.// If the value further that filtrer_range from mean value, it is rejected.bool pressure_ok(float press);uint32_t get_error_count() const { return _error_count; }#if AP_BARO_MSP_ENABLEDvirtual void handle_msp(const MSP::msp_baro_data_message_t &pkt) {}
#endif#if AP_BARO_EXTERNALAHRS_ENABLEDvirtual void handle_external(const AP_ExternalAHRS::baro_data_message_t &pkt) {}
#endif/*device driver IDs. These are used to fill in the devtype fieldof the device ID, which shows up as BARO_DEVID* parameters tousers.*/enum DevTypes {DEVTYPE_BARO_SITL     = 0x01,DEVTYPE_BARO_BMP085   = 0x02,DEVTYPE_BARO_BMP280   = 0x03,DEVTYPE_BARO_BMP388   = 0x04,DEVTYPE_BARO_DPS280   = 0x05,DEVTYPE_BARO_DPS310   = 0x06,DEVTYPE_BARO_FBM320   = 0x07,DEVTYPE_BARO_ICM20789 = 0x08,DEVTYPE_BARO_KELLERLD = 0x09,DEVTYPE_BARO_LPS2XH   = 0x0A,DEVTYPE_BARO_MS5611   = 0x0B,DEVTYPE_BARO_SPL06    = 0x0C,DEVTYPE_BARO_UAVCAN   = 0x0D,DEVTYPE_BARO_MSP      = 0x0E,DEVTYPE_BARO_ICP101XX = 0x0F,DEVTYPE_BARO_ICP201XX = 0x10,DEVTYPE_BARO_MS5607   = 0x11,DEVTYPE_BARO_MS5837   = 0x12,DEVTYPE_BARO_MS5637   = 0x13,DEVTYPE_BARO_BMP390   = 0x14,};protected:// reference to frontend objectAP_Baro &_frontend;void _copy_to_frontend(uint8_t instance, float pressure, float temperature);// semaphore for access to shared frontend dataHAL_Semaphore _sem;virtual void update_healthy_flag(uint8_t instance);// mean pressure for range filterfloat _mean_pressure; // number of dropped samples. Not used for now, but can be usable to choose more reliable sensoruint32_t _error_count;// set bus ID of this instance, for BARO_DEVID parametersvoid set_bus_id(uint8_t instance, uint32_t id) {_frontend.sensors[instance].bus_id.set(int32_t(id));}
};

3. 方法实现

由于气压数据来自CAN总线，因此，其逻辑与MSP协议类似，相对简单，没有校准等复杂物理公式。

3.1 probe

实例初始化。

AP_Baro_DroneCAN::probe├──> WITH_SEMAPHORE(_sem_registry);├──> AP_Baro_DroneCAN* backend = nullptr;│├──> <for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++)>│   └──> <_detected_modules[i].driver == nullptr && _detected_modules[i].ap_dronecan != nullptr>│       ││       │    /********************************************************************************│       │     * Registered DroneCAN Baro Node                                                *│       │     ********************************************************************************/│       ├──> backend = new AP_Baro_DroneCAN(baro);│       ├──> <backend == nullptr>│       │   └──> AP::can().log_text(AP_CANManager::LOG_ERROR,LOG_TAG,│       │                  "Failed register DroneCAN Baro Node %d on Bus %d\n",│       │                  _detected_modules[i].node_id,│       │                  _detected_modules[i].ap_dronecan->get_driver_index());│       ├──> <else>│       │   ├──> _detected_modules[i].driver = backend;│       │   ├──> backend->_pressure = 0;│       │   ├──> backend->_pressure_count = 0;│       │   ├──> backend->_ap_dronecan = _detected_modules[i].ap_dronecan;│       │   ├──> backend->_node_id = _detected_modules[i].node_id;│       │   ├──> backend->_instance = backend->_frontend.register_sensor();│       │   ├──> backend->set_bus_id(backend->_instance, AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN,│       │   │                                                                      _detected_modules[i].ap_dronecan->get_driver_index(),│       │   │                                                                      backend->_node_id, 0));│       │   └──> AP::can().log_text(AP_CANManager::LOG_INFO,LOG_TAG,│       │                  "Registered DroneCAN Baro Node %d on Bus %d\n",│       │                  _detected_modules[i].node_id,│       │                  _detected_modules[i].ap_dronecan->get_driver_index());│       └──> break;└──> return backend;

3.2 update

front-end / back-end数据更新。

AP_Baro_DroneCAN::update├──> float pressure = 0;├──> WITH_SEMAPHORE(_sem_baro);└──> <new_pressure>├──> <_pressure_count != 0>│   ├──> pressure = _pressure / _pressure_count;│   ├──>_pressure_count = 0;│   └──>_pressure = 0;├──> _copy_to_frontend(_instance, pressure, _temperature);├──> _frontend.set_external_temperature(_temperature);└──> new_pressure = false;

3.3 subscribe_msgs

CAN协议消息处理钩子。

AP_Baro_DroneCAN::subscribe_msgs├──> ap_dronecan == nullptr>│   └──> return;├──> <Canard::allocate_sub_arg_callback(ap_dronecan, &handle_pressure, ap_dronecan->get_driver_index()) == nullptr>│   └──> AP_BoardConfig::allocation_error("pressure_sub");└──> <Canard::allocate_sub_arg_callback(ap_dronecan, &handle_temperature, ap_dronecan->get_driver_index()) == nullptr>└──> AP_BoardConfig::allocation_error("temperature_sub");

3.4 handle_pressure/handle_temperature

处理CAN协议中气压数据。

AP_Baro_DroneCAN::handle_pressure├──> WITH_SEMAPHORE(_sem_registry);├──> driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, true);├──> <driver == nullptr>│   └──> return;└──> WITH_SEMAPHORE(driver->_sem_baro);├──> _update_and_wrap_accumulator(&driver->_pressure, msg.static_pressure, &driver->_pressure_count, 32);└──> driver->new_pressure = true;

处理CAN协议中温度数据。

AP_Baro_DroneCAN::handle_temperature├──> WITH_SEMAPHORE(_sem_registry);├──> driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, false);├──> <driver == nullptr>│   └──> return;└──> WITH_SEMAPHORE(driver->_sem_baro);└──> driver->_temperature = KELVIN_TO_C(msg.static_temperature);

3.5 CAN port

目前的官方文档有不一致的问题，希望后续能有完善和更新。

• 【1】Inconsistent CAN startup documentation
• 【2】Inconsistent CAN startup documentation #5475

init_ardupilot└──> AP_CANManager::init└──> AP_DroneCAN::init└──> AP_Baro_DroneCAN::subscribe_msgs

4. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计