diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 73bf98ab25297..6b94311ace856 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -47,6 +47,7 @@ class AP_GPS_Backend; class RTCM3_Parser; +class AP_GPS_SBF; /// @class AP_GPS /// GPS driver main class @@ -139,7 +140,9 @@ class AP_GPS #if GPS_MOVING_BASELINE MovingBase mb_params; #endif // GPS_MOVING_BASELINE - +#if AP_GPS_SBF_ENABLED + AP_GPS_SBF *sbf_backend; +#endif static const struct AP_Param::GroupInfo var_info[]; }; diff --git a/libraries/AP_GPS/AP_GPS_Params.cpp b/libraries/AP_GPS/AP_GPS_Params.cpp index f7fc29ead9747..152d7e846a3be 100644 --- a/libraries/AP_GPS/AP_GPS_Params.cpp +++ b/libraries/AP_GPS/AP_GPS_Params.cpp @@ -19,6 +19,7 @@ #if AP_GPS_ENABLED #include "AP_GPS.h" +#include "AP_GPS_SBF.h" // table of user settable parameters const AP_Param::GroupInfo AP_GPS::Params::var_info[] = { @@ -113,6 +114,12 @@ const AP_Param::GroupInfo AP_GPS::Params::var_info[] = { AP_GROUPINFO("CAN_OVRIDE", 9, AP_GPS::Params, override_node_id, 0), #endif +#if AP_GPS_SBF_ENABLED + // @Group: + // @Path: ./SIM_GPS_SBF.cpp + AP_SUBGROUPPTR(sbf_backend, "", 10, AP_GPS::Params, AP_GPS_SBF), +#endif + AP_GROUPEND }; diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 4a18a3295526a..055bccf6a5923 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 @@ -58,6 +61,38 @@ constexpr const char *AP_GPS_SBF::portIdentifiers[]; constexpr const char* AP_GPS_SBF::_initialisation_blob[]; constexpr const char* AP_GPS_SBF::sbas_on_blob[]; +// table of user settable parameters +const AP_Param::GroupInfo AP_GPS_SBF::var_info[] = { + + // @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", 1, AP_GPS_SBF, _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", 2, AP_GPS_SBF, _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", 3, AP_GPS_SBF, _logging_overwrite, 0), + + AP_GROUPEND +}; + AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, @@ -76,6 +111,11 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA)) { state.gps_yaw_configured = true; } + + // set ptr in params so private parameters are accessible + _params.sbf_backend = this; + + AP_Param::setup_object_defaults(this, var_info); } AP_GPS_SBF::~AP_GPS_SBF (void) { @@ -138,8 +178,71 @@ AP_GPS_SBF::read(void) config_string = nullptr; } break; + case Config_State::LogStream: + if (_logging_frequency <= 0.0f && _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 (_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 (_logging_frequency <= 0.1f) { + logging_frequency = SBF_0_1_HZ; + } else if (_logging_frequency <= 0.2f) { + logging_frequency = SBF_0_2_HZ; + } else if (_logging_frequency <= 0.5f) { + logging_frequency = SBF_0_5_HZ; + } else if (_logging_frequency <= 1.0f) { + logging_frequency = SBF_1_0_HZ; + } else if (_logging_frequency <= 2.0f) { + logging_frequency = SBF_2_0_HZ; + } else if (_logging_frequency <= 5.0f) { + logging_frequency = SBF_5_0_HZ; + } else if (_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(_logging_level.get())) { + case GPS_Logging_Level::LITE: + logged_blocks = "PostProcess+Event"; + break; + case GPS_Logging_Level::BASIC: + logged_blocks = "PostProcess+Event+Comment+ReceiverStatus"; + break; + case GPS_Logging_Level::DEFAULT: + default: + logged_blocks = "Support+Event+Comment"; + break; + case GPS_Logging_Level::FULL: + logged_blocks = "Support+Event+Comment+BBSamples"; + break; + } + if (asprintf(&config_string, "sso,Stream%d,Dsk1,%s%s,%s\n", + (int)GPS_SBF_LOG_STREAM_NUMBER, + _logging_overwrite ? "" : "+", + logged_blocks, + logging_frequency) == -1) { + config_string = nullptr; + } + } + + break; + case Config_State::LogFile: + if (asprintf(&config_string, "sfn,DSK1,Incremental,'ardup'\n") == -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") : "", @@ -377,9 +480,28 @@ AP_GPS_SBF::parse(uint8_t temp) config_step = Config_State::SSO; break; case Config_State::SSO: - config_step = Config_State::Constellation; + if ((params.gnss_mode&0x6F)!=0) { + config_step = Config_State::Constellation; + } + else if (_logging_overwrite) { + config_step = Config_State::LogStream; + } + else { + config_step = Config_State::Blob; + } break; case Config_State::Constellation: + if (_logging_overwrite) { + config_step = Config_State::LogStream; + } + else { + config_step = Config_State::Blob; + } + break; + case Config_State::LogStream: + config_step = Config_State::LogFile; + break; + case Config_State::LogFile: 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 b4e1bb28c62b1..f8cf248919464 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: @@ -60,6 +69,8 @@ class AP_GPS_SBF : public AP_GPS_Backend bool get_error_codes(uint32_t &error_codes) const override { error_codes = RxError; return true; }; + static const struct AP_Param::GroupInfo var_info[]; + private: bool parse(uint8_t temp); @@ -68,15 +79,30 @@ class AP_GPS_SBF : public AP_GPS_Backend static const uint8_t SBF_PREAMBLE1 = '$'; static const uint8_t SBF_PREAMBLE2 = '@'; + // GPS receiver log level which determines what data is logged + enum class GPS_Logging_Level : uint8_t { + LITE = 0, + BASIC = 1, + DEFAULT = 2, + FULL = 3, + }; + + // SBF specific parameters + AP_Float _logging_frequency; // frequency at which the receiver should log messages + AP_Enum _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 + uint8_t _init_blob_index; uint32_t _init_blob_time; enum class Config_State { Baud_Rate, SSO, + Constellation, + LogStream, + LogFile, Blob, SBAS, SGA, - Constellation, Complete }; Config_State config_step; @@ -85,7 +111,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