From 79aae2114c43f1fdc44ebed78fd500816fa2cdec Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 14 Aug 2012 18:32:46 +1000 Subject: [PATCH] GPS: support getting raw velocity values from a UBlox this may give better velocity numbers than the value from COG --- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 4 ++ libraries/AP_GPS/GPS.cpp | 37 ++++++------------- libraries/AP_GPS/GPS.h | 21 ++++------- .../GPS_UBLOX_test/GPS_UBLOX_test.pde | 4 ++ 4 files changed, 28 insertions(+), 38 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index e93edde1c1..82734dae4a 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -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: diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp index 0b6cd77a26..12502d18c2 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -52,36 +52,23 @@ 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); - 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_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; + 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 { - // calculate a mildly smoothed acceleration value - _acceleration = (0.7 * _acceleration) + (0.3 * (deltav/deltat)); + float gps_heading = ToRad(ground_course * 0.01); + float gps_speed = ground_speed * 0.01; + float sin_heading, cos_heading; - // calculate the components, to save time in the AHRS code - _acceleration_north = _acceleration * cos_heading; - _acceleration_east = _acceleration * sin_heading; + cos_heading = cos(gps_heading); + sin_heading = sin(gps_heading); + + _velocity_north = gps_speed * cos_heading; + _velocity_east = gps_speed * sin_heading; } } } diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index f5f9400b0a..42a6627af0 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -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 diff --git a/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde b/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde index 9b56e8843d..f180a90b63 100644 --- a/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde +++ b/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde @@ -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:");