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

AP_Arming: break out a gyros-consistent method

Browse Source
This commit is contained in:
Peter Barker 2017-07-27 14:25:06 +10:00 committed by Francisco Ferreira
parent d2b8ea4bb0
commit 410e72f83c
2 changed files with 32 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
libraries/AP_Arming/AP_Arming.cpp
View File

@ -208,6 +208,33 @@ bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins)
return true; return true;
} }
bool AP_Arming::ins_gyros_consistent(const AP_InertialSensor &ins)
{
if (ins.get_gyro_count() <= 1) {
return true;
}
const Vector3f &prime_gyro_vec = ins.get_gyro();
for(uint8_t i=0; i<ins.get_gyro_count(); i++) {
if (!ins.use_gyro(i)) {
continue;
}
// get next gyro vector
const Vector3f &gyro_vec = ins.get_gyro(i);
Vector3f vec_diff = gyro_vec - prime_gyro_vec;
// allow for up to 5 degrees/s difference. Pass if it has
// been OK in last 10 seconds
if (vec_diff.length() <= radians(5)) {
last_gyro_pass_ms[i] = AP_HAL::millis();
}
if (AP_HAL::millis() - last_gyro_pass_ms[i] > 10000) {
return false;
}
}
return true;
}
bool AP_Arming::ins_checks(bool report) bool AP_Arming::ins_checks(bool report)
{ {
if ((checks_to_perform & ARMING_CHECK_ALL) || if ((checks_to_perform & ARMING_CHECK_ALL) ||
@ -255,27 +282,11 @@ bool AP_Arming::ins_checks(bool report)
} }
// check all gyros are giving consistent readings // check all gyros are giving consistent readings
if (ins.get_gyro_count() > 1) { if (!ins_gyros_consistent(ins)) {
const Vector3f &prime_gyro_vec = ins.get_gyro(); if (report) {
for(uint8_t i=0; i<ins.get_gyro_count(); i++) { gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Gyros inconsistent");
if (!ins.use_gyro(i)) {
continue;
}
// get next gyro vector
const Vector3f &gyro_vec = ins.get_gyro(i);
Vector3f vec_diff = gyro_vec - prime_gyro_vec;
// allow for up to 5 degrees/s difference. Pass if it has
// been OK in last 10 seconds
if (vec_diff.length() <= radians(5)) {
last_gyro_pass_ms[i] = AP_HAL::millis();
}
if (AP_HAL::millis() - last_gyro_pass_ms[i] > 10000) {
if (report) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Gyros inconsistent");
}
return false;
}
} }
return false;
} }
} }

1
libraries/AP_Arming/AP_Arming.h
View File

@ -110,5 +110,6 @@ protected:
private: private:
bool ins_accels_consistent(const AP_InertialSensor &ins); bool ins_accels_consistent(const AP_InertialSensor &ins);
bool ins_gyros_consistent(const AP_InertialSensor &ins);
}; };