diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 5402bdcc1c..69612016c4 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -418,6 +418,17 @@ bool AP_InertialSensor::get_gyro_health_all(void) const return (get_gyro_count() > 0); } +// gyro_calibration_ok_all - returns true if all gyros were calibrated successfully +bool AP_InertialSensor::gyro_calibrated_ok_all() const +{ + for (uint8_t i=0; i 0); +} + // get_accel_health_all - return true if all accels are healthy bool AP_InertialSensor::get_accel_health_all(void) const { @@ -550,6 +561,7 @@ AP_InertialSensor::_init_gyro() best_diff[k] = 0; last_average[k].zero(); converged[k] = false; + _gyro_cal_ok[k] = true; // default calibration ok flag to true } for(int8_t c = 0; c < 5; c++) { @@ -625,6 +637,8 @@ AP_InertialSensor::_init_gyro() hal.console->printf_P(PSTR("gyro[%u] did not converge: diff=%f dps\n"), (unsigned)k, ToDeg(best_diff[k])); _gyro_offset[k] = best_avg[k]; + // flag calibration as failed for this gyro + _gyro_cal_ok[k] = false; } } } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index f4bb5a7295..6b14c8f4b5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -120,6 +120,8 @@ public: bool get_gyro_health(void) const { return get_gyro_health(_get_primary_gyro()); } bool get_gyro_health_all(void) const; virtual uint8_t get_gyro_count(void) const { return 1; }; + bool gyro_calibrated_ok(uint8_t instance) const { return _gyro_cal_ok[instance]; } + bool gyro_calibrated_ok_all() const; virtual bool get_accel_health(uint8_t instance) const { return true; } bool get_accel_health(void) const { return get_accel_health(get_primary_accel()); } @@ -224,6 +226,9 @@ protected: // board orientation from AHRS enum Rotation _board_orientation; + + // calibrated_ok flags + bool _gyro_cal_ok[INS_MAX_INSTANCES]; }; #include "AP_InertialSensor_Oilpan.h"