Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Arming: record only aggregate sensor consistency check times

Browse Source 
All used sensors must be consistent with the primary sensor for 10 seconds, so we don't need to record times individually
This commit is contained in:
Peter Barker 2022-07-16 14:44:33 +10:00 committed by Peter Barker
parent fa71a8ccf2
commit f77d1812b8
2 changed files with 28 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_Arming/AP_Arming.cpp
View File

@ -332,14 +332,22 @@ bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins)
// EKF is less sensitive to Z-axis error
vec_diff.z *= 0.5f;
if (vec_diff.length() <= threshold) {
last_accel_pass_ms[i] = now;
}
if (now - last_accel_pass_ms[i] > 10000) {
if (vec_diff.length() > threshold) {
// this sensor disagrees with the primary sensor, so
// accels are inconsistent:
return false;
}
}
// we didn't return false in the loop above, so sensors are
// consistent right now:
last_accel_pass_ms = now;
// must pass for at least 10 seconds before we're considered consistent:
if (now - last_accel_pass_ms > 10000) {
return false;
}
return true;
}
@ -359,16 +367,23 @@ bool AP_Arming::ins_gyros_consistent(const AP_InertialSensor &ins)
// get next gyro vector
const Vector3f &gyro_vec = ins.get_gyro(i);
const 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] = now;
}
if (now - last_gyro_pass_ms[i] > 10000) {
// allow for up to 5 degrees/s difference
if (vec_diff.length() > radians(5)) {
// this sensor disagrees with the primary sensor, so
// gyros are inconsistent:
return false;
}
}
// we didn't return false in the loop above, so sensors are
// consistent right now:
last_gyro_pass_ms = now;
// must pass for at least 10 seconds before we're considered consistent:
if (now - last_gyro_pass_ms > 10000) {
return false;
}
return true;
}

6
libraries/AP_Arming/AP_Arming.h
View File

@ -3,7 +3,7 @@
#include <AP_HAL/AP_HAL_Boards.h>
#include <AP_HAL/Semaphores.h>
#include <AP_Param/AP_Param.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
class AP_Arming {
public:
@ -142,8 +142,8 @@ protected:
// internal members
bool armed;
uint32_t last_accel_pass_ms[INS_MAX_INSTANCES];
uint32_t last_gyro_pass_ms[INS_MAX_INSTANCES];
uint32_t last_accel_pass_ms;
uint32_t last_gyro_pass_ms;
virtual bool barometer_checks(bool report);