From 65222e216f6f41445efc393a3a6be02903e03929 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 19 Feb 2024 10:57:18 +1100 Subject: [PATCH 1/4] AP_GPS: use probing when probing for GPS, add detected message --- libraries/AP_GPS/AP_GPS.cpp | 8 ++++++++ libraries/AP_GPS/AP_GPS.h | 1 + libraries/AP_GPS/GPS_Backend.cpp | 2 +- 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 25a790787a8d4..52f622458ef97 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -999,6 +999,14 @@ void AP_GPS::update_instance(uint8_t instance) tnow = state[instance].last_corrected_gps_time_us/1000U; state[instance].corrected_timestamp_updated = false; } + + // we set delta_time_ms to the timeout value when clearing + // state; use it being zero to mark first message + if (!state[instance].announced_detection) { + state[instance].announced_detection = true; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %d: detected %s", instance + 1, drivers[instance]->name()); + } + // delta will only be correct after parsing two messages timing[instance].delta_time_ms = tnow - timing[instance].last_message_time_ms; timing[instance].last_message_time_ms = tnow; diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index a5ca9cdceaddd..b48bf453a4742 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -212,6 +212,7 @@ class AP_GPS float undulation; //snprintf(buffer, buflen, - "GPS %d: detected as %s at %d baud", + "GPS %d: probing for %s at %d baud", instance + 1, name(), int(gps._baudrates[dstate.current_baud])); From d63f806fa216cf5de32e166ab0f16c93cd5104c3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 19 Feb 2024 10:57:36 +1100 Subject: [PATCH 2/4] SITL: add diagnostics when bad GPS backend specified --- libraries/SITL/SIM_GPS.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp index 53f858a94634a..38735248d97e2 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/libraries/SITL/SIM_GPS.cpp @@ -26,6 +26,8 @@ #include "SIM_GPS_SBP.h" #include "SIM_GPS_UBLOX.h" +#include + // the number of GPS leap seconds - copied from AP_GPS.h #define GPS_LEAPSECONDS_MILLIS 18000ULL @@ -279,6 +281,9 @@ void GPS::check_backend_allocation() #endif }; + if (configured_type != Type::NONE && backend == nullptr) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SIM_GPS: No backend for %u", (unsigned)configured_type); + } allocated_type = configured_type; } From 13d339d00f7cb3af9337ed3ace823f0cbd291e54 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 19 Feb 2024 10:58:47 +1100 Subject: [PATCH 3/4] SITL: correct documentation around simulated GPS type --- libraries/SITL/SITL.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 2aeed73ab6ed2..46ff0cba1294f 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -499,7 +499,7 @@ const AP_Param::GroupInfo SIM::var_gps[] = { // @Param: GPS_TYPE // @DisplayName: GPS 1 type // @Description: Sets the type of simulation used for GPS 1 - // @Values: 0:None, 1:UBlox, 5:NMEA, 6:SBP, 7:File, 8:Nova, 9:SBP, 10:Trimble, 19:MSP + // @Values: 0:None, 1:UBlox, 5:NMEA, 6:SBP, 7:File, 8:Nova, 9:SBP2, 11:Trimble, 19:MSP // @User: Advanced AP_GROUPINFO("GPS_TYPE", 3, SIM, gps_type[0], GPS::Type::UBLOX), // @Param: GPS_BYTELOSS From 0bae80c54a923cb1c21e7c189ef15697c621f3c0 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 19 Feb 2024 21:49:54 +1100 Subject: [PATCH 4/4] autotest: adjust tests for new GPS messages --- Tools/autotest/vehicle_test_suite.py | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 5b7e4f54b7ecd..dbffada9201c8 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -13509,11 +13509,11 @@ def GPSTypes(self): # if gps_type is None we auto-detect sim_gps = [ # (0, "NONE"), - (1, "UBLOX", None, "u-blox", 5, 'detected'), - (5, "NMEA", 5, "NMEA", 5, 'detected'), - (6, "SBP", None, "SBP", 5, 'detected'), + (1, "UBLOX", None, "u-blox", 5, 'probing'), + (5, "NMEA", 5, "NMEA", 5, 'probing'), + (6, "SBP", None, "SBP", 5, 'probing'), # (7, "SBP2", 9, "SBP2", 5), # broken, "waiting for config data" - (8, "NOVA", 15, "NOVA", 5, 'detected'), # no attempt to auto-detect this in AP_GPS + (8, "NOVA", 15, "NOVA", 5, 'probing'), # no attempt to auto-detect this in AP_GPS (11, "GSOF", 11, "GSOF", 5, 'specified'), # no attempt to auto-detect this in AP_GPS (19, "MSP", 19, "MSP", 32, 'specified'), # no attempt to auto-detect this in AP_GPS # (9, "FILE"), @@ -13528,7 +13528,11 @@ def GPSTypes(self): self.set_parameter("GPS_TYPE", gps_type) self.context_clear_collection('STATUSTEXT') self.reboot_sitl() - self.wait_statustext("%s as %s" % (detect_prefix, detect_name), check_context=True) + if detect_prefix == "probing": + self.wait_statustext(f"probing for {detect_name}", check_context=True) + else: + self.wait_statustext(f"specified as {detect_name}", check_context=True) + self.wait_statustext(f"detected {detect_name}", check_context=True) n = self.poll_home_position(timeout=120) distance = self.get_distance_int(orig, n) if distance > 1: @@ -13565,8 +13569,8 @@ def MultipleGPS(self): # AP_GPS::init() at boot time, so it will never be detected. self.context_collect("STATUSTEXT") self.reboot_sitl() - self.wait_statustext("GPS 1: detected as u-blox", check_context=True) - self.wait_statustext("GPS 2: detected as u-blox", check_context=True) + self.wait_statustext("GPS 1: detected u-blox", check_context=True) + self.wait_statustext("GPS 2: detected u-blox", check_context=True) m = self.assert_receive_message("GPS2_RAW") self.progress(self.dump_message_verbose(m)) # would be nice for it to take some time to get a fix....