diff --git a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp index c4aa22a73c..c2d4771772 100644 --- a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp +++ b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp @@ -24,6 +24,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -189,7 +190,7 @@ AP_AdvancedFailsafe::check(uint32_t last_heartbeat_ms, bool geofence_breached, u uint32_t now = AP_HAL::millis(); bool gcs_link_ok = ((now - last_heartbeat_ms) < 10000); - bool gps_lock_ok = ((now - gps.last_fix_time_ms()) < 3000); + bool gps_lock_ok = ((now - AP::gps().last_fix_time_ms()) < 3000); switch (_state) { case STATE_PREFLIGHT: @@ -324,6 +325,7 @@ AP_AdvancedFailsafe::check_altlimit(void) // see if the barometer is dead const AP_Baro &baro = AP::baro(); + const AP_GPS &gps = AP::gps(); if (AP_HAL::millis() - baro.get_last_update() > 5000) { // the barometer has been unresponsive for 5 seconds. See if we can switch to GPS if (_amsl_margin_gps != -1 && diff --git a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h index 77f2b8a04a..7082ae9876 100644 --- a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h +++ b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h @@ -24,7 +24,6 @@ #include #include #include -#include #include #include @@ -51,11 +50,8 @@ public: }; // Constructor - AP_AdvancedFailsafe(AP_Mission &_mission, const AP_GPS &_gps) : - mission(_mission), - gps(_gps), - _gps_loss_count(0), - _comms_loss_count(0) + AP_AdvancedFailsafe(AP_Mission &_mission) : + mission(_mission) { AP_Param::setup_object_defaults(this, var_info); @@ -100,7 +96,6 @@ protected: enum state _state; AP_Mission &mission; - const AP_GPS &gps; AP_Int8 _enable; // digital output pins for communicating with the failsafe board