AP_InertialSensor: Add threshold arguments to consistency checks

Added arguments to the gyros_consistent and accels_consistent methods for allowable threshold and required time for checks

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -1421,8 +1421,11 @@ bool AP_InertialSensor::get_gyro_health_all(void) const
     return (get_gyro_count() > 0);
 }
 
-bool AP_InertialSensor::gyros_consistent()
+// threshold in degrees/s to be consistent, consistent_time_sec duration for which
+// gyros need to be consistent to be considered consistent
+bool AP_InertialSensor::gyros_consistent(uint8_t threshold, uint8_t consistent_time_sec)
 {
+
     const uint8_t gyro_count = get_gyro_count();
     if (gyro_count <= 1) {
         return true;
@@ -1437,8 +1440,8 @@ bool AP_InertialSensor::gyros_consistent()
         // get next gyro vector
         const Vector3f &gyro_vec = 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)) {
+        // allow for up to threshold degrees/s difference
+        if (vec_diff.length() > radians(threshold)) {
             // this sensor disagrees with the primary sensor, so
             // gyros are inconsistent:
             last_gyro_pass_ms = 0;
@@ -1452,8 +1455,8 @@ bool AP_InertialSensor::gyros_consistent()
         last_gyro_pass_ms = now;
     }
 
-    // must pass for at least 10 seconds before we're considered consistent:
-    if (now - last_gyro_pass_ms < 10000) {
+    // must pass for at least consistent_time_sec seconds before we're considered consistent:
+    if (now - last_gyro_pass_ms < consistent_time_sec * 1000) {
         return false;
     }
 
@@ -1499,7 +1502,9 @@ bool AP_InertialSensor::get_accel_health_all(void) const
     return (get_accel_count() > 0);
 }
 
-bool AP_InertialSensor::accels_consistent(float accel_error_threshold)
+// accel_error_threshold in m/s/s to be consistent, consistent_time_sec duration for which
+// accels need to be consistent to be considered consistent
+bool AP_InertialSensor::accels_consistent(float accel_error_threshold, uint8_t consistent_time_sec)
 {
     const uint8_t accel_count = get_accel_count();
     if (accel_count <= 1) {
@@ -1515,7 +1520,7 @@ bool AP_InertialSensor::accels_consistent(float accel_error_threshold)
         // get next accel vector
         const Vector3f &accel_vec = 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
+        // allow for user-defined difference, threshold m/s/s. Has to pass in last consistent_time_sec seconds
         float threshold = accel_error_threshold;
         if (i >= 2) {
             /*
@@ -1532,9 +1537,6 @@ bool AP_InertialSensor::accels_consistent(float accel_error_threshold)
         if (vec_diff.length() > threshold) {
             // this sensor disagrees with the primary sensor, so
             // accels are inconsistent:
-
-
-
             last_accel_pass_ms = 0;
             return false;
         }
@@ -1546,8 +1548,8 @@ bool AP_InertialSensor::accels_consistent(float accel_error_threshold)
         last_accel_pass_ms = now;
     }
 
-    // must pass for at least 10 seconds before we're considered consistent:
-    if (now - last_accel_pass_ms < 10000) {
+    // must pass for at least consistent_time_sec seconds before we're considered consistent:
+    if (now - last_accel_pass_ms < consistent_time_sec * 1000) {
         return false;
     }
 

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -137,7 +137,7 @@ class AP_InertialSensor : AP_AccelCal_Client
     bool get_gyro_health(uint8_t instance) const { return (instance<_gyro_count) ? _gyro_healthy[instance] : false; }
     bool get_gyro_health(void) const { return get_gyro_health(_primary_gyro); }
     bool get_gyro_health_all(void) const;
-    bool gyros_consistent();
+    bool gyros_consistent(uint8_t threshold, uint8_t consistent_time_sec);
     uint8_t get_gyro_count(void) const { return MIN(INS_MAX_INSTANCES, _gyro_count); }
     bool gyro_calibrated_ok(uint8_t instance) const { return _gyro_cal_ok[instance]; }
     bool gyro_calibrated_ok_all() const;
@@ -147,7 +147,7 @@ class AP_InertialSensor : AP_AccelCal_Client
     bool get_accel_health(uint8_t instance) const { return (instance<_accel_count) ? _accel_healthy[instance] : false; }
     bool get_accel_health(void) const { return get_accel_health(_primary_accel); }
     bool get_accel_health_all(void) const;
-    bool accels_consistent(float accel_error_threshold);
+    bool accels_consistent(float accel_error_threshold, uint8_t consistent_time_sec);
     uint8_t get_accel_count(void) const { return MIN(INS_MAX_INSTANCES, _accel_count); }
     bool accel_calibrated_ok_all() const;
     bool use_accel(uint8_t instance) const;