From 5d2bfab23ce71f3cf2dd4c68a3d8a224318bb2eb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 31 Oct 2012 13:56:04 +1100 Subject: [PATCH] AP_GPS: added velocity_down() call on GPSes with raw velocity support this will be used for DCM correction --- libraries/AP_GPS/GPS.cpp | 4 ++++ libraries/AP_GPS/GPS.h | 7 +++++++ 2 files changed, 11 insertions(+) diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp index d5dfbcdf23..3aa9560122 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -59,6 +59,7 @@ GPS::update(void) // the GPS is able to give us velocity numbers directly _velocity_north = _vel_north * 0.01; _velocity_east = _vel_east * 0.01; + _velocity_down = _vel_down * 0.01; } else { float gps_heading = ToRad(ground_course * 0.01); float gps_speed = ground_speed * 0.01; @@ -69,6 +70,9 @@ GPS::update(void) _velocity_north = gps_speed * cos_heading; _velocity_east = gps_speed * sin_heading; + + // no good way to get descent rate + _velocity_down = 0; } } } diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index 2a1d50bdf9..21b1efdb22 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -128,6 +128,9 @@ public: float velocity_east(void) { return _status == GPS_OK ? _velocity_east : 0; } + float velocity_down(void) { + return _status == GPS_OK ? _velocity_down : 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 @@ -141,6 +144,9 @@ public: // the time we got our last fix in system milliseconds uint32_t last_fix_time; + // return true if the GPS supports raw velocity values + + protected: Stream *_port; ///< port the GPS is attached to @@ -222,6 +228,7 @@ private: // components of the velocity, in m/s float _velocity_north; float _velocity_east; + float _velocity_down; }; inline int32_t