From aafdd1c11aaa955d9715b09a3380ebf00454490d Mon Sep 17 00:00:00 2001 From: Thomas Frans Date: Wed, 3 Apr 2024 16:07:09 +0200 Subject: [PATCH] AP_GPS: make receiver logging configurable through parameters Add parameters that allow users to configure logging from within their ground control station software. --- libraries/AP_GPS/AP_GPS.cpp | 26 +++++++++++++ libraries/AP_GPS/AP_GPS.h | 11 ++++++ libraries/AP_GPS/AP_GPS_SBF.cpp | 65 +++++++++++++++++++++++++++++++++ libraries/AP_GPS/AP_GPS_SBF.h | 11 +++++- 4 files changed, 112 insertions(+), 1 deletion(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index d6a541a06d058d..7d5b4bb59ffe19 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -274,6 +274,32 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { AP_SUBGROUPINFO(params[1], "2_", 33, AP_GPS, AP_GPS::Params), #endif + // @Param: _LOG_HZ + // @DisplayName: Log frequency + // @Description: The frequency with which the receiver logs information. + // @Units: Hz + // @Increment: 0.1 + // @Range: 0 20 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("_LOG_HZ", 34, AP_GPS, _logging_frequency, 1.0f), + + // @Param: _LOG_LEVEL + // @DisplayName: Log level + // @Description: The level of detail the receiver should log. + // @Values: 0:Lite, 1:Basic, 2:Default, 3:Full + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("_LOG_LEVEL", 35, AP_GPS, _logging_level, 2), + + // @Param: _LOG_FORCE + // @DisplayName: Log overwrite + // @Description: Whether logging setup should overwrite existing logging on the receiver. + // @Values: 0:Disabled, 1:Enabled + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("_LOG_FORCE", 36, AP_GPS, _logging_overwrite, 0), + AP_GROUPEND }; diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 73bf98ab252974..9331b1733d0c14 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -184,6 +184,14 @@ class AP_GPS KNOWN = 3, ///< The full covariance array is reported by the GPS }; + // GPS receiver log level which determines what data is logged + enum GPS_Logging_Level { + LITE = 0, + BASIC = 1, + DEFAULT = 2, + FULL = 3, + }; + /* The GPS_State structure is filled in by the backend driver as it parses each message from the GPS. @@ -621,6 +629,9 @@ class AP_GPS AP_Int8 _blend_mask; AP_Int16 _driver_options; AP_Int8 _primary; + AP_Float _logging_frequency; // frequency at which the receiver should log messages + AP_Int8 _logging_level; // amount of details the receiver should log + AP_Int8 _logging_overwrite; // whether to override existing logging on the receiver or add to it uint32_t _log_gps_bit = -1; diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 4a18a3295526a3..33877a00bf6a4e 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -45,6 +45,9 @@ do { \ #ifndef GPS_SBF_STREAM_NUMBER #define GPS_SBF_STREAM_NUMBER 1 #endif +#ifndef GPS_SBF_LOG_STREAM_NUMBER + #define GPS_SBF_LOG_STREAM_NUMBER 2 +#endif #define SBF_EXCESS_COMMAND_BYTES 5 // 2 start bytes + validity byte + space byte + endline byte @@ -137,6 +140,65 @@ AP_GPS_SBF::read(void) extra_config) == -1) { config_string = nullptr; } + break; + case Config_State::Log: + if (gps._logging_frequency <= 0.0f && gps._logging_overwrite) { + // hard-disable logging, clear existing stream + if (asprintf(&config_string, "sso,Stream%d,none,none,off\n", (int)GPS_SBF_LOG_STREAM_NUMBER) == -1) { + config_string = nullptr; + } + } else if (gps._logging_frequency <= 0.0f) { + // soft-disable logging, keep existing stream + if (asprintf(&config_string, "sso,Stream%d,,,off\n", (int)GPS_SBF_LOG_STREAM_NUMBER) == -1) { + config_string = nullptr; + } + } else { + // enable logging + const char* logging_frequency; + if (gps._logging_frequency <= 0.1f) { + logging_frequency = SBF_0_1_HZ; + } else if (gps._logging_frequency <= 0.2f) { + logging_frequency = SBF_0_2_HZ; + } else if (gps._logging_frequency <= 0.5f) { + logging_frequency = SBF_0_5_HZ; + } else if (gps._logging_frequency <= 1.0f) { + logging_frequency = SBF_1_0_HZ; + } else if (gps._logging_frequency <= 2.0f) { + logging_frequency = SBF_2_0_HZ; + } else if (gps._logging_frequency <= 5.0f) { + logging_frequency = SBF_5_0_HZ; + } else if (gps._logging_frequency <= 10.0f) { + logging_frequency = SBF_10_0_HZ; + } else { + logging_frequency = SBF_20_0_HZ; + } + + const char* logged_blocks; + switch (gps._logging_level) { + case AP_GPS::GPS_Logging_Level::LITE: + logged_blocks = "PostProcess+Event"; + break; + case AP_GPS::GPS_Logging_Level::BASIC: + logged_blocks = "PostProcess+Event+Comment+ReceiverStatus"; + break; + case AP_GPS::GPS_Logging_Level::DEFAULT: + default: + logged_blocks = "Support+Event+Comment"; + break; + case AP_GPS::GPS_Logging_Level::FULL: + logged_blocks = "Support+Event+Comment+BBSamples"; + break; + } + + if (asprintf(&config_string, "sso,Stream%d,Dsk1,%s%s,%s\nsfn,DSK1,Incremental,'ardup'\n", + (int)GPS_SBF_LOG_STREAM_NUMBER, + gps._logging_overwrite ? "" : "+", + logged_blocks, + logging_frequency) == -1) { + config_string = nullptr; + } + } + break; case Config_State::Constellation: if ((params.gnss_mode&0x6F)!=0) { @@ -380,6 +442,9 @@ AP_GPS_SBF::parse(uint8_t temp) config_step = Config_State::Constellation; break; case Config_State::Constellation: + config_step = Config_State::Log; + break; + case Config_State::Log: config_step = Config_State::Blob; break; case Config_State::Blob: diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index b4e1bb28c62b1e..20a94941d3d34d 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -28,6 +28,15 @@ #define SBF_DISK_FULL (1 << 8) #define SBF_DISK_MOUNTED (1 << 9) +#define SBF_0_1_HZ "sec10"; +#define SBF_0_2_HZ "sec5"; +#define SBF_0_5_HZ "sec2"; +#define SBF_1_0_HZ "sec1"; +#define SBF_2_0_HZ "msec500"; +#define SBF_5_0_HZ "msec200"; +#define SBF_10_0_HZ "msec100"; +#define SBF_20_0_HZ "msec50"; + class AP_GPS_SBF : public AP_GPS_Backend { public: @@ -73,6 +82,7 @@ class AP_GPS_SBF : public AP_GPS_Backend enum class Config_State { Baud_Rate, SSO, + Log, Blob, SBAS, SGA, @@ -85,7 +95,6 @@ class AP_GPS_SBF : public AP_GPS_Backend "srd,Moderate,UAV", "sem,PVT,5", "spm,Rover,all", - "sso,Stream2,Dsk1,postprocess+event+comment+ReceiverStatus,msec100", #if defined (GPS_SBF_EXTRA_CONFIG) GPS_SBF_EXTRA_CONFIG #endif