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.count = 0;
state[i].cal.sum = 0; state[i].cal.sum = 0;
state[i].cal.read_count = 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); GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Airspeed %u calibration started", i+1);
} }
#endif // HAL_BUILD_AP_PERIPH #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) { state[i].cal.read_count > 15) {
if (state[i].cal.count == 0) { if (state[i].cal.count == 0) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Airspeed %u unhealthy", i + 1); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Airspeed %u unhealthy", i + 1);
calibration_state[i] = CalibrationState::FAILED;
} else { } else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Airspeed %u calibrated", i + 1); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Airspeed %u calibrated", i + 1);
float calibrated_offset = state[i].cal.sum / state[i].cal.count; 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); param[i].offset.set_and_save(calibrated_offset);
calibration_state[i] = CalibrationState::SUCCESS;
} }
state[i].cal.start_ms = 0; state[i].cal.start_ms = 0;
return; return;
@ -570,6 +573,23 @@ void AP_Airspeed::update_calibration(uint8_t i, float raw_pressure)
#endif // HAL_BUILD_AP_PERIPH #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 // read one airspeed sensor
void AP_Airspeed::read(uint8_t i) 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 #if AP_AIRSPEED_MSP_ENABLED
void handle_msp(const MSP::msp_airspeed_data_message_t &pkt); void handle_msp(const MSP::msp_airspeed_data_message_t &pkt);
#endif #endif
enum class CalibrationState {
NOT_STARTED,
IN_PROGRESS,
SUCCESS,
FAILED
};
// get aggregate calibration state for the Airspeed library:
CalibrationState get_calibration_state() const;
private: private:
static AP_Airspeed *_singleton; static AP_Airspeed *_singleton;
@ -217,6 +226,8 @@ private:
AP_Airspeed_Params param[AIRSPEED_MAX_SENSORS]; AP_Airspeed_Params param[AIRSPEED_MAX_SENSORS];
CalibrationState calibration_state[AIRSPEED_MAX_SENSORS];
struct airspeed_state { struct airspeed_state {
float raw_airspeed; float raw_airspeed;
float airspeed; float airspeed;