From 2c7a1137905323f37cb63cc77e7d22d6ce3a2dd5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 31 Aug 2015 08:24:35 +1000 Subject: [PATCH] AP_GPS: ensure all GPS drivers give headings as 0..360 degrees this prevents inconsistency between interfaces, and fixes a MAVLink reporting bug with UAVCAN GPS --- libraries/AP_GPS/AP_GPS.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 | 4 ++-- libraries/AP_GPS/AP_GPS_PX4.cpp | 2 +- libraries/AP_GPS/AP_GPS_SBP.cpp | 5 +---- libraries/AP_GPS/AP_GPS_SIRF.cpp | 2 +- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 2 +- 8 files changed, 9 insertions(+), 12 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index cbb68a5898..77a43fa945 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -454,7 +454,7 @@ 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 = degrees(atan2f(istate.velocity.y, istate.velocity.x)) * 100UL; + istate.ground_course_cd = wrap_360_cd(degrees(atan2f(istate.velocity.y, istate.velocity.x)) * 100UL); istate.hdop = hdop; istate.num_sats = _num_sats; istate.have_vertical_velocity |= _have_vertical_velocity; diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp index eb90bed748..8b1d07af43 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 = swap_int32(_buffer.msg.ground_course) / 10000; + state.ground_course_cd = wrap_360_cd(swap_int32(_buffer.msg.ground_course) / 10000); 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 5968082bcb..9f5f9b6beb 100644 --- a/libraries/AP_GPS/AP_GPS_MTK19.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK19.cpp @@ -144,7 +144,7 @@ restart: } state.location.alt = _buffer.msg.altitude; state.ground_speed = _buffer.msg.ground_speed*0.01f; - state.ground_course_cd = _buffer.msg.ground_course; + state.ground_course_cd = wrap_360_cd(_buffer.msg.ground_course); 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 636497c073..4eef322f58 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -296,7 +296,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 = _new_course; + state.ground_course_cd = wrap_360_cd(_new_course); make_gps_time(_new_date, _new_time * 10); state.last_gps_time_ms = hal.scheduler->millis(); // To-Do: add support for proper reporting of 2D and 3D fix @@ -314,7 +314,7 @@ bool AP_GPS_NMEA::_term_complete() break; case _GPS_SENTENCE_GPVTG: state.ground_speed = _new_speed*0.01f; - state.ground_course_cd = _new_course; + state.ground_course_cd = wrap_360_cd(_new_course); // 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 6a8896aa41..0ee5ad5959 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 = int32_t(double(_gps_pos.cog_rad) / M_PI * 18000. +.5); + state.ground_course_cd = wrap_360_cd(degrees(_gps_pos.cog_rad)*100); 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_SBP.cpp b/libraries/AP_GPS/AP_GPS_SBP.cpp index 11b92fd6a6..7ac0911d3c 100644 --- a/libraries/AP_GPS/AP_GPS_SBP.cpp +++ b/libraries/AP_GPS/AP_GPS_SBP.cpp @@ -277,10 +277,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 = (int32_t) 100*ToDeg(atan2f(state.velocity[1], state.velocity[0])); - if (state.ground_course_cd < 0) { - state.ground_course_cd += 36000; - } + state.ground_course_cd = wrap_360_cd((int32_t) 100*ToDeg(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 ad5a2e30ed..385a0b210e 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 = swap_int16(_buffer.nav.ground_course); + state.ground_course_cd = wrap_360_cd(swap_int16(_buffer.nav.ground_course)); 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 e4a29d4bda..9415cc1ed6 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -634,7 +634,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 = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100 + state.ground_course_cd = wrap_360_cd(_buffer.velned.heading_2d / 1000); // Heading 2D deg * 100000 rescaled to deg * 100 state.have_vertical_velocity = true; state.velocity.x = _buffer.velned.ned_north * 0.01f; state.velocity.y = _buffer.velned.ned_east * 0.01f;