diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 99a66ff5cf..fa77cf3dd0 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -334,7 +334,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: _DRV_OPTIONS // @DisplayName: driver options // @Description: Additional backend specific options - // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL for uBlox driver + // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL // @User: Advanced AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0), diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp index 8a0743ca43..52f0cd6c06 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp @@ -383,10 +383,11 @@ void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uin Location loc = { }; loc.lat = msg.latitude_deg_1e8 / 10; loc.lng = msg.longitude_deg_1e8 / 10; - loc.alt = msg.height_msl_mm / 10; + const int32_t alt_amsl_cm = msg.height_msl_mm / 10; interim_state.have_undulation = true; interim_state.undulation = (msg.height_msl_mm - msg.height_ellipsoid_mm) * 0.001; interim_state.location = loc; + set_alt_amsl_cm(interim_state, alt_amsl_cm); handle_velocity(msg.ned_velocity[0], msg.ned_velocity[1], msg.ned_velocity[2]); diff --git a/libraries/AP_GPS/AP_GPS_ERB.cpp b/libraries/AP_GPS/AP_GPS_ERB.cpp index 3326ac6040..1dde8997f2 100644 --- a/libraries/AP_GPS/AP_GPS_ERB.cpp +++ b/libraries/AP_GPS/AP_GPS_ERB.cpp @@ -150,9 +150,9 @@ AP_GPS_ERB::_parse_gps(void) _last_pos_time = _buffer.pos.time; state.location.lng = (int32_t)(_buffer.pos.longitude * (double)1e7); state.location.lat = (int32_t)(_buffer.pos.latitude * (double)1e7); - state.location.alt = (int32_t)(_buffer.pos.altitude_msl * 100); state.have_undulation = true; state.undulation = _buffer.pos.altitude_msl - _buffer.pos.altitude_ellipsoid; + set_alt_amsl_cm(state, _buffer.pos.altitude_msl * 100); state.status = next_fix; _new_position = true; state.horizontal_accuracy = _buffer.pos.horizontal_accuracy * 1.0e-3f; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index f9b6a02859..c47828209f 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -329,7 +329,7 @@ bool AP_GPS_NMEA::_term_complete() case _GPS_SENTENCE_GGA: _last_GGA_ms = now; if (_last_KSXT_pos_ms == 0 && _last_AGRICA_ms == 0) { - state.location.alt = _new_altitude; + set_alt_amsl_cm(state, _new_altitude); state.location.lat = _new_latitude; state.location.lng = _new_longitude; } @@ -426,7 +426,7 @@ bool AP_GPS_NMEA::_term_complete() } state.location.lat = _ksxt.fields[2]*1.0e7; state.location.lng = _ksxt.fields[1]*1.0e7; - state.location.alt = _ksxt.fields[3]*1.0e2; + set_alt_amsl_cm(state, _ksxt.fields[3]*1.0e2); _last_KSXT_pos_ms = now; if (_ksxt.fields[9] >= 1) { // we have 3D fix @@ -458,8 +458,9 @@ bool AP_GPS_NMEA::_term_complete() _last_3D_velocity_ms = now; state.location.lat = ag.lat*1.0e7; state.location.lng = ag.lng*1.0e7; - state.location.alt = ag.alt*1.0e2; state.undulation = -ag.undulation; + state.have_undulation = true; + set_alt_amsl_cm(state, ag.alt*1.0e2); state.velocity = ag.vel_NED; velocity_to_speed_course(state); state.speed_accuracy = ag.vel_stddev.length(); @@ -469,7 +470,6 @@ bool AP_GPS_NMEA::_term_complete() state.have_speed_accuracy = true; state.have_horizontal_accuracy = true; state.have_vertical_accuracy = true; - state.have_undulation = true; check_new_itow(ag.itow, _sentence_length); break; } diff --git a/libraries/AP_GPS/AP_GPS_NOVA.cpp b/libraries/AP_GPS/AP_GPS_NOVA.cpp index 7ca259fb05..bea9bfd675 100644 --- a/libraries/AP_GPS/AP_GPS_NOVA.cpp +++ b/libraries/AP_GPS/AP_GPS_NOVA.cpp @@ -208,9 +208,9 @@ AP_GPS_NOVA::process_message(void) state.location.lat = (int32_t) (bestposu.lat * (double)1e7); state.location.lng = (int32_t) (bestposu.lng * (double)1e7); - state.location.alt = (int32_t) (bestposu.hgt * 100); state.have_undulation = true; state.undulation = bestposu.undulation; + set_alt_amsl_cm(state, bestposu.hgt * 100); state.num_sats = bestposu.svsused; diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 5119786f27..3b1d8d7009 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -415,9 +415,9 @@ AP_GPS_SBF::process_message(void) if (temp.Latitude > -200000) { state.location.lat = (int32_t)(temp.Latitude * RAD_TO_DEG_DOUBLE * (double)1e7); state.location.lng = (int32_t)(temp.Longitude * RAD_TO_DEG_DOUBLE * (double)1e7); - state.location.alt = (int32_t)(((float)temp.Height - temp.Undulation) * 1e2f); state.have_undulation = true; state.undulation = -temp.Undulation; + set_alt_amsl_cm(state, ((float)temp.Height - temp.Undulation) * 1e2f); } if (temp.NrSV != 255) { diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp index f9b0b57140..da37a94aff 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -183,9 +183,11 @@ AP_GPS_SIRF::_parse_gps(void) } state.location.lat = int32_t(be32toh(_buffer.nav.latitude)); state.location.lng = int32_t(be32toh(_buffer.nav.longitude)); - state.location.alt = int32_t(be32toh(_buffer.nav.altitude_msl)); + const int32_t alt_amsl = int32_t(be32toh(_buffer.nav.altitude_msl)); + const int32_t alt_ellipsoid = int32_t(be32toh(_buffer.nav.altitude_ellipsoid)); + state.undulation = (alt_amsl - alt_ellipsoid)*0.01; state.have_undulation = true; - state.undulation = (state.location.alt - int32_t(be32toh(_buffer.nav.altitude_ellipsoid)))*0.01; + set_alt_amsl_cm(state, alt_amsl); state.ground_speed = int32_t(be32toh(_buffer.nav.ground_speed))*0.01f; state.ground_course = wrap_360(int16_t(be16toh(_buffer.nav.ground_course)*0.01f)); state.num_sats = _buffer.nav.satellites; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index cf3cdd6d30..9b2be38adb 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -1368,13 +1368,9 @@ AP_GPS_UBLOX::_parse_gps(void) _last_pos_time = _buffer.posllh.itow; state.location.lng = _buffer.posllh.longitude; state.location.lat = _buffer.posllh.latitude; - if (option_set(AP_GPS::HeightEllipsoid)) { - state.location.alt = _buffer.posllh.altitude_ellipsoid / 10; - } else { - state.location.alt = _buffer.posllh.altitude_msl / 10; - } state.have_undulation = true; state.undulation = (_buffer.posllh.altitude_msl - _buffer.posllh.altitude_ellipsoid) * 0.001; + set_alt_amsl_cm(state, _buffer.posllh.altitude_msl / 10); state.status = next_fix; _new_position = true; @@ -1532,13 +1528,9 @@ AP_GPS_UBLOX::_parse_gps(void) _last_pos_time = _buffer.pvt.itow; state.location.lng = _buffer.pvt.lon; state.location.lat = _buffer.pvt.lat; - if (option_set(AP_GPS::HeightEllipsoid)) { - state.location.alt = _buffer.pvt.h_ellipsoid / 10; - } else { - state.location.alt = _buffer.pvt.h_msl / 10; - } state.have_undulation = true; state.undulation = (_buffer.pvt.h_msl - _buffer.pvt.h_ellipsoid) * 0.001; + set_alt_amsl_cm(state, _buffer.pvt.h_msl / 10); switch (_buffer.pvt.fix_type) { case 0: diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 11635d5bca..a6e28c5fa6 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -437,6 +437,21 @@ good_yaw: } #endif // GPS_MOVING_BASELINE +/* + set altitude in location structure, honouring the driver option for + MSL vs ellipsoid height + */ +void AP_GPS_Backend::set_alt_amsl_cm(AP_GPS::GPS_State &_state, int32_t alt_amsl_cm) +{ + if (option_set(AP_GPS::HeightEllipsoid) && _state.have_undulation) { + // user has asked ArduPilot to use ellipsoid height in the + // canonical height for mission and navigation + _state.location.alt = alt_amsl_cm - _state.undulation*100; + } else { + _state.location.alt = alt_amsl_cm; + } +} + #if AP_GPS_DEBUG_LOGGING_ENABLED /* diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 09f680b37f..38db811785 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -156,6 +156,9 @@ protected: void log_data(const uint8_t *data, uint16_t length); #endif + // set alt in location, honouring GPS driver option for ellipsoid height + void set_alt_amsl_cm(AP_GPS::GPS_State &_state, int32_t alt_amsl_cm); + private: // itow from previous message uint64_t _pseudo_itow;