赞
踩
鉴于ArduPilot开源飞控之AP_Baro中涉及Sensor Driver有以下总线类型:
ArduPilot之开源代码Sensor Drivers设计的front-end / back-end分层设计思路,AP_Baro主要描述的是front-end。
为了更好的从整体理解气压计这个传感器的嵌入式应用,这里深入到back-end驱动层,针对基于CAN协议的气压计设备,进行一个研读和理解。
AP_Baro_Backend
驱动层需实现方法:
注:通常来说使用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 frontend virtual 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 sensor virtual 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_ENABLED virtual void handle_msp(const MSP::msp_baro_data_message_t &pkt) {} #endif #if AP_BARO_EXTERNALAHRS_ENABLED virtual void handle_external(const AP_ExternalAHRS::baro_data_message_t &pkt) {} #endif /* device driver IDs. These are used to fill in the devtype field of the device ID, which shows up as BARO_DEVID* parameters to users. */ 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 object AP_Baro &_frontend; void _copy_to_frontend(uint8_t instance, float pressure, float temperature); // semaphore for access to shared frontend data HAL_Semaphore _sem; virtual void update_healthy_flag(uint8_t instance); // mean pressure for range filter float _mean_pressure; // number of dropped samples. Not used for now, but can be usable to choose more reliable sensor uint32_t _error_count; // set bus ID of this instance, for BARO_DEVID parameters void set_bus_id(uint8_t instance, uint32_t id) { _frontend.sensors[instance].bus_id.set(int32_t(id)); } };
由于气压数据来自CAN总线,因此,其逻辑与MSP协议类似,相对简单,没有校准等复杂物理公式。
实例初始化。
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;
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;
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");
处理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);
目前的官方文档有不一致的问题,希望后续能有完善和更新。
init_ardupilot
└──> AP_CANManager::init
└──> AP_DroneCAN::init
└──> AP_Baro_DroneCAN::subscribe_msgs
【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。