Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: added last_ground_speed() method 

used for dead-reckoning in AHRS
This commit is contained in:
Andrew Tridgell 2012-08-11 11:59:47 +10:00
parent 8a6fcf998b
commit 618f43bef6
2 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_GPS/GPS.cpp
View File

@ -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;

7
libraries/AP_GPS/GPS.h
View File

@ -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;