mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 18:08:30 -04:00
GPS: added velocity and acceleration components
used by AHRS for acceleration correction
This commit is contained in:
parent
fe865bc1dc
commit
37db5247cd
@ -11,6 +11,8 @@
|
||||
# define Debug(fmt, args...)
|
||||
#endif
|
||||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
#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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user