Skip to content

Commit

Permalink
AP_GPS: make receiver logging configurable through parameters
Browse files Browse the repository at this point in the history 
Add parameters that allow users to configure logging from within their ground
control station software.
  • Loading branch information
@flyingthingsintothings
flyingthingsintothings committed Mar 19, 2024
1 parent 463301a commit bc999da
Show file tree
Hide file tree
Showing 4 changed files with 119 additions and 1 deletion.
26 changes: 26 additions & 0 deletions libraries/AP_GPS/AP_GPS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -278,6 +278,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", 32, 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", 33, 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", 34, AP_GPS, _logging_overwrite, 0),

AP_GROUPEND
};

Expand Down
18 changes: 18 additions & 0 deletions libraries/AP_GPS/AP_GPS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -206,6 +206,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.
Expand Down Expand Up @@ -638,6 +646,16 @@ class AP_GPS
AP_Int8 _blend_mask;
AP_Int16 _driver_options;
AP_Int8 _primary;
#if HAL_ENABLE_DRONECAN_DRIVERS
AP_Int32 _node_id[GPS_MAX_RECEIVERS];
AP_Int32 _override_node_id[GPS_MAX_RECEIVERS];
#endif
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
#if GPS_MOVING_BASELINE
MovingBase mb_params[GPS_MAX_RECEIVERS];
#endif // GPS_MOVING_BASELINE

uint32_t _log_gps_bit = -1;

Expand Down
65 changes: 65 additions & 0 deletions libraries/AP_GPS/AP_GPS_SBF.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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 = "comment+ReceiverStatus";
break;
case AP_GPS::GPS_Logging_Level::BASIC:
logged_blocks = "comment+ReceiverStatus+PostProcess+Event";
break;
case AP_GPS::GPS_Logging_Level::DEFAULT:
default:
logged_blocks = "comment+ReceiverStatus+PostProcess+Event+Support";
break;
case AP_GPS::GPS_Logging_Level::FULL:
logged_blocks = "comment+ReceiverStatus+PostProcess+Event+support+BBSamples";
break;
}

if (asprintf(&config_string, "sso,Stream%d,Dsk1,%s%s,%s\nsfn,DSK1,Incremental,'px4log'\n",
(int)GPS_SBF_LOG_STREAM_NUMBER,
gps._logging_overwrite ? "" : "+",
logged_blocks,
logging_frequency) == -1) {
config_string = nullptr;
}
}

break;
case Config_State::Blob:
if (asprintf(&config_string, "%s\n", _initialisation_blob[_init_blob_index]) == -1) {
Expand Down Expand Up @@ -364,6 +426,9 @@ AP_GPS_SBF::parse(uint8_t temp)
config_step = Config_State::SSO;
break;
case Config_State::SSO:
config_step = Config_State::Log;
break;
case Config_State::Log:
config_step = Config_State::Blob;
break;
case Config_State::Blob:
Expand Down
11 changes: 10 additions & 1 deletion libraries/AP_GPS/AP_GPS_SBF.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -73,6 +82,7 @@ class AP_GPS_SBF : public AP_GPS_Backend
enum class Config_State {
Baud_Rate,
SSO,
Log,
Blob,
SBAS,
SGA,
Expand All @@ -84,7 +94,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
Expand Down

0 comments on commit bc999da

Please sign in to comment.