From 618f43bef65c5e563a027087c9563aa0c22e613c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 11 Aug 2012 11:59:47 +1000 Subject: [PATCH] AP_GPS: added last_ground_speed() method used for dead-reckoning in AHRS --- libraries/AP_GPS/GPS.cpp | 4 ++-- libraries/AP_GPS/GPS.h | 7 ++++++- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp index 6ce84a0223..0b6cd77a26 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -54,7 +54,7 @@ GPS::update(void) if (_status == GPS_OK) { // update our acceleration float deltat = 1.0e-3 * (_idleTimer - last_fix_time); - float deltav = 1.0e-2 * ((float)ground_speed - (float)_last_ground_speed); + float deltav = 1.0e-2 * ((float)ground_speed - (float)_last_ground_speed_cm); float gps_heading = ToRad(ground_course * 0.01); float gps_speed = ground_speed * 0.01; float sin_heading, cos_heading; @@ -63,7 +63,7 @@ GPS::update(void) sin_heading = sin(gps_heading); last_fix_time = _idleTimer; - _last_ground_speed = ground_speed; + _last_ground_speed_cm = ground_speed; _velocity_north = gps_speed * cos_heading; _velocity_east = gps_speed * sin_heading; diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index 7c3438045e..f5f9400b0a 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -134,6 +134,11 @@ public: float velocity_north(void) { return _status == GPS_OK? _velocity_north : 0; } float velocity_east(void) { return _status == GPS_OK? _velocity_east : 0; } + // last ground speed in m/s. This can be used when we have no GPS + // lock to return the last ground speed we had with lock + float last_ground_speed(void) { return _last_ground_speed_cm * 0.01; } + + // the time we got our last fix in system milliseconds uint32_t last_fix_time; @@ -202,7 +207,7 @@ private: GPS_Status _status; // previous ground speed in cm/s - uint32_t _last_ground_speed; + uint32_t _last_ground_speed_cm; // smoothed estimate of our acceleration, in m/s/s float _acceleration;