diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp index 87c4c8f2e3..6fcde0429a 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -33,6 +33,24 @@ GPS::update(void) // reset the idle timer _idleTimer = millis(); + + 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); + _last_fix_time = _idleTimer; + _last_ground_speed = ground_speed; + + if (deltat > 2.0 || deltat == 0) { + // we didn't get a fix for 2 seconds - set + // acceleration to zero, as the estimate will be too + // far out + _acceleration = 0; + } else { + // calculate a mildly smoothed acceleration value + _acceleration = (0.7 * _acceleration) + (0.3 * (deltav/deltat)); + } + } } } diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index 397fbfad5b..9889f96a02 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -108,7 +108,10 @@ public: /// 1200ms allows a small amount of slack over the worst-case 1Hz update /// rate. /// - unsigned long idleTimeout; + uint32_t idleTimeout; + + // our approximate linear acceleration in m/s/s + float acceleration(void) { return _acceleration; } protected: Stream *_port; ///< port the GPS is attached to @@ -165,11 +168,19 @@ private: /// Last time that the GPS driver got a good packet from the GPS /// - unsigned long _idleTimer; + uint32_t _idleTimer; /// Our current status GPS_Status _status; + // the time we got our last fix in system milliseconds + uint32_t _last_fix_time; + + // previous ground speed in cm/s + uint32_t _last_ground_speed; + + // smoothed estimate of our acceleration + float _acceleration; }; inline long