Copter: integrate baro glitch protection

scottlee1 · Jul 28, 2014 · 7cc1501 · 7cc1501
1 parent 4db4471
commit 7cc1501
Show file tree

Hide file tree

Showing 5 changed files with 30 additions and 5 deletions.
diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
@@ -106,6 +106,7 @@
 #include <AP_ADC.h>             // ArduPilot Mega Analog to Digital Converter Library
 #include <AP_ADC_AnalogSource.h>
 #include <AP_Baro.h>
+#include <AP_Baro_Glitch.h>     // Baro glitch protection library
 #include <AP_Compass.h>         // ArduPilot Mega Magnetometer Library
 #include <AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
 #include <AP_Curve.h>           // Curve used to linearlise throttle pwm to thrust
@@ -267,6 +268,7 @@ static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::spi);
 #else
  #error Unrecognized CONFIG_BARO setting
 #endif
+static Baro_Glitch baro_glitch(barometer);
 
 #if CONFIG_COMPASS == HAL_COMPASS_PX4
 static AP_Compass_PX4 compass;
@@ -626,9 +628,9 @@ static float G_Dt = 0.02;
 // Inertial Navigation
 ////////////////////////////////////////////////////////////////////////////////
 #if AP_AHRS_NAVEKF_AVAILABLE
-static AP_InertialNav_NavEKF inertial_nav(ahrs, barometer, gps_glitch);
+static AP_InertialNav_NavEKF inertial_nav(ahrs, barometer, gps_glitch, baro_glitch);
 #else
-static AP_InertialNav inertial_nav(ahrs, barometer, gps_glitch);
+static AP_InertialNav inertial_nav(ahrs, barometer, gps_glitch, baro_glitch);
 #endif
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -1377,7 +1379,7 @@ static void read_AHRS(void)
     ahrs.update();
 }
 
-// read baro and sonar altitude at 20hz
+// read baro and sonar altitude at 10hz
 static void update_altitude()
 {
     // read in baro altitude

diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
@@ -126,7 +126,8 @@ class Parameters {
         k_param_geofence_limit,         // deprecated - remove
         k_param_altitude_limit,         // deprecated - remove
         k_param_fence,
-        k_param_gps_glitch,             // 70
+        k_param_gps_glitch,
+        k_param_baro_glitch,            // 71
 
         //
         // 75: Singlecopter, CoaxCopter

diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde
@@ -1029,6 +1029,10 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Path: ../libraries/AP_GPS/AP_GPS_Glitch.cpp
     GOBJECT(gps_glitch,      "GPSGLITCH_",   GPS_Glitch),
 
+    // @Group: BAROGLITCH_
+    // @Path: ../libraries/AP_Baro/AP_Baro_Glitch.cpp
+    GOBJECT(baro_glitch,    "BAROGLITCH_",  Baro_Glitch),
+
 #if FRAME_CONFIG ==     HELI_FRAME
     // @Group: H_
     // @Path: ../libraries/AP_Motors/AP_MotorsHeli.cpp

diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
@@ -348,6 +348,7 @@ enum FlipState {
 #define ERROR_SUBSYSTEM_PARACHUTE           15
 #define ERROR_SUBSYSTEM_EKF_CHECK           16
 #define ERROR_SUBSYSTEM_FAILSAFE_EKF        17
+#define ERROR_SUBSYSTEM_BARO                18
 // general error codes
 #define ERROR_CODE_ERROR_RESOLVED           0
 #define ERROR_CODE_FAILED_TO_INITIALISE     1
@@ -374,6 +375,8 @@ enum FlipState {
 // EKF check definitions
 #define ERROR_CODE_EKF_CHECK_BAD_COMPASS    2
 #define ERROR_CODE_EKF_CHECK_BAD_COMPASS_CLEARED    0
+// Baro specific error codes
+#define ERROR_CODE_BARO_GLITCH              2
 
 // Arming Check Enable/Disable bits
 #define ARMING_CHECK_NONE                   0x00

diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde
@@ -25,7 +25,22 @@ static int32_t read_barometer(void)
     if (g.log_bitmask & MASK_LOG_IMU) {
         Log_Write_Baro();
     }
-    return barometer.get_altitude() * 100.0f;
+    int32_t balt = barometer.get_altitude() * 100.0f;
+
+    // run glitch protection and update AP_Notify if home has been initialised
+    baro_glitch.check_alt();
+    bool report_baro_glitch = (baro_glitch.glitching() && !ap.usb_connected && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
+    if (AP_Notify::flags.baro_glitching != report_baro_glitch) {
+        if (baro_glitch.glitching()) {
+            Log_Write_Error(ERROR_SUBSYSTEM_BARO, ERROR_CODE_BARO_GLITCH);
+        } else {
+            Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_ERROR_RESOLVED);
+        }
+        AP_Notify::flags.baro_glitching = report_baro_glitch;
+    }
+
+    // return altitude
+    return balt;
 }
 
 // return sonar altitude in centimeters