AP_GPS: added a common velocity_to_speed_course()

this saves flash using common code

libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp

@@ -62,8 +62,7 @@ void AP_GPS_ExternalAHRS::handle_external(const AP_ExternalAHRS::gps_data_messag
     state.velocity.y = pkt.ned_vel_east;
     state.velocity.z = pkt.ned_vel_down;
 
-    state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x)));
+    velocity_to_speed_course(state);
-    state.ground_speed = state.velocity.xy().length();
 
     state.have_speed_accuracy = true;
     state.have_horizontal_accuracy = true;

libraries/AP_GPS/AP_GPS_MAV.cpp

@@ -82,8 +82,7 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg)
                 }
 
                 state.velocity = vel;
-                state.ground_course = wrap_360(degrees(atan2f(vel.y, vel.x)));
+                velocity_to_speed_course(state);
-                state.ground_speed = vel.xy().length();
             }
 
             if (have_sa) {

libraries/AP_GPS/AP_GPS_MSP.cpp

@@ -60,8 +60,7 @@ void AP_GPS_MSP::handle_msp(const MSP::msp_gps_data_message_t &pkt)
     vel.z = pkt.ned_vel_down * 0.01;
     state.velocity = vel;
 
-    state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x)));
+    velocity_to_speed_course(state);
-    state.ground_speed = state.velocity.xy().length();
 
     state.have_speed_accuracy = true;
     state.have_horizontal_accuracy = true;

libraries/AP_GPS/AP_GPS_NMEA.cpp

@@ -399,8 +399,7 @@ bool AP_GPS_NMEA::_term_complete()
                     state.have_vertical_velocity = true;
                     _last_vvelocity_ms = now;
                     // we prefer a true 3D velocity when available
-                    state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x)));
+                    velocity_to_speed_course(state);
-                    state.ground_speed = state.velocity.xy().length();
                     _last_3D_velocity_ms = now;
                 } else if (_phd.msg_id == 26) {
                     state.horizontal_accuracy = MAX(_phd.fields[0],_phd.fields[1]) * 0.001;
@@ -430,8 +429,7 @@ bool AP_GPS_NMEA::_term_complete()
                     state.have_vertical_velocity = true;
                     _last_vvelocity_ms = now;
                     // we prefer a true 3D velocity when available
-                    state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x)));
+                    velocity_to_speed_course(state);
-                    state.ground_speed = state.velocity.xy().length();
                     _last_3D_velocity_ms = now;
                 }
                 if (is_equal(3.0f, float(_ksxt.fields[10]))) {
@@ -455,6 +453,7 @@ bool AP_GPS_NMEA::_term_complete()
                 state.location.alt = ag.alt*1.0e2;
                 state.undulation   = -ag.undulation;
                 state.velocity = ag.vel_NED;
+                velocity_to_speed_course(state);
                 state.speed_accuracy = ag.vel_stddev.length();
                 state.horizontal_accuracy = ag.pos_stddev.xy().length();
                 state.vertical_accuracy = ag.pos_stddev.z;

libraries/AP_GPS/AP_GPS_SBF.cpp

@@ -401,10 +401,7 @@ AP_GPS_SBF::process_message(void)
 
             state.have_vertical_velocity = true;
 
-            float ground_vector_sq = state.velocity[0] * state.velocity[0] + state.velocity[1] * state.velocity[1];
+            velocity_to_speed_course(state);
-            state.ground_speed = (float)safe_sqrt(ground_vector_sq);
-
-            state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
             state.rtk_age_ms = temp.MeanCorrAge * 10;
 
             // value is expressed as twice the rms error = int16 * 0.01/2

libraries/AP_GPS/AP_GPS_SBP.cpp

@@ -273,10 +273,7 @@ AP_GPS_SBP::_attempt_state_update()
         state.velocity[2]       = (float)(last_vel_ned.d * 1.0e-3);
         state.have_vertical_velocity = true;
 
-        float ground_vector_sq = state.velocity[0]*state.velocity[0] + state.velocity[1]*state.velocity[1];
+        velocity_to_speed_course(state);
-        state.ground_speed = safe_sqrt(ground_vector_sq);
-
-        state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
 
         // Update position state
 

libraries/AP_GPS/AP_GPS_SBP2.cpp

@@ -312,10 +312,7 @@ AP_GPS_SBP2::_attempt_state_update()
         state.velocity[1]       = (float)(last_vel_ned.e * 1.0e-3);
         state.velocity[2]       = (float)(last_vel_ned.d * 1.0e-3);
 
-        float ground_vector_sq = state.velocity[0]*state.velocity[0] + state.velocity[1]*state.velocity[1];
+        velocity_to_speed_course(state);
-        state.ground_speed = safe_sqrt(ground_vector_sq);
-
-        state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
 
         state.speed_accuracy        = safe_sqrt(
                                         powf((float)last_vel_ned.h_accuracy * 1.0e-3f, 2) + 

libraries/AP_GPS/AP_GPS_SITL.cpp

@@ -100,8 +100,7 @@ bool AP_GPS_SITL::read(void)
     state.velocity.y = speedE;
     state.velocity.z = speedD;
 
-    state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x)));
+    velocity_to_speed_course(state);
-    state.ground_speed = state.velocity.xy().length();
 
     state.have_speed_accuracy = true;
     state.have_horizontal_accuracy = true;

