diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 7f9be23406..cd05feb0c5 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -106,6 +106,7 @@ #include // ArduPilot Mega Analog to Digital Converter Library #include #include +#include // Baro glitch protection library #include // ArduPilot Mega Magnetometer Library #include // ArduPilot Mega Vector/Matrix math Library #include // 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 index c5629252d2..f107988b9c 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -126,7 +126,8 @@ public: 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 index ed4fc6bf39..03336bfb1a 100644 --- 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 index 80e52d5b2a..c5de9059d9 100644 --- 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 index 5323a6c984..62d4ee7138 100644 --- 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