Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 14:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: remove GPS glitch and failsafe

Browse Source 
The EKF failsafe now captures all failures that could lead to a bad
position including GPS glitches and a bad compass meaning we do not need
this protection in the main flight code.
This commit is contained in:
Randy Mackay 2015-03-13 11:05:30 +09:00
parent 0344ec5d89
commit fbfc94cf69
11 changed files with 9 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/AP_State.pde
View File

@ -86,16 +86,6 @@ void set_failsafe_battery(bool b)
AP_Notify::flags.failsafe_battery = b;
}
// ---------------------------------------------
static void set_failsafe_gps(bool b)
{
failsafe.gps = b;
// update AP_Notify
AP_Notify::flags.failsafe_gps = b;
}
// ---------------------------------------------
static void set_failsafe_gcs(bool b)
{

26
ArduCopter/ArduCopter.pde
View File

@ -103,7 +103,6 @@
#include <GCS_MAVLink.h> // MAVLink GCS definitions
#include <AP_SerialManager.h> // Serial manager library
#include <AP_GPS.h> // ArduPilot GPS library
#include <AP_GPS_Glitch.h> // GPS glitch protection library
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <AP_ADC_AnalogSource.h>
@ -257,8 +256,6 @@ static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor:
static AP_GPS gps;
static GPS_Glitch gps_glitch(gps);
// flight modes convenience array
static AP_Int8 *flight_modes = &g.flight_mode1;
@ -408,7 +405,6 @@ static struct {
uint8_t rc_override_active : 1; // 0 // true if rc control are overwritten by ground station
uint8_t radio : 1; // 1 // A status flag for the radio failsafe
uint8_t battery : 1; // 2 // A status flag for the battery failsafe
uint8_t gps : 1; // 3 // A status flag for the gps failsafe
uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe
uint8_t ekf : 1; // 5 // true if ekf failsafe has occurred
@ -602,7 +598,7 @@ static float G_Dt = 0.02;
////////////////////////////////////////////////////////////////////////////////
// Inertial Navigation
////////////////////////////////////////////////////////////////////////////////
static AP_InertialNav_NavEKF inertial_nav(ahrs, barometer, gps_glitch);
static AP_InertialNav_NavEKF inertial_nav(ahrs);
////////////////////////////////////////////////////////////////////////////////
// Attitude, Position and Waypoint navigation objects
@ -1175,12 +1171,11 @@ static void one_hz_loop()
static void update_GPS(void)
{
static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; // time of last gps message
bool report_gps_glitch;
bool gps_updated = false;
gps.update();
// logging and glitch protection run after every gps message
// log after every gps message
for (uint8_t i=0; i<gps.num_sensors(); i++) {
if (gps.last_message_time_ms(i) != last_gps_reading[i]) {
last_gps_reading[i] = gps.last_message_time_ms(i);
@ -1195,20 +1190,6 @@ static void update_GPS(void)
}
if (gps_updated) {
// run glitch protection and update AP_Notify if home has been initialised
if (home_is_set()) {
gps_glitch.check_position();
report_gps_glitch = (gps_glitch.glitching() && !ap.usb_connected && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
if (AP_Notify::flags.gps_glitching != report_gps_glitch) {
if (gps_glitch.glitching()) {
Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_GPS_GLITCH);
}else{
Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_ERROR_RESOLVED);
}
AP_Notify::flags.gps_glitching = report_gps_glitch;
}
}
// set system time if necessary
set_system_time_from_GPS();
@ -1228,9 +1209,6 @@ static void update_GPS(void)
#endif
}
}
// check for loss of gps
failsafe_gps_check();
}
static void

4
ArduCopter/GCS_Mavlink.pde
View File

@ -43,7 +43,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
uint32_t custom_mode = control_mode;
// set system as critical if any failsafe have triggered
if (failsafe.radio || failsafe.battery || failsafe.gps || failsafe.gcs || failsafe.ekf) {
if (failsafe.radio || failsafe.battery || failsafe.gcs || failsafe.ekf) {
system_status = MAV_STATE_CRITICAL;
}
@ -197,7 +197,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
}
if (gps.status() > AP_GPS::NO_GPS && (!gps_glitch.glitching()||ap.usb_connected)) {
if (gps.status() > AP_GPS::NO_GPS) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
}
#if OPTFLOW == ENABLED

5
ArduCopter/Parameters.h
View File

@ -135,7 +135,7 @@ public:
k_param_geofence_limit, // deprecated - remove
k_param_altitude_limit, // deprecated - remove
k_param_fence,
k_param_gps_glitch,
k_param_gps_glitch, // deprecated
k_param_baro_glitch, // 71 - deprecated
//
@ -258,7 +258,7 @@ public:
k_param_rc_speed = 192,
k_param_failsafe_battery_enabled,
k_param_throttle_mid,
k_param_failsafe_gps_enabled,
k_param_failsafe_gps_enabled, // remove
k_param_rc_9,
k_param_rc_12,
k_param_failsafe_gcs, // 198
@ -342,7 +342,6 @@ public:
AP_Float fs_batt_voltage; // battery voltage below which failsafe will be triggered
AP_Float fs_batt_mah; // battery capacity (in mah) below which failsafe will be triggered
AP_Int8 failsafe_gps_enabled; // gps failsafe enabled
AP_Int8 failsafe_gcs; // ground station failsafe behavior
AP_Int16 gps_hdop_good; // GPS Hdop value at or below this value represent a good position

11
ArduCopter/Parameters.pde
View File

@ -115,13 +115,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard
GSCALAR(fs_batt_mah, "FS_BATT_MAH", FS_BATT_MAH_DEFAULT),
// @Param: FS_GPS_ENABLE
// @DisplayName: GPS Failsafe Enable
// @Description: Controls what action will be taken if GPS signal is lost for at least 5 seconds
// @Values: 0:Disabled,1:Land,2:AltHold,3:Land even from Stabilize
// @User: Standard
GSCALAR(failsafe_gps_enabled, "FS_GPS_ENABLE", FS_GPS_LAND),
// @Param: FS_GCS_ENABLE
// @DisplayName: Ground Station Failsafe Enable
// @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. NB. The GCS Failsafe is only active when RC_OVERRIDE is being used to control the vehicle.
@ -899,10 +892,6 @@ const AP_Param::Info var_info[] PROGMEM = {
GOBJECT(rally, "RALLY_", AP_Rally),
#endif
// @Group: GPSGLITCH_
// @Path: ../libraries/AP_GPS/AP_GPS_Glitch.cpp
GOBJECT(gps_glitch, "GPSGLITCH_", GPS_Glitch),
#if FRAME_CONFIG == HELI_FRAME
// @Group: H_
// @Path: ../libraries/AP_Motors/AP_MotorsHeli.cpp

2
ArduCopter/compassmot.pde
View File

@ -101,7 +101,6 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
// disable throttle and battery failsafe
g.failsafe_throttle = FS_THR_DISABLED;
g.failsafe_battery_enabled = FS_BATT_DISABLED;
g.failsafe_gps_enabled = FS_GPS_DISABLED;
// disable motor compensation
compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED);
@ -259,7 +258,6 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
// re-enable failsafes
g.failsafe_throttle.load();
g.failsafe_battery_enabled.load();
g.failsafe_gps_enabled.load();
// flag we have completed
ap.compass_mot = false;

5
ArduCopter/config.h
View File

@ -274,10 +274,7 @@
# define BOARD_VOLTAGE_MAX 5.8f // max board voltage in volts for pre-arm checks
#endif
// GPS failsafe
#ifndef FAILSAFE_GPS_TIMEOUT_MS
# define FAILSAFE_GPS_TIMEOUT_MS 5000 // gps failsafe triggers after 5 seconds with no GPS
#endif
// prearm GPS hdop check
#ifndef GPS_HDOP_GOOD_DEFAULT
# define GPS_HDOP_GOOD_DEFAULT 230 // minimum hdop that represents a good position. used during pre-arm checks if fence is enabled
#endif

6
ArduCopter/defines.h
View File

@ -320,11 +320,11 @@ enum FlipState {
#define ERROR_SUBSYSTEM_OPTFLOW 4
#define ERROR_SUBSYSTEM_FAILSAFE_RADIO 5
#define ERROR_SUBSYSTEM_FAILSAFE_BATT 6
#define ERROR_SUBSYSTEM_FAILSAFE_GPS 7
#define ERROR_SUBSYSTEM_FAILSAFE_GPS 7 // not used
#define ERROR_SUBSYSTEM_FAILSAFE_GCS 8
#define ERROR_SUBSYSTEM_FAILSAFE_FENCE 9
#define ERROR_SUBSYSTEM_FLIGHT_MODE 10
#define ERROR_SUBSYSTEM_GPS 11
#define ERROR_SUBSYSTEM_GPS 11 // not used
#define ERROR_SUBSYSTEM_CRASH_CHECK 12
#define ERROR_SUBSYSTEM_FLIP 13
#define ERROR_SUBSYSTEM_AUTOTUNE 14
@ -343,8 +343,6 @@ enum FlipState {
#define ERROR_CODE_FAILSAFE_OCCURRED 1
// subsystem specific error codes -- compass
#define ERROR_CODE_COMPASS_FAILED_TO_READ 2
// subsystem specific error codes -- gps
#define ERROR_CODE_GPS_GLITCH 2
// subsystem specific error codes -- main
#define ERROR_CODE_MAIN_INS_DELAY 1
// subsystem specific error codes -- crash checker

63
ArduCopter/events.pde
View File

@ -162,69 +162,6 @@ static void failsafe_battery_event(void)
}
// failsafe_gps_check - check for gps failsafe
static void failsafe_gps_check()
{
uint32_t last_gps_update_ms;
// return immediately if gps failsafe is disabled or we have never had GPS lock
if (g.failsafe_gps_enabled == FS_GPS_DISABLED || !home_is_set()) {
// if we have just disabled the gps failsafe, ensure the gps failsafe event is cleared
if (failsafe.gps) {
failsafe_gps_off_event();
set_failsafe_gps(false);
}
return;
}
// calc time since last gps update
last_gps_update_ms = millis() - gps_glitch.last_good_update();
// check if all is well
if( last_gps_update_ms < FAILSAFE_GPS_TIMEOUT_MS) {
// check for recovery from gps failsafe
if( failsafe.gps ) {
failsafe_gps_off_event();
set_failsafe_gps(false);
}
return;
}
// do nothing if gps failsafe already triggered or motors disarmed
if( failsafe.gps || !motors.armed()) {
return;
}
// GPS failsafe event has occured
// update state, warn the ground station and log to dataflash
set_failsafe_gps(true);
gcs_send_text_P(SEVERITY_HIGH,PSTR("Lost GPS!"));
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GPS, ERROR_CODE_FAILSAFE_OCCURRED);
// take action based on flight mode and FS_GPS_ENABLED parameter
if (mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) {
if (g.failsafe_gps_enabled == FS_GPS_ALTHOLD && !failsafe.radio) {
set_mode(ALT_HOLD);
// alert pilot to mode change
AP_Notify::events.failsafe_mode_change = 1;
}else{
set_mode_land_with_pause();
}
}
// if flight mode is LAND ensure it's not the GPS controlled LAND
if (control_mode == LAND) {
land_do_not_use_GPS();
}
}
// failsafe_gps_off_event - actions to take when GPS contact is restored
static void failsafe_gps_off_event(void)
{
// log recovery of GPS in logs?
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GPS, ERROR_CODE_FAILSAFE_RESOLVED);
}
// failsafe_gcs_check - check for ground station failsafe
static void failsafe_gcs_check()
{

2
ArduCopter/motor_test.pde
View File

@ -118,7 +118,6 @@ static uint8_t mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_se
// disable throttle, battery and gps failsafe
g.failsafe_throttle = FS_THR_DISABLED;
g.failsafe_battery_enabled = FS_BATT_DISABLED;
g.failsafe_gps_enabled = FS_GPS_DISABLED;
g.failsafe_gcs = FS_GCS_DISABLED;
// turn on notify leds
@ -160,7 +159,6 @@ static void motor_test_stop()
// re-enable failsafes
g.failsafe_throttle.load();
g.failsafe_battery_enabled.load();
g.failsafe_gps_enabled.load();
g.failsafe_gcs.load();
// turn off notify leds

14
ArduCopter/motors.pde
View File

@ -499,11 +499,6 @@ static bool pre_arm_gps_checks(bool display_failure)
// check if flight mode requires GPS
bool gps_required = mode_requires_GPS(control_mode);
// if GPS failsafe will triggers even in stabilize mode we need GPS before arming
if (g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) {
gps_required = true;
}
#if AC_FENCE == ENABLED
// if circular fence is enabled we need GPS
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) {
@ -517,15 +512,6 @@ static bool pre_arm_gps_checks(bool display_failure)
return true;
}
// check GPS is not glitching
if (gps_glitch.glitching()) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: GPS Glitch"));
}
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
// ensure GPS is ok
if (!position_ok()) {
if (display_failure) {