ArduPilot开源飞控之AP_Baro_SITL

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。

为了AP_Baro代码研读的完整性，就继续简单的整理下下针对AP_Baro_SITL研读和理解。

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. 方法实现

AP_Baro_SITL是一个模拟器件，其气压数据来源于模拟系统，对于模拟系统这里不展开，其传递参量的主要方式是全局变量_sitl->state.altitude。

3.1 AP_Baro_SITL

实例初始化，注册一个定时回调函数。

AP_Baro_SITL::AP_Baro_SITL└──> <_sitl != nullptr>├──> _instance = _frontend.register_sensor();├──> <APM_BUILD_TYPE(APM_BUILD_ArduSub)>│   └──> _frontend.set_type(_instance, AP_Baro::BARO_TYPE_WATER);├──> set_bus_id(_instance, AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, 0, _instance, DEVTYPE_BARO_SITL));││  /********************************************************************************│   * start periodic call back                                                     *│   ********************************************************************************/└──> hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_Baro_SITL::_timer, void));

3.2 _timer

定时模拟高度数据（这里不涉及温度的校准，但是做了一些模拟的噪音，比如：baro glitch/drift/noise/temperature/wind）。

AP_Baro_SITL::_timer│  /********************************************************************************│   * 100Hz                                                                        *│   ********************************************************************************/├──> const uint32_t now = AP_HAL::millis();├──> <(now - _last_sample_time) < 10>│   └──> return;│├──> _last_sample_time = now;├──> float sim_alt = _sitl->state.altitude;├──> <_sitl->baro[_instance].disable>│   └──> return;  // barometer is disabled││  /********************************************************************************│   * Update simulated altitude                                                    *│   ********************************************************************************/│  // Noise for simulated altitude├──> sim_alt += _sitl->baro[_instance].drift * now * 0.001f;├──> sim_alt += _sitl->baro[_instance].noise * rand_float();││  // add baro glitch├──> sim_alt += _sitl->baro[_instance].glitch;││  // add delay├──> uint32_t best_time_delta = 200;  // initialise large time representing buffer entry closest to current time - delay.├──> uint8_t best_index = 0;  // initialise number representing the index of the entry in buffer closest to delay.││  // storing data from sensor to buffer├──> <now - _last_store_time >= 10>  // store data every 10 ms.│   ├──> _last_store_time = now;│   ├──> <_store_index > _buffer_length - 1>  │   │   └──> _store_index = 0;  // reset buffer index if index greater than size of buffer│   ││   │  // if freezed barometer, report altitude to last recorded altitude│   ├──> <_sitl->baro[_instance].freeze == 1>│   │   └──> sim_alt = _last_altitude;│   ├──> < else >│   │   └──> _last_altitude = sim_alt;│   ││   ├──> _buffer[_store_index].data = sim_alt;  // add data to current index│   ├──> _buffer[_store_index].time = _last_store_time;  // add time_stamp to current index│   └──> _store_index = _store_index + 1;  // increment index││  // return delayed measurement├──> const uint32_t delayed_time = now - _sitl->baro[_instance].delay;  // get time corresponding to delay││  // find data corresponding to delayed time in buffer├──> <for (uint8_t i = 0; i <= _buffer_length - 1; i++)>│   │  // find difference between delayed time and time stamp in buffer│   ├──> uint32_t time_delta = abs((int32_t)(delayed_time - _buffer[i].time));│   │  // if this difference is smaller than last delta, store this time│   └──> <time_delta < best_time_delta>│       ├──> best_index = i;│       └──> best_time_delta = time_delta;│├──> <best_time_delta < 200>  // only output stored state if < 200 msec retrieval error│   └──> sim_alt = _buffer[best_index].data;││  /********************************************************************************│   * Temperature adjust                                                           *│   ********************************************************************************/├──> <!APM_BUILD_TYPE(APM_BUILD_ArduSub)>│   ├──> float sigma, delta, theta;│   ├──> AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta);│   ├──> float p = SSL_AIR_PRESSURE * delta;│   ├──> float T = KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta);│   └──> temperature_adjustment(p, T);├──> <else>│   ├──> float rho, delta, theta;│   ├──> AP_Baro::SimpleUnderWaterAtmosphere(-sim_alt * 0.001f, rho, delta, theta);│   ├──> float p = SSL_AIR_PRESSURE * delta;│   └──> float T = KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta);││  /********************************************************************************│   * add in correction for wind effects                                           *│   ********************************************************************************/├──> p += wind_pressure_correction(_instance);│├──> _recent_press = p;├──> _recent_temp = T;└──> _has_sample = true;

3.3 temperature_adjustment

温度模拟修正。

AP_Baro_SITL::temperature_adjustment├──> const float tsec = AP_HAL::millis() * 0.001f;├──> const float T_sensor = T + AP::sitl()->temp_board_offset;├──> const float tconst = AP::sitl()->temp_tconst;├──> <tsec < 23 * tconst> // time which past the equation below equals T_sensor within approx. 1E-9│   ├──> const float T0 = AP::sitl()->temp_start;│   └──> T = T_sensor - (T_sensor - T0) * expf(-tsec / tconst);├──> < else >│   └──> T = T_sensor;├──> const float baro_factor = AP::sitl()->temp_baro_factor;├──> const float Tzero = 30.0f;  // start baro adjustment at 30C└──> <is_positive(baro_factor)>│  // this produces a pressure change with temperature that│  // closely matches what has been observed with a ICM-20789│  // barometer. A typical factor is 1.2.└──> p -= powf(MAX(T - Tzero, 0), baro_factor);

3.4 wind_pressure_correction

风力压强修正。

AP_Baro_SITL::wind_pressure_correction├──> const auto &bp = AP::sitl()->baro[instance];││  // correct for static pressure position errors├──> const Vector3f &airspeed_vec_bf = AP::sitl()->state.velocity_air_bf;│├──> float error = 0.0;├──> const float sqx = sq(airspeed_vec_bf.x);├──> const float sqy = sq(airspeed_vec_bf.y);├──> const float sqz = sq(airspeed_vec_bf.z);││  // error for x├──> <is_positive(airspeed_vec_bf.x)>│   └──> error += bp.wcof_xp * sqx;├──> < else >│   └──> error += bp.wcof_xn * sqx;││  // error for y├──> <is_positive(airspeed_vec_bf.y)>│   └──> error += bp.wcof_yp * sqy;├──> < else >│   └──> error += bp.wcof_yn * sqy;││  // error for z├──> <is_positive(airspeed_vec_bf.z)>│   └──> error += bp.wcof_zp * sqz;├──> < else >│   └──> error += bp.wcof_zn * sqz;│└──> return error * 0.5 * SSL_AIR_DENSITY * AP::baro().get_air_density_ratio();

3.5 update

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

AP_Baro_SITL::update├──> <!_has_sample>│   └──> return;├──> WITH_SEMAPHORE(_sem);├──> _copy_to_frontend(_instance, _recent_press, _recent_temp);└──> _has_sample = false;

4. 参考资料

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