diff --git a/libraries/AP_GPS/AP_GPS_HIL.cpp b/libraries/AP_GPS/AP_GPS_HIL.cpp index 12f5f46067..adfd30f2d9 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.cpp +++ b/libraries/AP_GPS/AP_GPS_HIL.cpp @@ -35,10 +35,10 @@ void AP_GPS_HIL::setHIL(uint32_t _time, float _latitude, float _longitude, float time = _time; latitude = _latitude*1.0e7f; longitude = _longitude*1.0e7f; - altitude = _altitude*1.0e2f; - ground_speed = _ground_speed*1.0e2f; - ground_course = _ground_course*1.0e2f; - speed_3d = _speed_3d*1.0e2f; + altitude_cm = _altitude*1.0e2f; + ground_speed_cm = _ground_speed*1.0e2f; + ground_course_cd = _ground_course*1.0e2f; + speed_3d_cm = _speed_3d*1.0e2f; num_sats = _num_sats; fix = FIX_3D; new_data = true; diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp index a60f824e8d..8ff8d7b68c 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK.cpp @@ -147,9 +147,9 @@ restart: } latitude = _swapl(&_buffer.msg.latitude) * 10; longitude = _swapl(&_buffer.msg.longitude) * 10; - altitude = _swapl(&_buffer.msg.altitude); - ground_speed = _swapl(&_buffer.msg.ground_speed); - ground_course = _swapl(&_buffer.msg.ground_course) / 10000; + altitude_cm = _swapl(&_buffer.msg.altitude); + ground_speed_cm = _swapl(&_buffer.msg.ground_speed); + ground_course_cd = _swapl(&_buffer.msg.ground_course) / 10000; num_sats = _buffer.msg.satellites; // time from gps is UTC, but convert here to msToD diff --git a/libraries/AP_GPS/AP_GPS_MTK19.cpp b/libraries/AP_GPS/AP_GPS_MTK19.cpp index 57a9a34f05..6c4320ae8b 100644 --- a/libraries/AP_GPS/AP_GPS_MTK19.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK19.cpp @@ -151,9 +151,9 @@ restart: latitude = _buffer.msg.latitude; longitude = _buffer.msg.longitude; } - altitude = _buffer.msg.altitude; - ground_speed = _buffer.msg.ground_speed; - ground_course = _buffer.msg.ground_course; + altitude_cm = _buffer.msg.altitude; + ground_speed_cm = _buffer.msg.ground_speed; + ground_course_cd = _buffer.msg.ground_course; num_sats = _buffer.msg.satellites; hdop = _buffer.msg.hdop; date = _buffer.msg.utc_date; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 04170b2a0d..27a53ec3c7 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -241,12 +241,12 @@ bool AP_GPS_NMEA::_term_complete() date = _new_date; latitude = _new_latitude * 10; // degrees*10e5 -> 10e7 longitude = _new_longitude * 10; // degrees*10e5 -> 10e7 - ground_speed = _new_speed; - ground_course = _new_course; + ground_speed_cm = _new_speed; + ground_course_cd = _new_course; fix = GPS::FIX_3D; // To-Do: add support for proper reporting of 2D and 3D fix break; case _GPS_SENTENCE_GPGGA: - altitude = _new_altitude; + altitude_cm = _new_altitude; time = _new_time; latitude = _new_latitude * 10; // degrees*10e5 -> 10e7 longitude = _new_longitude * 10; // degrees*10e5 -> 10e7 @@ -255,8 +255,8 @@ bool AP_GPS_NMEA::_term_complete() fix = GPS::FIX_3D; // To-Do: add support for proper reporting of 2D and 3D fix break; case _GPS_SENTENCE_GPVTG: - ground_speed = _new_speed; - ground_course = _new_course; + ground_speed_cm = _new_speed; + ground_course_cd = _new_course; // VTG has no fix indicator, can't change fix status break; } diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp index a97ed7cb69..99069f71ab 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -180,11 +180,11 @@ AP_GPS_SIRF::_parse_gps(void) } latitude = _swapl(&_buffer.nav.latitude); longitude = _swapl(&_buffer.nav.longitude); - altitude = _swapl(&_buffer.nav.altitude_msl); - ground_speed = _swapi(&_buffer.nav.ground_speed); + altitude_cm = _swapl(&_buffer.nav.altitude_msl); + ground_speed_cm = _swapi(&_buffer.nav.ground_speed); // at low speeds, ground course wanders wildly; suppress changes if we are not moving - if (ground_speed > 50) - ground_course = _swapi(&_buffer.nav.ground_course); + if (ground_speed_cm > 50) + ground_course_cd = _swapi(&_buffer.nav.ground_course); num_sats = _buffer.nav.satellites; return true; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 4ca2adecab..233ebae415 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -221,7 +221,7 @@ AP_GPS_UBLOX::_parse_gps(void) time = _buffer.posllh.time; longitude = _buffer.posllh.longitude; latitude = _buffer.posllh.latitude; - altitude = _buffer.posllh.altitude_msl / 10; + altitude_cm = _buffer.posllh.altitude_msl / 10; fix = next_fix; _new_position = true; break; @@ -265,9 +265,9 @@ AP_GPS_UBLOX::_parse_gps(void) break; case MSG_VELNED: Debug("MSG_VELNED"); - 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 + speed_3d_cm = _buffer.velned.speed_3d; // cm/s + ground_speed_cm = _buffer.velned.speed_2d; // cm/s + ground_course_cd = _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; diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp index e9434e769a..2cadc1c2b1 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -74,7 +74,7 @@ GPS::update(void) if (_status >= GPS_OK_FIX_2D) { last_fix_time = _idleTimer; - _last_ground_speed_cm = ground_speed; + _last_ground_speed_cm = ground_speed_cm; if (_have_raw_velocity) { // the GPS is able to give us velocity numbers directly @@ -82,8 +82,8 @@ GPS::update(void) _velocity_east = _vel_east * 0.01f; _velocity_down = _vel_down * 0.01f; } else { - float gps_heading = ToRad(ground_course * 0.01f); - float gps_speed = ground_speed * 0.01f; + float gps_heading = ToRad(ground_course_cd * 0.01f); + float gps_speed = ground_speed_cm * 0.01f; float sin_heading, cos_heading; cos_heading = cosf(gps_heading); diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index 6c40eba4c8..7aed52efd2 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -103,10 +103,10 @@ public: uint32_t date; ///< GPS date (FORMAT TBD) int32_t latitude; ///< latitude in degrees * 10,000,000 int32_t longitude; ///< longitude in degrees * 10,000,000 - int32_t altitude; ///< altitude in cm - uint32_t ground_speed; ///< ground speed in cm/sec - int32_t ground_course; ///< ground course in 100ths of a degree - int32_t speed_3d; ///< 3D speed in cm/sec (not always available) + int32_t altitude_cm; ///< altitude in cm + uint32_t ground_speed_cm; ///< ground speed in cm/sec + int32_t ground_course_cd; ///< ground course in 100ths of a degree + int32_t speed_3d_cm; ///< 3D speed in cm/sec (not always available) int16_t hdop; ///< horizontal dilution of precision in cm uint8_t num_sats; ///< Number of visible satelites