From 37db5247cdb154dea910ab054a88789ecffedc4a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 23 Mar 2012 16:23:23 +1100 Subject: [PATCH] GPS: added velocity and acceleration components used by AHRS for acceleration correction --- libraries/AP_GPS/GPS.cpp | 18 ++++++++++++++++++ libraries/AP_GPS/GPS.h | 17 ++++++++++++++++- 2 files changed, 34 insertions(+), 1 deletion(-) diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp index ea7e336153..6ce84a0223 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -11,6 +11,8 @@ # define Debug(fmt, args...) #endif +#include +#include #include "GPS.h" #if defined(ARDUINO) && ARDUINO >= 100 #include "Arduino.h" @@ -53,17 +55,33 @@ GPS::update(void) // 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 gps_heading = ToRad(ground_course * 0.01); + float gps_speed = ground_speed * 0.01; + float sin_heading, cos_heading; + + cos_heading = cos(gps_heading); + sin_heading = sin(gps_heading); + last_fix_time = _idleTimer; _last_ground_speed = ground_speed; + _velocity_north = gps_speed * cos_heading; + _velocity_east = gps_speed * sin_heading; + 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; + _acceleration_north = 0; + _acceleration_east = 0; } else { // calculate a mildly smoothed acceleration value _acceleration = (0.7 * _acceleration) + (0.3 * (deltav/deltat)); + + // calculate the components, to save time in the AHRS code + _acceleration_north = _acceleration * cos_heading; + _acceleration_east = _acceleration * sin_heading; } } } diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index 7cadcb8400..3d739c0017 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -126,6 +126,14 @@ public: // our approximate linear acceleration in m/s/s float acceleration(void) { return _acceleration; } + // components of acceleration in 2D, in m/s/s + float acceleration_north(void) { return _status == GPS_OK? _acceleration_north : 0; } + float acceleration_east(void) { return _status == GPS_OK? _acceleration_east : 0; } + + // components of velocity in 2D, in m/s + float velocity_north(void) { return _status == GPS_OK? _velocity_north : 0; } + float velocity_east(void) { return _status == GPS_OK? _velocity_east : 0; } + // the time we got our last fix in system milliseconds uint32_t last_fix_time; @@ -196,8 +204,15 @@ private: // previous ground speed in cm/s uint32_t _last_ground_speed; - // smoothed estimate of our acceleration + // smoothed estimate of our acceleration, in m/s/s float _acceleration; + float _acceleration_north; + float _acceleration_east; + + // components of the velocity, in m/s + float _velocity_north; + float _velocity_east; + }; inline long