Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Airspeed: add support for mavlink in-progress message

This commit is contained in:
Peter Barker 2022-12-08 17:47:13 +11:00 committed by Peter Barker
parent b9d718c938
commit d7ec1e7b0c
2 changed files with 32 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_Airspeed/AP_Airspeed.cpp
View File

@ -523,6 +523,7 @@ void AP_Airspeed::calibrate(bool in_startup)
state[i].cal.count = 0;
state[i].cal.sum = 0;
state[i].cal.read_count = 0;
calibration_state[i] = CalibrationState::IN_PROGRESS;
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Airspeed %u calibration started", i+1);
}
#endif // HAL_BUILD_AP_PERIPH
@ -544,6 +545,7 @@ void AP_Airspeed::update_calibration(uint8_t i, float raw_pressure)
state[i].cal.read_count > 15) {
if (state[i].cal.count == 0) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Airspeed %u unhealthy", i + 1);
calibration_state[i] = CalibrationState::FAILED;
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Airspeed %u calibrated", i + 1);
float calibrated_offset = state[i].cal.sum / state[i].cal.count;
@ -557,6 +559,7 @@ void AP_Airspeed::update_calibration(uint8_t i, float raw_pressure)
}
}
param[i].offset.set_and_save(calibrated_offset);
calibration_state[i] = CalibrationState::SUCCESS;
}
state[i].cal.start_ms = 0;
return;
@ -570,6 +573,23 @@ void AP_Airspeed::update_calibration(uint8_t i, float raw_pressure)
#endif // HAL_BUILD_AP_PERIPH
}
// get aggregate calibration state for the Airspeed library:
AP_Airspeed::CalibrationState AP_Airspeed::get_calibration_state() const
{
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
switch (calibration_state[i]) {
case CalibrationState::SUCCESS:
case CalibrationState::NOT_STARTED:
continue;
case CalibrationState::IN_PROGRESS:
return CalibrationState::IN_PROGRESS;
case CalibrationState::FAILED:
return CalibrationState::FAILED;
}
}
return CalibrationState::SUCCESS;
}
// read one airspeed sensor
void AP_Airspeed::read(uint8_t i)
{

13
libraries/AP_Airspeed/AP_Airspeed.h
View File

@ -201,7 +201,16 @@ public:
#if AP_AIRSPEED_MSP_ENABLED
void handle_msp(const MSP::msp_airspeed_data_message_t &pkt);
#endif
enum class CalibrationState {
NOT_STARTED,
IN_PROGRESS,
SUCCESS,
FAILED
};
// get aggregate calibration state for the Airspeed library:
CalibrationState get_calibration_state() const;
private:
static AP_Airspeed *_singleton;
@ -217,6 +226,8 @@ private:
AP_Airspeed_Params param[AIRSPEED_MAX_SENSORS];
CalibrationState calibration_state[AIRSPEED_MAX_SENSORS];
struct airspeed_state {
float raw_airspeed;
float airspeed;