Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialNav: convert to new GPS API

Browse Source
This commit is contained in:
Andrew Tridgell 2014-03-31 18:08:02 +11:00
parent 5a2e84e792
commit d04d33a02d
3 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_InertialNav/AP_InertialNav.cpp
View File

@ -130,13 +130,14 @@ void AP_InertialNav::check_gps()
const uint32_t now = hal.scheduler->millis(); const uint32_t now = hal.scheduler->millis();
// compare gps time to previous reading // 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 // 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 // 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{ }else{
// if GPS updates stop arriving degrade position error to 10% over 2 seconds (assumes 100hz update rate) // 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) { 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 // reset the inertial nav position and velocity to gps values
if (_flags.gps_glitching) { if (_flags.gps_glitching) {
set_position_xy(x,y); 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.x = 0;
_position_error.y = 0; _position_error.y = 0;
}else{ }else{

4
libraries/AP_InertialNav/AP_InertialNav.h
View File

@ -34,10 +34,9 @@ class AP_InertialNav
public: public:
// Constructor // 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), _ahrs(ahrs),
_baro(baro), _baro(baro),
_gps(gps),
_xy_enabled(false), _xy_enabled(false),
_gps_last_update(0), _gps_last_update(0),
_gps_last_time(0), _gps_last_time(0),
@ -274,7 +273,6 @@ protected:
AP_AHRS &_ahrs; // reference to ahrs object AP_AHRS &_ahrs; // reference to ahrs object
AP_Baro &_baro; // reference to barometer AP_Baro &_baro; // reference to barometer
GPS*& _gps; // pointer to gps
// XY Axis specific variables // XY Axis specific variables
bool _xy_enabled; // xy position estimates enabled bool _xy_enabled; // xy position estimates enabled

4
libraries/AP_InertialNav/AP_InertialNav_NavEKF.h
View File

@ -14,8 +14,8 @@ class AP_InertialNav_NavEKF : public AP_InertialNav
{ {
public: public:
// Constructor // Constructor
AP_InertialNav_NavEKF(AP_AHRS &ahrs, AP_Baro &baro, GPS*& gps, GPS_Glitch& gps_glitch ) : AP_InertialNav_NavEKF(AP_AHRS &ahrs, AP_Baro &baro, GPS_Glitch& gps_glitch ) :
AP_InertialNav(ahrs, baro, gps, gps_glitch) AP_InertialNav(ahrs, baro, gps_glitch)
{ {
} }