AP_GPS: change ground course to be in degrees

more accuracy for replay

libraries/AP_GPS/AP_GPS.cpp

@@ -467,7 +467,7 @@ AP_GPS::update(void)
 void 
 AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms, 
                const Location &_location, const Vector3f &_velocity, uint8_t _num_sats, 
-               uint16_t hdop, bool _have_vertical_velocity)
+               uint16_t hdop)
 {
     if (instance >= GPS_MAX_INSTANCES) {
         return;
@@ -479,10 +479,9 @@ AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms,
     istate.location.options = 0;
     istate.velocity = _velocity;
     istate.ground_speed = pythagorous2(istate.velocity.x, istate.velocity.y);
-    istate.ground_course_cd = wrap_360_cd(degrees(atan2f(istate.velocity.y, istate.velocity.x)) * 100UL);
+    istate.ground_course = wrap_360(degrees(atan2f(istate.velocity.y, istate.velocity.x)));
     istate.hdop = hdop;
     istate.num_sats = _num_sats;
-    istate.have_vertical_velocity |= _have_vertical_velocity;
     istate.last_gps_time_ms = tnow;
     uint64_t gps_time_ms = time_epoch_ms - (17000ULL*86400ULL + 52*10*7000ULL*86400ULL - 15000ULL);
     istate.time_week     = gps_time_ms / (86400*7*(uint64_t)1000);
@@ -493,7 +492,7 @@ AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms,
 }
 
 // set accuracy for HIL
-void AP_GPS::setHIL_Accuracy(uint8_t instance, float vdop, float hacc, float vacc, float sacc)
+void AP_GPS::setHIL_Accuracy(uint8_t instance, float vdop, float hacc, float vacc, float sacc, bool _have_vertical_velocity)
 {
     GPS_State &istate = state[instance];
     istate.vdop = vdop;
@@ -503,6 +502,7 @@ void AP_GPS::setHIL_Accuracy(uint8_t instance, float vdop, float hacc, float vac
     istate.have_horizontal_accuracy = true;
     istate.have_vertical_accuracy = true;
     istate.have_speed_accuracy = true;
+    istate.have_vertical_velocity |= _have_vertical_velocity;
 }
 
 /**
@@ -574,7 +574,7 @@ AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
         get_hdop(0),
         get_vdop(0),
         ground_speed(0)*100,  // cm/s
-        ground_course_cd(0), // 1/100 degrees,
+        ground_course(0)*100, // 1/100 degrees,
         num_sats(0));
 }
 
@@ -602,7 +602,7 @@ AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
         get_hdop(1),
         get_vdop(1),
         ground_speed(1)*100,  // cm/s
-        ground_course_cd(1), // 1/100 degrees,
+        ground_course(1)*100, // 1/100 degrees,
         num_sats(1),
         0,
         0);

libraries/AP_GPS/AP_GPS.h

@@ -112,7 +112,7 @@ public:
         uint16_t time_week;                 ///< GPS week number
         Location location;                  ///< last fix location
         float ground_speed;                 ///< ground speed in m/sec
-        int32_t ground_course_cd;           ///< ground course in 100ths of a degree
+        float ground_course;                ///< ground course in degrees
         uint16_t hdop;                      ///< horizontal dilution of precision in cm
         uint16_t vdop;                      ///< vertical dilution of precision in cm
         uint8_t num_sats;                   ///< Number of visible satelites        
@@ -217,8 +217,14 @@ public:
     }
 
     // ground course in centidegrees
+    float ground_course(uint8_t instance) const {
+        return state[instance].ground_course;
+    }
+    float ground_course() const {
+        return ground_course(primary_instance);
+    }
     int32_t ground_course_cd(uint8_t instance) const {
-        return state[instance].ground_course_cd;
+        return ground_course(instance) * 100;
     }
     int32_t ground_course_cd() const {
         return ground_course_cd(primary_instance);
@@ -305,10 +311,10 @@ public:
     // set position for HIL
     void setHIL(uint8_t instance, GPS_Status status, uint64_t time_epoch_ms, 
                 const Location &location, const Vector3f &velocity, uint8_t num_sats,
-                uint16_t hdop, bool _have_vertical_velocity);
+                uint16_t hdop);
 
     // set accuracy for HIL
-    void setHIL_Accuracy(uint8_t instance, float vdop, float hacc, float vacc, float sacc);
+    void setHIL_Accuracy(uint8_t instance, float vdop, float hacc, float vacc, float sacc, bool _have_vertical_velocity);
     
     static const struct AP_Param::GroupInfo var_info[];
 

libraries/AP_GPS/AP_GPS_ERB.cpp

@@ -201,7 +201,7 @@ AP_GPS_ERB::_parse_gps(void)
         _last_vel_time         = _buffer.vel.time;
         state.ground_speed     = _buffer.vel.speed_2d * 0.01f;        // m/s
         // Heading 2D deg * 100000 rescaled to deg * 100
-        state.ground_course_cd = wrap_360_cd(_buffer.vel.heading_2d / 1000);
+        state.ground_course = wrap_360(_buffer.vel.heading_2d * 1.0e-5f);
         state.have_vertical_velocity = true;
         state.velocity.x = _buffer.vel.vel_north * 0.01f;
         state.velocity.y = _buffer.vel.vel_east * 0.01f;

libraries/AP_GPS/AP_GPS_GSOF.cpp

@@ -310,7 +310,7 @@ AP_GPS_GSOF::process_message(void)
                 if ((vflag & 1) == 1)
                 {
                     state.ground_speed = SwapFloat(gsof_msg.data, a + 1);
-                    state.ground_course_cd = (int32_t)(ToDeg(SwapFloat(gsof_msg.data, a + 5)) * 100);
+                    state.ground_course = degrees(SwapFloat(gsof_msg.data, a + 5));
                     fill_3d_velocity();
                     state.velocity.z = -SwapFloat(gsof_msg.data, a + 9);
                     state.have_vertical_velocity = true;

libraries/AP_GPS/AP_GPS_MTK.cpp

@@ -147,7 +147,7 @@ restart:
             state.location.lng  = swap_int32(_buffer.msg.longitude) * 10;
             state.location.alt  = swap_int32(_buffer.msg.altitude);
             state.ground_speed      = swap_int32(_buffer.msg.ground_speed) * 0.01f;
-            state.ground_course_cd  = wrap_360_cd(swap_int32(_buffer.msg.ground_course) / 10000);
+            state.ground_course     = wrap_360(swap_int32(_buffer.msg.ground_course) * 1.0e-6f);
             state.num_sats          = _buffer.msg.satellites;
 
             if (state.status >= AP_GPS::GPS_OK_FIX_2D) {

libraries/AP_GPS/AP_GPS_MTK19.cpp

@@ -145,7 +145,7 @@ restart:
 			}
             state.location.alt      = _buffer.msg.altitude;
             state.ground_speed      = _buffer.msg.ground_speed*0.01f;
-            state.ground_course_cd  = wrap_360_cd(_buffer.msg.ground_course);
+            state.ground_course     = wrap_360(_buffer.msg.ground_course*0.01f);
             state.num_sats          = _buffer.msg.satellites;
             state.hdop              = _buffer.msg.hdop;
             

libraries/AP_GPS/AP_GPS_NMEA.cpp

@@ -291,7 +291,7 @@ bool AP_GPS_NMEA::_term_complete()
                     state.location.lat     = _new_latitude;
                     state.location.lng     = _new_longitude;
                     state.ground_speed     = _new_speed*0.01f;
-                    state.ground_course_cd = wrap_360_cd(_new_course);
+                    state.ground_course    = wrap_360(_new_course*0.01f);
                     make_gps_time(_new_date, _new_time * 10);
                     state.last_gps_time_ms = now;
                     // To-Do: add support for proper reporting of 2D and 3D fix
@@ -310,8 +310,8 @@ bool AP_GPS_NMEA::_term_complete()
                     break;
                 case _GPS_SENTENCE_VTG:
                     _last_VTG_ms = now;
-                    state.ground_speed     = _new_speed*0.01f;
+                    state.ground_speed  = _new_speed*0.01f;
-                    state.ground_course_cd = wrap_360_cd(_new_course);
+                    state.ground_course = wrap_360(_new_course*0.01f);
                     fill_3d_velocity();
                     // VTG has no fix indicator, can't change fix status
                     break;

libraries/AP_GPS/AP_GPS_PX4.cpp

@@ -63,7 +63,7 @@ AP_GPS_PX4::read(void)
                 state.location.alt = _gps_pos.alt/10;
 
                 state.ground_speed = _gps_pos.vel_m_s;
-                state.ground_course_cd = wrap_360_cd(degrees(_gps_pos.cog_rad)*100);
+                state.ground_course = wrap_360(degrees(_gps_pos.cog_rad));
                 state.hdop = _gps_pos.eph*100;
 
                 // convert epoch timestamp back to gps epoch - evil hack until we get the genuine

libraries/AP_GPS/AP_GPS_QURT.cpp

@@ -88,7 +88,7 @@ AP_GPS_QURT::read(void)
     state.location.alt = data.alt_from_MSL;
 
     state.ground_speed = data.speed_over_ground;
-    state.ground_course_cd = data.course_over_ground;
+    state.ground_course = data.course_over_ground*0.01f;
 
     // convert epoch timestamp back to gps epoch - evil hack until we get the genuine
     // raw week information (or APM switches to Posix epoch ;-) )
@@ -101,7 +101,7 @@ AP_GPS_QURT::read(void)
     }
     
     state.have_vertical_velocity = true;
-    float gps_heading = radians(state.ground_course_cd * 0.01f);
+    float gps_heading = radians(state.ground_course);
     state.velocity.x = state.ground_speed * cosf(gps_heading);
     state.velocity.y = state.ground_speed * sinf(gps_heading);
     state.velocity.z = -data.climb_rate;

libraries/AP_GPS/AP_GPS_SBF.cpp

@@ -226,8 +226,7 @@ AP_GPS_SBF::process_message(void)
             float ground_vector_sq = state.velocity[0] * state.velocity[0] + state.velocity[1] * state.velocity[1];
             state.ground_speed = (float)safe_sqrt(ground_vector_sq);
 
-            state.ground_course_cd = (int32_t)(100 * ToDeg(atan2f(state.velocity[1], state.velocity[0])));
+            state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
-            state.ground_course_cd = wrap_360_cd(state.ground_course_cd);
 
             state.horizontal_accuracy = (float)temp.HAccuracy * 0.01f;
             state.vertical_accuracy = (float)temp.VAccuracy * 0.01f;

libraries/AP_GPS/AP_GPS_SBP.cpp

@@ -272,7 +272,7 @@ AP_GPS_SBP::_attempt_state_update()
         float ground_vector_sq = state.velocity[0]*state.velocity[0] + state.velocity[1]*state.velocity[1];
         state.ground_speed = safe_sqrt(ground_vector_sq);
 
-        state.ground_course_cd = wrap_360_cd((int32_t) 100*ToDeg(atan2f(state.velocity[1], state.velocity[0])));
+        state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
 
         // Update position state
 

libraries/AP_GPS/AP_GPS_SIRF.cpp

@@ -186,7 +186,7 @@ AP_GPS_SIRF::_parse_gps(void)
         state.location.lng      = swap_int32(_buffer.nav.longitude);
         state.location.alt      = swap_int32(_buffer.nav.altitude_msl);
         state.ground_speed      = swap_int32(_buffer.nav.ground_speed)*0.01f;
-        state.ground_course_cd  = wrap_360_cd(swap_int16(_buffer.nav.ground_course));
+        state.ground_course     = wrap_360(swap_int16(_buffer.nav.ground_course)*0.01f);
         state.num_sats          = _buffer.nav.satellites;
         fill_3d_velocity();
         return true;

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -923,7 +923,7 @@ AP_GPS_UBLOX::_parse_gps(void)
         Debug("MSG_VELNED");
         _last_vel_time         = _buffer.velned.time;
         state.ground_speed     = _buffer.velned.speed_2d*0.01f;          // m/s
-        state.ground_course_cd = wrap_360_cd(_buffer.velned.heading_2d / 1000);       // Heading 2D deg * 100000 rescaled to deg * 100
+        state.ground_course    = wrap_360(_buffer.velned.heading_2d * 1.0e-5f);       // Heading 2D deg * 100000
         state.have_vertical_velocity = true;
         state.velocity.x = _buffer.velned.ned_north * 0.01f;
         state.velocity.y = _buffer.velned.ned_east * 0.01f;

libraries/AP_GPS/GPS_Backend.cpp

@@ -131,7 +131,7 @@ void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds)
  */
 void AP_GPS_Backend::fill_3d_velocity(void)
 {
-    float gps_heading = ToRad(state.ground_course_cd * 0.01f);
+    float gps_heading = radians(state.ground_course);
 
     state.velocity.x = state.ground_speed * cosf(gps_heading);
     state.velocity.y = state.ground_speed * sinf(gps_heading);