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 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 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));
    }
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79

3. 方法实现

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

3.1 AP_Baro_SITL

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

AP_Baro_SITL::AP_Baro_SITL
 └──> <_sitl != nullptr>
    ├──> _instance = _frontend.register_sensor();
    ├──> 
    │   └──> _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));
1
2
3
4
5
6
7
8
9
10
11

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
 ├──> = 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
 ├──> 
 │   │  // 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
 │   └──> 
 │       ├──> best_index = i;
 │       └──> best_time_delta = time_delta;
 │
 ├──>   // only output stored state if < 200 msec retrieval error
 │   └──> sim_alt = _buffer[best_index].data;
 │
 │  /********************************************************************************
 │   * Temperature adjust                                                           *
 │   ********************************************************************************/
 ├──> 
 │   ├──> 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);
 ├──> 
 │   ├──> 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;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81

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;
 ├──>  // 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
 └──> 
     │  // 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);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

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
 ├──> 
 │   └──> error += bp.wcof_xp * sqx;
 ├──> < else >
 │   └──> error += bp.wcof_xn * sqx;
 │
 │  // error for y
 ├──> 
 │   └──> error += bp.wcof_yp * sqy;
 ├──> < else >
 │   └──> error += bp.wcof_yn * sqy;
 │
 │  // error for 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();
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

3.5 update

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

AP_Baro_SITL::update
 ├──> 
 │   └──> return;
 ├──> WITH_SEMAPHORE(_sem);
 ├──> _copy_to_frontend(_instance, _recent_press, _recent_temp);
 └──> _has_sample = false;
1
2
3
4
5
6

4. 参考资料

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