diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index d09016e16edd2779d60102b773929f4120d5b368..e8449c3b12397e676c13eb11d6702d3fccc7af8d 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -414,7 +414,7 @@ void Plane::acs_check(void) { acs.check(AP_ACS::ACS_FlightMode(control_mode), TECS_controller.get_flight_stage(), abs(throttle_percentage()), failsafe.last_heartbeat_ms, - gps.last_fix_time_ms(), geofence_breached(), is_flying()); + gps.num_sats(), geofence_breached(), is_flying()); AP_ACS::FailsafeState current_fs_state = acs.get_current_fs_state(); //always ignore failsafes in manual modes diff --git a/libraries/AP_ACS/AP_ACS.cpp b/libraries/AP_ACS/AP_ACS.cpp index 9af6f38ce2fa114161315d802de5f1f359abedec..1307489542a2a1d6cf3f7e9e06192ca010b0f335 100644 --- a/libraries/AP_ACS/AP_ACS.cpp +++ b/libraries/AP_ACS/AP_ACS.cpp @@ -19,7 +19,7 @@ const AP_Param::GroupInfo AP_ACS::var_info[] = { // @DisplayName: Watch the payload heartbeat // @Description: If this setting is not 0 then the plane will RTL if // it doesn't receive heartbeats from the payload companion computer. - // @User: Advanced + // @User: Advanced_ AP_GROUPINFO("WATCH_HB", 0, AP_ACS, _watch_heartbeat, 1), // @Param: KILL_THR @@ -45,6 +45,7 @@ AP_ACS::AP_ACS(const AP_BattMonitor* batt) , _motor_fail_workaround_start_ms(0) , _motor_restart_attempts(0) , _last_log_time(0) + , _last_gps_fix_time_ms(0) { AP_Param::setup_object_defaults(this, var_info); } @@ -99,7 +100,7 @@ void AP_ACS::set_throttle_kill_notified(bool tkn) { bool AP_ACS::check(ACS_FlightMode mode, AP_Vehicle::FixedWing::FlightStage flight_stage, int16_t thr_out, uint32_t last_heartbeat_ms, - uint32_t last_gps_fix_ms, bool fence_breached, bool is_flying) { + uint8_t num_gps_sats, bool fence_breached, bool is_flying) { uint32_t now = AP_HAL::millis(); @@ -128,7 +129,24 @@ bool AP_ACS::check(ACS_FlightMode mode, } //always check loss of GPS first - if ((now - last_gps_fix_ms) > 20000) { + if (num_gps_sats >= 6) { + //GPS is not failing + _last_gps_fix_time_ms = now; + + //If it was before we have to exit circle mode if we were in it. + if (_current_fs_state == GPS_LONG_FS + || _current_fs_state == GPS_SHORT_FS + || _current_fs_state == GPS_RECOVERING_FS) { + + if (mode == ACS_LOITER) { + //signal that we need to go back to previous flight mode + _current_fs_state = GPS_RECOVERING_FS; + return false; + } + } + } + + if ((now - _last_gps_fix_time_ms) > 20000) { if (_current_fs_state != GPS_LONG_FS) { hal.console->println("20 sec GPS FS"); } @@ -139,27 +157,15 @@ bool AP_ACS::check(ACS_FlightMode mode, //get_kill_throttle method //and the Arduplane code (see Attitude.pde) return false; - } else if ((now - last_gps_fix_ms) >= 5000) { + } else if ((now - _last_gps_fix_time_ms) >= 5000) { if (_current_fs_state != GPS_SHORT_FS) { hal.console->println("5 sec GPS FS"); _previous_mode = mode; } _current_fs_state = GPS_SHORT_FS; - return false; - } else { - //GPS is not failing - //If it was before we have to exit circle mode if we were in it. - if (_current_fs_state == GPS_LONG_FS - || _current_fs_state == GPS_SHORT_FS - || _current_fs_state == GPS_RECOVERING_FS) { - - if (mode == ACS_LOITER) { - //signal that we need to go back to previous flight mode - _current_fs_state = GPS_RECOVERING_FS; - return false; - } - } + return false; + } //ignore all failsafes except GPS when not flying diff --git a/libraries/AP_ACS/AP_ACS.h b/libraries/AP_ACS/AP_ACS.h index fcb0f088ed5c562c05684fb76ba7a3c608b4f003..cafe90ed511f8f9863cc81ffea53fcd86e7b3db5 100644 --- a/libraries/AP_ACS/AP_ACS.h +++ b/libraries/AP_ACS/AP_ACS.h @@ -75,7 +75,7 @@ public: //false if RTL should happen bool check(ACS_FlightMode mode, AP_Vehicle::FixedWing::FlightStage flight_stage, int16_t thr_out, - uint32_t last_heartbeat_ms, uint32_t last_gps_fix_ms, + uint32_t last_heartbeat_ms, uint8_t num_gps_sats, bool fence_breached, bool is_flying); #if AP_AHRS_NAVEKF_AVAILABLE @@ -109,6 +109,7 @@ protected: int _motor_restart_attempts; uint32_t _last_log_time; + uint32_t _last_gps_fix_time_ms; }; #endif // AP_ACS_H__