Browse Source

Copter: remove baro_glitch protection

mission-4.1.18
Randy Mackay 10 years ago
parent
commit
d7f624be39
  1. 7
      ArduCopter/ArduCopter.pde
  2. 2
      ArduCopter/Parameters.h
  3. 4
      ArduCopter/Parameters.pde
  4. 14
      ArduCopter/sensors.pde

7
ArduCopter/ArduCopter.pde

@ -108,7 +108,6 @@ @@ -108,7 +108,6 @@
#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
@ -265,8 +264,6 @@ static AP_Int8 *flight_modes = &g.flight_mode1; @@ -265,8 +264,6 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
static AP_Baro barometer;
static Baro_Glitch baro_glitch(barometer);
#if CONFIG_COMPASS == HAL_COMPASS_PX4
static AP_Compass_PX4 compass;
#elif CONFIG_COMPASS == HAL_COMPASS_VRBRAIN
@ -606,9 +603,9 @@ static float G_Dt = 0.02; @@ -606,9 +603,9 @@ static float G_Dt = 0.02;
// Inertial Navigation
////////////////////////////////////////////////////////////////////////////////
#if AP_AHRS_NAVEKF_AVAILABLE
static AP_InertialNav_NavEKF inertial_nav(ahrs, barometer, gps_glitch, baro_glitch);
static AP_InertialNav_NavEKF inertial_nav(ahrs, barometer, gps_glitch);
#else
static AP_InertialNav inertial_nav(ahrs, barometer, gps_glitch, baro_glitch);
static AP_InertialNav inertial_nav(ahrs, barometer, gps_glitch);
#endif
////////////////////////////////////////////////////////////////////////////////

2
ArduCopter/Parameters.h

@ -136,7 +136,7 @@ public: @@ -136,7 +136,7 @@ public:
k_param_altitude_limit, // deprecated - remove
k_param_fence,
k_param_gps_glitch,
k_param_baro_glitch, // 71
k_param_baro_glitch, // 71 - deprecated
//
// 75: Singlecopter, CoaxCopter

4
ArduCopter/Parameters.pde

@ -907,10 +907,6 @@ const AP_Param::Info var_info[] PROGMEM = { @@ -907,10 +907,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Path: ../libraries/AP_GPS/AP_GPS_Glitch.cpp
GOBJECT(gps_glitch, "GPSGLITCH_", GPS_Glitch),
// @Group: BAROGLTCH_
// @Path: ../libraries/AP_Baro/AP_Baro_Glitch.cpp
GOBJECT(baro_glitch, "BAROGLTCH_", Baro_Glitch),
#if FRAME_CONFIG == HELI_FRAME
// @Group: H_
// @Path: ../libraries/AP_Motors/AP_MotorsHeli.cpp

14
ArduCopter/sensors.pde

@ -8,8 +8,6 @@ static void init_barometer(bool full_calibration) @@ -8,8 +8,6 @@ static void init_barometer(bool full_calibration)
}else{
barometer.update_calibration();
}
// reset glitch protection to use new baro alt
baro_glitch.reset();
gcs_send_text_P(SEVERITY_LOW, PSTR("barometer calibration complete"));
}
@ -22,18 +20,6 @@ static void read_barometer(void) @@ -22,18 +20,6 @@ static void read_barometer(void)
}
baro_alt = barometer.get_altitude() * 100.0f;
baro_climbrate = barometer.get_climb_rate() * 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_BARO, ERROR_CODE_ERROR_RESOLVED);
}
AP_Notify::flags.baro_glitching = report_baro_glitch;
}
}
#if CONFIG_SONAR == ENABLED

Loading…
Cancel
Save