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

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

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;
+			if (_have_raw_velocity) {
-			_velocity_east  = gps_speed * sin_heading;
+				// the GPS is able to give us velocity numbers directly
-
+				_velocity_north = _vel_north * 0.01;
-			if (deltat > 2.0 || deltat == 0) {
+				_velocity_east  = _vel_east * 0.01;
-				// 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
+				float gps_heading = ToRad(ground_course * 0.01);
-				_acceleration = (0.7 * _acceleration) + (0.3 * (deltav/deltat));
+				float gps_speed   = ground_speed * 0.01;
+				float sin_heading, cos_heading;
 
-				// calculate the components, to save time in the AHRS code
+				cos_heading = cos(gps_heading);
-				_acceleration_north = _acceleration * cos_heading;
+				sin_heading = sin(gps_heading);
-				_acceleration_east  = _acceleration * sin_heading;
+
+				_velocity_north = gps_speed * cos_heading;
+				_velocity_east  = gps_speed * sin_heading;
 			}
 		}
     }

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

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:");