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

AP_Arming: Move accel/gyro_consistent to AP_IntertialSensor

Browse Source 
Move the accel_consistent and gyro_consistent methods from AP_Arming to AP_IntertialSensor
This commit is contained in:
Hayden Donald 2023-11-09 13:32:12 +11:00 committed by Andrew Tridgell
parent 0fca670f85
commit 2a55e6a030
1 changed files with 11 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
libraries/AP_Arming/AP_Arming.cpp
View File

@ -352,44 +352,15 @@ bool AP_Arming::logging_checks(bool report)
#if AP_INERTIALSENSOR_ENABLED
bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins)
{
const uint8_t accel_count = ins.get_accel_count();
if (accel_count <= 1) {
return true;
}
const Vector3f &prime_accel_vec = ins.get_accel();
const uint32_t now = AP_HAL::millis();
for(uint8_t i=0; i<accel_count; i++) {
if (!ins.use_accel(i)) {
continue;
}
// get next accel vector
const Vector3f &accel_vec = ins.get_accel(i);
Vector3f vec_diff = accel_vec - prime_accel_vec;
// allow for user-defined difference, typically 0.75 m/s/s. Has to pass in last 10 seconds
float threshold = accel_error_threshold;
if (i >= 2) {
/*
we allow for a higher threshold for IMU3 as it
runs at a different temperature to IMU1/IMU2,
and is not used for accel data in the EKF
*/
threshold *= 3;
}
// EKF is less sensitive to Z-axis error
vec_diff.z *= 0.5f;
if (vec_diff.length() > threshold) {
// this sensor disagrees with the primary sensor, so
// accels are inconsistent:
last_accel_pass_ms = 0;
return false;
}
if (!ins.accels_consistent(accel_error_threshold)) {
// accels are inconsistent:
last_accel_pass_ms = 0;
return false;
}
if (last_accel_pass_ms == 0) {
// we didn't return false in the loop above, so sensors are
// we didn't return false above, so sensors are
// consistent right now:
last_accel_pass_ms = now;
}
@ -404,30 +375,15 @@ bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins)
bool AP_Arming::ins_gyros_consistent(const AP_InertialSensor &ins)
{
const uint8_t gyro_count = ins.get_gyro_count();
if (gyro_count <= 1) {
return true;
}
const Vector3f &prime_gyro_vec = ins.get_gyro();
const uint32_t now = AP_HAL::millis();
for(uint8_t i=0; i<gyro_count; i++) {
if (!ins.use_gyro(i)) {
continue;
}
// 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
if (vec_diff.length() > radians(5)) {
// this sensor disagrees with the primary sensor, so
// gyros are inconsistent:
last_gyro_pass_ms = 0;
return false;
}
// allow for up to 5 degrees/s difference
if (!ins.gyros_consistent(5)) {
// gyros are inconsistent:
last_gyro_pass_ms = 0;
return false;
}
// we didn't return false in the loop above, so sensors are
// we didn't return false above, so sensors are
// consistent right now:
if (last_gyro_pass_ms == 0) {
last_gyro_pass_ms = now;