Copter: integrate baro glitch protection

This commit is contained in:
Randy Mackay 2014-07-28 17:38:32 +09:00
parent 4db4471f49
commit 7cc1501dc6
5 changed files with 30 additions and 5 deletions

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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