Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: get SITL to work with multiple baros

Browse Source 
baro readings are independent of each other
and can be configured to behave differently
This commit is contained in:
Harshit Kumar Sankhla 2020-06-30 12:33:24 +05:30 committed by Andrew Tridgell
parent 4115603f13
commit 27fc7dcf0a
3 changed files with 16 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_Baro/AP_Baro.cpp
View File

@ -573,10 +573,7 @@ void AP_Baro::init(void)
if (sitl == nullptr) { if (sitl == nullptr) {
AP_HAL::panic("No SITL pointer"); AP_HAL::panic("No SITL pointer");
} }
if (sitl->baro_count > 1) { for(uint8_t i = 0; i < sitl->baro_count; i++) {
::fprintf(stderr, "More than one baro not supported. Sorry.");
}
if (sitl->baro_count == 1) {
ADD_BACKEND(new AP_Baro_SITL(*this)); ADD_BACKEND(new AP_Baro_SITL(*this));
} }
#elif HAL_BARO_DEFAULT == HAL_BARO_HIL #elif HAL_BARO_DEFAULT == HAL_BARO_HIL

14
libraries/AP_Baro/AP_Baro_SITL.cpp
View File

@ -54,16 +54,16 @@ void AP_Baro_SITL::_timer()
float sim_alt = _sitl->state.altitude; float sim_alt = _sitl->state.altitude;
if (_sitl->baro_disable) { if (_sitl->baro_disable[_instance]) {
// barometer is disabled // barometer is disabled
return; return;
} }
sim_alt += _sitl->baro_drift * now / 1000.0f; sim_alt += _sitl->baro_drift[_instance] * now / 1000.0f;
sim_alt += _sitl->baro_noise * rand_float(); sim_alt += _sitl->baro_noise[_instance] * rand_float();
// add baro glitch // add baro glitch
sim_alt += _sitl->baro_glitch; sim_alt += _sitl->baro_glitch[_instance];
// add delay // add delay
uint32_t best_time_delta = 200; // initialise large time representing buffer entry closest to current time - delay. uint32_t best_time_delta = 200; // initialise large time representing buffer entry closest to current time - delay.
@ -118,6 +118,12 @@ void AP_Baro_SITL::_timer()
_has_sample = true; _has_sample = true;
} }
// unhealthy if baro is turned off or beyond supported instances
bool AP_Baro_SITL::healthy(uint8_t instance)
{
return !_sitl->baro_disable[instance];
}
// Read the sensor // Read the sensor
void AP_Baro_SITL::update(void) void AP_Baro_SITL::update(void)
{ {

5
libraries/AP_Baro/AP_Baro_SITL.h
View File

@ -14,7 +14,7 @@ public:
protected: protected:
void update_healthy_flag(uint8_t instance) override { _frontend.sensors[instance].healthy = true; } void update_healthy_flag(uint8_t instance) override { _frontend.sensors[instance].healthy = healthy(instance); };
private: private:
uint8_t _instance; uint8_t _instance;
@ -33,6 +33,9 @@ private:
// adjust for simulated board temperature // adjust for simulated board temperature
void temperature_adjustment(float &p, float &T); void temperature_adjustment(float &p, float &T);
// is the barometer usable for flight
bool healthy(uint8_t instance);
void _timer(); void _timer();
bool _has_sample; bool _has_sample;
uint32_t _last_sample_time; uint32_t _last_sample_time;