From eed17383242e2e67bff4285134971dc3bf5053b3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 19 Sep 2023 13:15:09 +1000 Subject: [PATCH] AP_GPS: implement the GPS_DRV_OPTION for ellisoid height in mode drivers this allows a lot more drivers to use the GPS_DRV_OPTION but to use ellisoid height. Particularly useful for DroneCAN GPS modules using ellisoid height instead of AMSL is useful in some specialised application --- libraries/AP_GPS/AP_GPS.cpp | 2 +- libraries/AP_GPS/AP_GPS_ERB.cpp | 2 +- libraries/AP_GPS/AP_GPS_NMEA.cpp | 8 ++++---- libraries/AP_GPS/AP_GPS_NOVA.cpp | 2 +- libraries/AP_GPS/AP_GPS_SBF.cpp | 2 +- libraries/AP_GPS/AP_GPS_SIRF.cpp | 7 +++++-- libraries/AP_GPS/AP_GPS_UAVCAN.cpp | 3 ++- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 12 ++---------- libraries/AP_GPS/GPS_Backend.cpp | 15 +++++++++++++++ libraries/AP_GPS/GPS_Backend.h | 3 +++ 10 files changed, 35 insertions(+), 21 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index b3cfa29340..3ff5f603b2 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -314,7 +314,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_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 312cbfcc33..3a72f6d557 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 86780bdd6b..385567a52b 100644 --- a/libraries/AP_GPS/AP_GPS_NOVA.cpp +++ b/libraries/AP_GPS/AP_GPS_NOVA.cpp @@ -205,9 +205,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 d758003c3f..2ef226f502 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -179,9 +179,12 @@ AP_GPS_SIRF::_parse_gps(void) } state.location.lat = swap_int32(_buffer.nav.latitude); state.location.lng = swap_int32(_buffer.nav.longitude); - state.location.alt = swap_int32(_buffer.nav.altitude_msl); + const int32_t alt_amsl = swap_int32(_buffer.nav.altitude_msl); + const int32_t alt_ellipsoid = swap_int32(_buffer.nav.altitude_ellipsoid); state.have_undulation = true; - state.undulation = (state.location.alt - swap_int32(_buffer.nav.altitude_ellipsoid))*0.01; + state.undulation = (alt_amsl - alt_ellipsoid)*0.01; + set_alt_amsl_cm(state, alt_amsl); + state.location.alt = swap_int32(_buffer.nav.altitude_msl); state.ground_speed = swap_int32(_buffer.nav.ground_speed)*0.01f; state.ground_course = wrap_360(swap_int16(_buffer.nav.ground_course)*0.01f); state.num_sats = _buffer.nav.satellites; diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp index 014f6546e5..dc92b1e515 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp @@ -436,10 +436,11 @@ void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb) Location loc = { }; loc.lat = cb.msg->latitude_deg_1e8 / 10; loc.lng = cb.msg->longitude_deg_1e8 / 10; - loc.alt = cb.msg->height_msl_mm / 10; + const int32_t alt_amsl_cm = cb.msg->height_msl_mm / 10; interim_state.have_undulation = true; interim_state.undulation = (cb.msg->height_msl_mm - cb.msg->height_ellipsoid_mm) * 0.001; interim_state.location = loc; + set_alt_amsl_cm(interim_state, alt_amsl_cm); handle_velocity(cb.msg->ned_velocity[0], cb.msg->ned_velocity[1], cb.msg->ned_velocity[2]); diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 7ef4c57f20..9caa950c03 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 b6379abde3..c93dbe91c3 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -468,6 +468,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 ce1fac0d62..005e1d96ab 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -157,6 +157,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;