From b424c49bc7933f962ce5b4b1928789dc942e8f38 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 May 2016 11:28:35 +1000 Subject: [PATCH] AP_GPS: change ground course to be in degrees more accuracy for replay --- libraries/AP_GPS/AP_GPS.cpp | 12 ++++++------ libraries/AP_GPS/AP_GPS.h | 14 ++++++++++---- libraries/AP_GPS/AP_GPS_ERB.cpp | 2 +- libraries/AP_GPS/AP_GPS_GSOF.cpp | 2 +- libraries/AP_GPS/AP_GPS_MTK.cpp | 2 +- libraries/AP_GPS/AP_GPS_MTK19.cpp | 2 +- libraries/AP_GPS/AP_GPS_NMEA.cpp | 6 +++--- libraries/AP_GPS/AP_GPS_PX4.cpp | 2 +- libraries/AP_GPS/AP_GPS_QURT.cpp | 4 ++-- libraries/AP_GPS/AP_GPS_SBF.cpp | 3 +-- libraries/AP_GPS/AP_GPS_SBP.cpp | 2 +- libraries/AP_GPS/AP_GPS_SIRF.cpp | 2 +- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 2 +- libraries/AP_GPS/GPS_Backend.cpp | 2 +- 14 files changed, 31 insertions(+), 26 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 5f8bc7d457..ab870fad87 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/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); diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index ea2f4449cf..ec1046a284 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/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[]; diff --git a/libraries/AP_GPS/AP_GPS_ERB.cpp b/libraries/AP_GPS/AP_GPS_ERB.cpp index 322890e3c8..e1e55c5db2 100644 --- a/libraries/AP_GPS/AP_GPS_ERB.cpp +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/libraries/AP_GPS/AP_GPS_GSOF.cpp index a558c8d564..609205a3f6 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.cpp +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp index 61ecd978ca..0822777708 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.cpp +++ b/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) { diff --git a/libraries/AP_GPS/AP_GPS_MTK19.cpp b/libraries/AP_GPS/AP_GPS_MTK19.cpp index de3e05e9d5..645e068f8f 100644 --- a/libraries/AP_GPS/AP_GPS_MTK19.cpp +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index c41f209c03..dacbae8982 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/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_course_cd = wrap_360_cd(_new_course); + state.ground_speed = _new_speed*0.01f; + state.ground_course = wrap_360(_new_course*0.01f); fill_3d_velocity(); // VTG has no fix indicator, can't change fix status break; diff --git a/libraries/AP_GPS/AP_GPS_PX4.cpp b/libraries/AP_GPS/AP_GPS_PX4.cpp index 6d98096727..fc59bf224d 100644 --- a/libraries/AP_GPS/AP_GPS_PX4.cpp +++ b/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 diff --git a/libraries/AP_GPS/AP_GPS_QURT.cpp b/libraries/AP_GPS/AP_GPS_QURT.cpp index 56c2b0a775..980737b41c 100644 --- a/libraries/AP_GPS/AP_GPS_QURT.cpp +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 54ff0ecafc..e0e60e63cf 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/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_cd = wrap_360_cd(state.ground_course_cd); + state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0]))); state.horizontal_accuracy = (float)temp.HAccuracy * 0.01f; state.vertical_accuracy = (float)temp.VAccuracy * 0.01f; diff --git a/libraries/AP_GPS/AP_GPS_SBP.cpp b/libraries/AP_GPS/AP_GPS_SBP.cpp index b6650675d1..3b5ddc98f6 100644 --- a/libraries/AP_GPS/AP_GPS_SBP.cpp +++ b/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 diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp index 8e27f34568..73cc480c23 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 314368a08d..4640b6e59f 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/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; diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 7ba759c891..80114cceb0 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/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);