From d04d33a02ddadd18b6b47e95852fbf3c2859d819 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 31 Mar 2014 18:08:02 +1100 Subject: [PATCH] AP_InertialNav: convert to new GPS API --- libraries/AP_InertialNav/AP_InertialNav.cpp | 10 ++++++---- libraries/AP_InertialNav/AP_InertialNav.h | 4 +--- libraries/AP_InertialNav/AP_InertialNav_NavEKF.h | 4 ++-- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp index b045ae2ec7..72dc5fb22c 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav.cpp @@ -130,13 +130,14 @@ void AP_InertialNav::check_gps() const uint32_t now = hal.scheduler->millis(); // compare gps time to previous reading - if( _gps != NULL && _gps->last_fix_time != _gps_last_time ) { + const AP_GPS &gps = _ahrs.get_gps(); + if(gps.last_fix_time_ms() != _gps_last_time ) { // call position correction method - correct_with_gps(now, _gps->longitude, _gps->latitude); + correct_with_gps(now, gps.location().lng, gps.location().lat); // record gps time and system time of this update - _gps_last_time = _gps->last_fix_time; + _gps_last_time = gps.last_fix_time_ms(); }else{ // if GPS updates stop arriving degrade position error to 10% over 2 seconds (assumes 100hz update rate) if (now - _gps_last_update > AP_INTERTIALNAV_GPS_TIMEOUT_MS) { @@ -181,7 +182,8 @@ void AP_InertialNav::correct_with_gps(uint32_t now, int32_t lon, int32_t lat) // reset the inertial nav position and velocity to gps values if (_flags.gps_glitching) { set_position_xy(x,y); - set_velocity_xy(_gps->velocity_north() * 100.0f,_gps->velocity_east() * 100.0f); + set_velocity_xy(_ahrs.get_gps().velocity().x * 100.0f, + _ahrs.get_gps().velocity().y * 100.0f); _position_error.x = 0; _position_error.y = 0; }else{ diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h index 748b9d23b8..83112a6fa0 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.h +++ b/libraries/AP_InertialNav/AP_InertialNav.h @@ -34,10 +34,9 @@ class AP_InertialNav public: // Constructor - AP_InertialNav(AP_AHRS &ahrs, AP_Baro &baro, GPS*& gps, GPS_Glitch& gps_glitch ) : + AP_InertialNav(AP_AHRS &ahrs, AP_Baro &baro, GPS_Glitch& gps_glitch ) : _ahrs(ahrs), _baro(baro), - _gps(gps), _xy_enabled(false), _gps_last_update(0), _gps_last_time(0), @@ -274,7 +273,6 @@ protected: AP_AHRS &_ahrs; // reference to ahrs object AP_Baro &_baro; // reference to barometer - GPS*& _gps; // pointer to gps // XY Axis specific variables bool _xy_enabled; // xy position estimates enabled diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h index d37987c28d..2b940b3b9f 100644 --- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h +++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h @@ -14,8 +14,8 @@ class AP_InertialNav_NavEKF : public AP_InertialNav { public: // Constructor - AP_InertialNav_NavEKF(AP_AHRS &ahrs, AP_Baro &baro, GPS*& gps, GPS_Glitch& gps_glitch ) : - AP_InertialNav(ahrs, baro, gps, gps_glitch) + AP_InertialNav_NavEKF(AP_AHRS &ahrs, AP_Baro &baro, GPS_Glitch& gps_glitch ) : + AP_InertialNav(ahrs, baro, gps_glitch) { }