Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -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);
}
// 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
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;
}
}
}

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_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"