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

INS: add gyro_calibrated_ok_all method

Browse Source 
This returns true if the gyros have been calibrated successfully
This commit is contained in:
Randy Mackay 2014-10-08 20:17:31 +09:00
parent 70ca87c4e6
commit 834f2bea07
2 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -418,6 +418,17 @@ bool AP_InertialSensor::get_gyro_health_all(void) const
return (get_gyro_count() > 0); 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<get_gyro_count(); i++) {
if (!gyro_calibrated_ok(i)) {
return false;
}
}
return (get_gyro_count() > 0);
}
// get_accel_health_all - return true if all accels are healthy // get_accel_health_all - return true if all accels are healthy
bool AP_InertialSensor::get_accel_health_all(void) const bool AP_InertialSensor::get_accel_health_all(void) const
{ {
@ -550,6 +561,7 @@ AP_InertialSensor::_init_gyro()
best_diff[k] = 0; best_diff[k] = 0;
last_average[k].zero(); last_average[k].zero();
converged[k] = false; converged[k] = false;
_gyro_cal_ok[k] = true; // default calibration ok flag to true
} }
for(int8_t c = 0; c < 5; c++) { 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"), hal.console->printf_P(PSTR("gyro[%u] did not converge: diff=%f dps\n"),
(unsigned)k, ToDeg(best_diff[k])); (unsigned)k, ToDeg(best_diff[k]));
_gyro_offset[k] = best_avg[k]; _gyro_offset[k] = best_avg[k];
// flag calibration as failed for this gyro
_gyro_cal_ok[k] = false;
} }
} }
} }

5
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -120,6 +120,8 @@ public:
bool get_gyro_health(void) const { return get_gyro_health(_get_primary_gyro()); } bool get_gyro_health(void) const { return get_gyro_health(_get_primary_gyro()); }
bool get_gyro_health_all(void) const; bool get_gyro_health_all(void) const;
virtual uint8_t get_gyro_count(void) const { return 1; }; 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; } virtual bool get_accel_health(uint8_t instance) const { return true; }
bool get_accel_health(void) const { return get_accel_health(get_primary_accel()); } bool get_accel_health(void) const { return get_accel_health(get_primary_accel()); }
@ -224,6 +226,9 @@ protected:
// board orientation from AHRS // board orientation from AHRS
enum Rotation _board_orientation; enum Rotation _board_orientation;
// calibrated_ok flags
bool _gyro_cal_ok[INS_MAX_INSTANCES];
}; };
#include "AP_InertialSensor_Oilpan.h" #include "AP_InertialSensor_Oilpan.h"