GPS: support getting raw velocity values from a UBlox

this may give better velocity numbers than the value from COG
This commit is contained in:
Andrew Tridgell 2012-08-14 18:32:46 +10:00
parent a1107602e9
commit 79aae2114c
4 changed files with 28 additions and 38 deletions

View File

@ -242,6 +242,10 @@ AP_GPS_UBLOX::_parse_gps(void)
speed_3d = _buffer.velned.speed_3d; // cm/s
ground_speed = _buffer.velned.speed_2d; // cm/s
ground_course = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
_have_raw_velocity = true;
_vel_north = _buffer.velned.ned_north;
_vel_east = _buffer.velned.ned_east;
_vel_down = _buffer.velned.ned_down;
_new_speed = true;
break;
default:

View File

@ -52,9 +52,14 @@ GPS::update(void)
_idleTimer = tnow;
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_cm);
last_fix_time = _idleTimer;
_last_ground_speed_cm = ground_speed;
if (_have_raw_velocity) {
// the GPS is able to give us velocity numbers directly
_velocity_north = _vel_north * 0.01;
_velocity_east = _vel_east * 0.01;
} else {
float gps_heading = ToRad(ground_course * 0.01);
float gps_speed = ground_speed * 0.01;
float sin_heading, cos_heading;
@ -62,26 +67,8 @@ GPS::update(void)
cos_heading = cos(gps_heading);
sin_heading = sin(gps_heading);
last_fix_time = _idleTimer;
_last_ground_speed_cm = 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;
}
}
}

View File

@ -123,13 +123,6 @@ public:
///
uint32_t idleTimeout;
// 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; }
@ -196,6 +189,14 @@ protected:
void _write_progstr_block(Stream *_fs, const prog_char *pstr, uint8_t size);
// velocities in cm/s if available from the GPS
int32_t _vel_north;
int32_t _vel_east;
int32_t _vel_down;
// does this GPS support raw velocity numbers?
bool _have_raw_velocity;
private:
@ -209,15 +210,9 @@ private:
// previous ground speed in cm/s
uint32_t _last_ground_speed_cm;
// 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

View File

@ -45,6 +45,10 @@ void loop()
Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:");
Serial.print(gps.ground_course / 100.0, DEC);
Serial.printf(" VEL: %.2f %.2f %.2f",
gps.velocity_north(),
gps.velocity_east(),
sqrt(sq(gps.velocity_north())+sq(gps.velocity_east())));
Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:");