aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: detect missing accels or gyros

Browse Source
This commit is contained in:
Andrew Tridgell 2016-11-05 20:20:43 +11:00
parent 106a91c64c
commit 2c1e9024f1
1 changed files with 27 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -802,6 +802,12 @@ bool AP_InertialSensor::gyro_calibrated_ok_all() const
return false; return false;
} }
} }
for (uint8_t i=get_gyro_count(); i<INS_MAX_INSTANCES; i++) {
if (_gyro_id[i] != 0) {
// missing gyro
return false;
}
}
return (get_gyro_count() > 0); return (get_gyro_count() > 0);
} }
@ -909,6 +915,12 @@ bool AP_InertialSensor::accel_calibrated_ok_all() const
return false; return false;
} }
} }
for (uint8_t i=get_accel_count(); i<INS_MAX_INSTANCES; i++) {
if (_accel_id[i] != 0) {
// missing accel
return false;
}
}
// check calibrated accels matches number of accels (no unused accels should have offsets or scaling) // check calibrated accels matches number of accels (no unused accels should have offsets or scaling)
if (get_accel_count() < INS_MAX_INSTANCES) { if (get_accel_count() < INS_MAX_INSTANCES) {
@ -1078,10 +1090,14 @@ AP_InertialSensor::_init_gyro()
// save parameters to eeprom // save parameters to eeprom
void AP_InertialSensor::_save_gyro_calibration() void AP_InertialSensor::_save_gyro_calibration()
{ {
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { for (uint8_t i=0; i<_gyro_count; i++) {
_gyro_offset[i].save(); _gyro_offset[i].save();
_gyro_id[i].save(); _gyro_id[i].save();
} }
for (uint8_t i=_gyro_count; i<INS_MAX_INSTANCES; i++) {
_gyro_offset[i].set_and_save(Vector3f());
_gyro_id[i].set_and_save(0);
}
} }
@ -1537,11 +1553,18 @@ void AP_InertialSensor::_acal_save_calibrations()
_accel_id[i].save(); _accel_id[i].save();
_accel_id_ok[i] = true; _accel_id_ok[i] = true;
} else { } else {
_accel_offset[i].set_and_save(Vector3f(0,0,0)); _accel_offset[i].set_and_save(Vector3f());
_accel_scale[i].set_and_save(Vector3f(0,0,0)); _accel_scale[i].set_and_save(Vector3f());
} }
} }
// clear any unused accels
for (uint8_t i=_accel_count; i<INS_MAX_INSTANCES; i++) {
_accel_id[i].set_and_save(0);
_accel_offset[i].set_and_save(Vector3f());
_accel_scale[i].set_and_save(Vector3f());
}
Vector3f aligned_sample; Vector3f aligned_sample;
Vector3f misaligned_sample; Vector3f misaligned_sample;
switch(_trim_option) { switch(_trim_option) {