网站服务器名字,广东深圳宝安区,做网页的软件html,大连金州开发区ArduPilot开源飞控之AP_Baro_SITL 1. 源由2. back-end抽象类3. 方法实现3.1 AP_Baro_SITL3.2 _timer3.3 temperature_adjustment3.4 wind_pressure_correction3.5 update 4. 参考资料 1. 源由
鉴于ArduPilot开源飞控之AP_Baro中涉及Sensor Driver有以下总线类型#xff1a;
… ArduPilot开源飞控之AP_Baro_SITL 1. 源由2. back-end抽象类3. 方法实现3.1 AP_Baro_SITL3.2 _timer3.3 temperature_adjustment3.4 wind_pressure_correction3.5 update 4. 参考资料 1. 源由
鉴于ArduPilot开源飞控之AP_Baro中涉及Sensor Driver有以下总线类型
I2CSerial UARTCANSITL //模拟传感器(暂时并列放在这里)
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::OwnPtrAP_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 dont 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之开源代码LibrarySketches设计 【5】ArduPilot之开源代码Sensor Drivers设计
