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

AP_Baro: only allow calibrated sensors to be used

Browse Source
This commit is contained in:
Andrew Tridgell 2015-01-09 12:59:01 +11:00
parent e8b1fc72e0
commit b1d8df3d54
2 changed files with 29 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_Baro/AP_Baro.cpp
View File

@ -78,6 +78,12 @@ void AP_Baro::calibrate()
// offset is supposed to be for within a flight
_alt_offset.set_and_save(0);
// start by assuming all sensors are calibrated (for healthy() test)
for (uint8_t i=0; i<_num_sensors; i++) {
sensors[i].calibrated = true;
sensors[i].alt_ok = true;
}
// let the barometer settle for a full second after startup
// the MS5611 reads quite a long way off for the first second,
// leading to about 1m of error if we don't wait
@ -97,34 +103,43 @@ void AP_Baro::calibrate()
// temperature settings
float sum_pressure[BARO_MAX_INSTANCES] = {0};
float sum_temperature[BARO_MAX_INSTANCES] = {0};
uint8_t count[BARO_MAX_INSTANCES] = {0};
const uint8_t num_samples = 5;
for (uint8_t c = 0; c < num_samples; c++) {
uint32_t tstart = hal.scheduler->millis();
bool all_sensors_healthy = false;
do {
update();
if (hal.scheduler->millis() - tstart > 500) {
hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful "
"for more than 500ms in AP_Baro::calibrate [3]\r\n"));
}
all_sensors_healthy = true;
for (uint8_t i=0; i<_num_sensors; i++) {
if (!healthy(i)) {
all_sensors_healthy = false;
}
}
} while (!all_sensors_healthy);
} while (!healthy());
for (uint8_t i=0; i<_num_sensors; i++) {
sum_pressure[i] += sensors[i].pressure;
sum_temperature[i] += sensors[i].temperature;
if (healthy(i)) {
sum_pressure[i] += sensors[i].pressure;
sum_temperature[i] += sensors[i].temperature;
count[i] += 1;
}
}
hal.scheduler->delay(100);
}
for (uint8_t i=0; i<_num_sensors; i++) {
sensors[i].ground_pressure.set_and_save(sum_pressure[i] / num_samples);
sensors[i].ground_temperature.set_and_save(sum_temperature[i] / num_samples);
if (count[i] == 0) {
sensors[i].calibrated = false;
} else {
sensors[i].ground_pressure.set_and_save(sum_pressure[i] / count[i]);
sensors[i].ground_temperature.set_and_save(sum_temperature[i] / count[i]);
}
}
// panic if all sensors are not calibrated
for (uint8_t i=0; i<_num_sensors; i++) {
if (sensors[i].calibrated) {
return;
}
}
hal.scheduler->panic(PSTR("AP_Baro: all sensors uncalibrated"));
}
/*

3
libraries/AP_Baro/AP_Baro.h
View File

@ -42,7 +42,7 @@ public:
// healthy - returns true if sensor and derived altitude are good
bool healthy(void) const { return healthy(_primary); }
bool healthy(uint8_t instance) const { return sensors[instance].healthy && sensors[instance].alt_ok; }
bool healthy(uint8_t instance) const { return sensors[instance].healthy && sensors[instance].alt_ok && sensors[instance].calibrated; }
// check if all baros are healthy - used for SYS_STATUS report
bool all_healthy(void) const;
@ -136,6 +136,7 @@ private:
uint32_t last_update_ms; // last update time in ms
bool healthy:1; // true if sensor is healthy
bool alt_ok:1; // true if calculated altitude is ok
bool calibrated:1; // true if calculated calibrated successfully
float pressure; // pressure in Pascal
float temperature; // temperature in degrees C
float altitude; // calculated altitude