From c044385fff904861bf42483eb802a125979c266c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 10:17:45 +1000 Subject: [PATCH] AP_AHRS: added groundspeed() method this gives ground speed in m/s from GPS --- libraries/AP_AHRS/AP_AHRS.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index d5284bd7cd..d034288289 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -191,6 +191,14 @@ public: // return a ground vector estimate in meters/second, in North/East order Vector2f groundspeed_vector(void); + // return ground speed estimate in meters/second. Used by ground vehicles. + float groundspeed(void) const { + if (!_gps || _gps->status() <= GPS::NO_FIX) { + return 0.0f; + } + return _gps->ground_speed_cm * 0.01f; + } + // return true if we will use compass for yaw virtual bool use_compass(void) const { return _compass && _compass->use_for_yaw(); }