mirror of https://github.com/ArduPilot/ardupilot
GPS: added an acceleration estimate to the GPS driver
This uses the GPS ground speed to estimate acceleration
This commit is contained in:
parent
7fbfcc2721
commit
57f67ca6e3
|
@ -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));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue