mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
Copter: integrate baro glitch protection
This commit is contained in:
parent
4db4471f49
commit
7cc1501dc6
@ -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
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user