From e15442355a5e901d37b619fec80ee47b95eb8aa2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 2 Dec 2017 10:50:46 +1100 Subject: [PATCH] AP_AHRS: use GPS singleton --- libraries/AP_AHRS/AP_AHRS.cpp | 10 +++++----- libraries/AP_AHRS/AP_AHRS.h | 8 +------- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 17 +++++++++++------ libraries/AP_AHRS/AP_AHRS_DCM.h | 4 ++-- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 14 +++++++++----- libraries/AP_AHRS/AP_AHRS_NavEKF.h | 2 +- .../AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp | 2 +- 7 files changed, 30 insertions(+), 27 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 284bb4b31b..003769d5a0 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/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) { diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index f73385f242..3ab4a31bd5 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/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; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index fc54daead4..8a25dba490 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/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, diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 2c8bc2dd0f..2200423ec6 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/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) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index e10cb9892e..15ac030481 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/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 diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 6b0a455b60..5dd0a2dacd 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/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 */ diff --git a/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp b/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp index 1b81f71266..2f81a72f65 100644 --- a/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp +++ b/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}; };