AP_Baro: emit calibrated messages for each connected baro

Thanks to OlliW for pointing this out
This commit is contained in:
Peter Barker 2018-10-11 07:38:53 +11:00 committed by Peter Barker
parent d94663d5af
commit e503df48ff
1 changed files with 6 additions and 2 deletions

View File

@ -244,12 +244,16 @@ void AP_Baro::calibrate(bool save)
_guessed_ground_temperature = get_external_temperature(); _guessed_ground_temperature = get_external_temperature();
// panic if all sensors are not calibrated // panic if all sensors are not calibrated
uint8_t num_calibrated = 0;
for (uint8_t i=0; i<_num_sensors; i++) { for (uint8_t i=0; i<_num_sensors; i++) {
if (sensors[i].calibrated) { if (sensors[i].calibrated) {
gcs().send_text(MAV_SEVERITY_INFO, "Barometer calibration complete"); gcs().send_text(MAV_SEVERITY_INFO, "Barometer %u calibration complete", i+1);
return; num_calibrated++;
} }
} }
if (num_calibrated) {
return;
}
AP_HAL::panic("AP_Baro: all sensors uncalibrated"); AP_HAL::panic("AP_Baro: all sensors uncalibrated");
} }