diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 73829001c8771d..5b313e930fcc1c 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 e45aef88c44c95..a3a70479056af0 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. @@ -620,6 +628,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 e5a102998bf96f..d2767ea5107d5e 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 @@ -138,6 +141,78 @@ AP_GPS_SBF::read(void) 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) { + //IMES not taken into account by Septentrio receivers + if (asprintf(&config_string, "sst, %s%s%s%s%s%s\n", (params.gnss_mode&(1U<<0))!=0 ? "GPS" : "", + (params.gnss_mode&(1U<<1))!=0 ? ((params.gnss_mode&0x01)==0 ? "SBAS" : "+SBAS") : "", + (params.gnss_mode&(1U<<2))!=0 ? ((params.gnss_mode&0x03)==0 ? "GALILEO" : "+GALILEO") : "", + (params.gnss_mode&(1U<<3))!=0 ? ((params.gnss_mode&0x07)==0 ? "BEIDOU" : "+BEIDOU") : "", + (params.gnss_mode&(1U<<5))!=0 ? ((params.gnss_mode&0x0F)==0 ? "QZSS" : "+QZSS") : "", + (params.gnss_mode&(1U<<6))!=0 ? ((params.gnss_mode&0x2F)==0 ? "GLONASS" : "+GLONASS") : "") == -1) { + config_string=nullptr; + } + } + break; case Config_State::Blob: if (asprintf(&config_string, "%s\n", _initialisation_blob[_init_blob_index]) == -1) { config_string = nullptr; @@ -364,6 +439,12 @@ AP_GPS_SBF::parse(uint8_t temp) config_step = Config_State::SSO; break; case Config_State::SSO: + 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 8817653f42ed74..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,9 +82,11 @@ class AP_GPS_SBF : public AP_GPS_Backend enum class Config_State { Baud_Rate, SSO, + Log, Blob, SBAS, SGA, + Constellation, Complete }; Config_State config_step; @@ -84,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