AP_AHRS: use GPS singleton

libraries/AP_AHRS/AP_AHRS.cpp

@@ -143,10 +143,10 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret) const
 {
     if (airspeed_sensor_enabled()) {
         *airspeed_ret = _airspeed->get_airspeed();
-        if (_wind_max > 0 && _gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
+        if (_wind_max > 0 && AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
             // constrain the airspeed by the ground speed
             // and AHRS_WIND_MAX
-            const float gnd_speed = _gps.ground_speed();
+            const float gnd_speed = AP::gps().ground_speed();
             float true_airspeed = *airspeed_ret * get_EAS2TAS();
             true_airspeed = constrain_float(true_airspeed,
                                             gnd_speed - _wind_max,
@@ -193,7 +193,7 @@ Vector2f AP_AHRS::groundspeed_vector(void)
     Vector2f gndVelGPS;
     float airspeed;
     const bool gotAirspeed = airspeed_estimate_true(&airspeed);
-    const bool gotGPS = (_gps.status() >= AP_GPS::GPS_OK_FIX_2D);
+    const bool gotGPS = (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D);
     if (gotAirspeed) {
         const Vector3f wind = wind_estimate();
         const Vector2f wind2d(wind.x, wind.y);
@@ -203,8 +203,8 @@ Vector2f AP_AHRS::groundspeed_vector(void)
 
     // Generate estimate of ground speed vector using GPS
     if (gotGPS) {
-        const float cog = radians(_gps.ground_course_cd()*0.01f);
-        gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * _gps.ground_speed();
+        const float cog = radians(AP::gps().ground_course_cd()*0.01f);
+        gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * AP::gps().ground_speed();
     }
     // If both ADS and GPS data is available, apply a complementary filter
     if (gotAirspeed && gotGPS) {

libraries/AP_AHRS/AP_AHRS.h

@@ -53,7 +53,7 @@ public:
     friend class AP_AHRS_View;
     
     // Constructor
-    AP_AHRS(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) :
+    AP_AHRS(AP_InertialSensor &ins, AP_Baro &baro) :
         roll(0.0f),
         pitch(0.0f),
         yaw(0.0f),
@@ -68,7 +68,6 @@ public:
         _compass_last_update(0),
         _ins(ins),
         _baro(baro),
-        _gps(gps),
         _cos_roll(1.0f),
         _cos_pitch(1.0f),
         _cos_yaw(1.0f),
@@ -201,10 +200,6 @@ public:
         return _beacon;
     }
 
-    const AP_GPS &get_gps() const {
-        return _gps;
-    }
-
     const AP_InertialSensor &get_ins() const {
         return _ins;
     }
@@ -628,7 +623,6 @@ protected:
     //       IMU under us without our noticing.
     AP_InertialSensor   &_ins;
     AP_Baro             &_baro;
-    const AP_GPS        &_gps;
 
     // a vector to capture the difference between the controller and body frames
     AP_Vector3f         _trim;

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -364,7 +364,7 @@ AP_AHRS_DCM::_yaw_gain(void) const
 // return true if we have and should use GPS
 bool AP_AHRS_DCM::have_gps(void) const
 {
-    if (_gps.status() <= AP_GPS::NO_FIX || !_gps_use) {
+    if (AP::gps().status() <= AP_GPS::NO_FIX || !_gps_use) {
         return false;
     }
     return true;
@@ -395,7 +395,7 @@ bool AP_AHRS_DCM::use_compass(void)
         // we don't have any alterative to the compass
         return true;
     }
-    if (_gps.ground_speed() < GPS_SPEED_MIN) {
+    if (AP::gps().ground_speed() < GPS_SPEED_MIN) {
         // we are not going fast enough to use the GPS
         return true;
     }
@@ -404,8 +404,8 @@ bool AP_AHRS_DCM::use_compass(void)
     // degrees and the estimated wind speed is less than 80% of the
     // ground speed, then switch to GPS navigation. This will help
     // prevent flyaways with very bad compass offsets
-    const int32_t error = abs(wrap_180_cd(yaw_sensor - _gps.ground_course_cd()));
-    if (error > 4500 && _wind.length() < _gps.ground_speed()*0.8f) {
+    const int32_t error = abs(wrap_180_cd(yaw_sensor - AP::gps().ground_course_cd()));
+    if (error > 4500 && _wind.length() < AP::gps().ground_speed()*0.8f) {
         if (AP_HAL::millis() - _last_consistent_heading > 2000) {
             // start using the GPS for heading if the compass has been
             // inconsistent with the GPS for 2 seconds
@@ -429,6 +429,8 @@ AP_AHRS_DCM::drift_correction_yaw(void)
     float yaw_error;
     float yaw_deltat;
 
+    const AP_GPS &_gps = AP::gps();
+
     if (AP_AHRS_DCM::use_compass()) {
         /*
           we are using compass for yaw
@@ -607,6 +609,8 @@ AP_AHRS_DCM::drift_correction(float deltat)
     // we have integrated over
     _ra_deltat += deltat;
 
+    const AP_GPS &_gps = AP::gps();
+
     if (!have_gps() ||
             _gps.status() < AP_GPS::GPS_OK_FIX_3D ||
             _gps.num_sats() < _gps_minsats) {
@@ -956,6 +960,7 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const
     loc.flags.relative_alt = 0;
     loc.flags.terrain_alt = 0;
     location_offset(loc, _position_offset_north, _position_offset_east);
+    const AP_GPS &_gps = AP::gps();
     if (_flags.fly_forward && _have_position) {
         float gps_delay_sec = 0;
         _gps.get_lag(gps_delay_sec);
@@ -983,10 +988,10 @@ bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret) const
         ret = true;
     }
 
-    if (ret && _wind_max > 0 && _gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
+    if (ret && _wind_max > 0 && AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
         // constrain the airspeed by the ground speed
         // and AHRS_WIND_MAX
-        const float gnd_speed = _gps.ground_speed();
+        const float gnd_speed = AP::gps().ground_speed();
         float true_airspeed = *airspeed_ret * get_EAS2TAS();
         true_airspeed = constrain_float(true_airspeed,
                                         gnd_speed - _wind_max,

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -23,8 +23,8 @@
 
 class AP_AHRS_DCM : public AP_AHRS {
 public:
-    AP_AHRS_DCM(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps)
-        : AP_AHRS(ins, baro, gps)
+    AP_AHRS_DCM(AP_InertialSensor &ins, AP_Baro &baro)
+        : AP_AHRS(ins, baro)
         , _omega_I_sum_time(0.0f)
         , _renorm_val_sum(0.0f)
         , _renorm_val_count(0)

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -30,8 +30,12 @@
 extern const AP_HAL::HAL& hal;
 
 // constructor
-AP_AHRS_NavEKF::AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, NavEKF2 &_EKF2, NavEKF3 &_EKF3, Flags flags) :
-    AP_AHRS_DCM(ins, baro, gps),
+AP_AHRS_NavEKF::AP_AHRS_NavEKF(AP_InertialSensor &ins,
+                               AP_Baro &baro,
+                               NavEKF2 &_EKF2,
+                               NavEKF3 &_EKF3,
+                               Flags flags) :
+    AP_AHRS_DCM(ins, baro),
     EKF2(_EKF2),
     EKF3(_EKF3),
     _ekf2_started(false),
@@ -991,7 +995,7 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const
             get_filter_status(filt_state);
 #endif
         }
-        if (hal.util->get_soft_armed() && !filt_state.flags.using_gps && _gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+        if (hal.util->get_soft_armed() && !filt_state.flags.using_gps && AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
             // if the EKF is not fusing GPS and we have a 3D lock, then
             // plane and rover would prefer to use the GPS position from
             // DCM. This is a safety net while some issues with the EKF
@@ -1009,8 +1013,8 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const
         if (!filt_state.flags.horiz_vel ||
             (!filt_state.flags.horiz_pos_abs && !filt_state.flags.horiz_pos_rel)) {
             if ((!_compass || !_compass->use_for_yaw()) &&
-                _gps.status() >= AP_GPS::GPS_OK_FIX_3D &&
-                _gps.ground_speed() < 2) {
+                AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D &&
+                AP::gps().ground_speed() < 2) {
                 /*
                   special handling for non-compass mode when sitting
                   still. The EKF may not yet have aligned its yaw. We

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -44,7 +44,7 @@ public:
     };
 
     // Constructor
-    AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps,
+    AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro,
                    NavEKF2 &_EKF2, NavEKF3 &_EKF3, Flags flags = FLAG_NONE);
 
     /* Do not allow copies */

libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp

@@ -28,7 +28,7 @@ public:
     RangeFinder sonar{serial_manager, ROTATION_PITCH_270};
     NavEKF2 EKF2{&ahrs, barometer, sonar};
     NavEKF3 EKF3{&ahrs, barometer, sonar};
-    AP_AHRS_NavEKF ahrs{ins, barometer, gps, EKF2, EKF3,
+    AP_AHRS_NavEKF ahrs{ins, barometer, EKF2, EKF3,
             AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
 };