libraries/AP_GPS/AP_GPS_UAVCAN.cpp

@@ -377,8 +377,7 @@ void AP_GPS_UAVCAN::handle_velocity(const float vx, const float vy, const float
     if (!uavcan::isNaN(vx)) {
         const Vector3f vel(vx, vy, vz);
         interim_state.velocity = vel;
-        interim_state.ground_speed = vel.xy().length();
+        velocity_to_speed_course(interim_state);
-        interim_state.ground_course = wrap_360(degrees(atan2f(vel.y, vel.x)));
         // assume we have vertical velocity if we ever get a non-zero Z velocity
         if (!isnanf(vel.z) && !is_zero(vel.z)) {
             interim_state.have_vertical_velocity = true;

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -1587,8 +1587,7 @@ AP_GPS_UBLOX::_parse_gps(void)
         state.velocity.x = _buffer.velned.ned_north * 0.01f;
         state.velocity.y = _buffer.velned.ned_east * 0.01f;
         state.velocity.z = _buffer.velned.ned_down * 0.01f;
-        state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x)));
+        velocity_to_speed_course(state);
-        state.ground_speed = state.velocity.xy().length();
         state.have_speed_accuracy = true;
         state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f;
 #if UBLOX_FAKE_3DLOCK

libraries/AP_GPS/GPS_Backend.cpp

@@ -133,6 +133,15 @@ void AP_GPS_Backend::fill_3d_velocity(void)
     state.have_vertical_velocity = false;
 }
 
+/*
+  fill in 3D velocity for a GPS that doesn't give vertical velocity numbers
+ */
+void AP_GPS_Backend::velocity_to_speed_course(AP_GPS::GPS_State &s)
+{
+    s.ground_course = wrap_360(degrees(atan2f(s.velocity.y, s.velocity.x)));
+    s.ground_speed = s.velocity.xy().length();
+}
+
 void
 AP_GPS_Backend::inject_data(const uint8_t *data, uint16_t len)
 {

libraries/AP_GPS/GPS_Backend.h

@@ -117,6 +117,11 @@ protected:
      */
     void fill_3d_velocity(void);
 
+    /*
+      fill ground course and speed from velocity
+     */
+    void velocity_to_speed_course(AP_GPS::GPS_State &s);
+
     /*
        fill in time_week_ms and time_week from BCD date and time components
        assumes MTK19 millisecond form of bcd_time