Skip to content

Commit

Permalink
AP_GPS: support GNSS receiver resilience information over MAVLink
Browse files Browse the repository at this point in the history 
Add support for reporting resilience information from GNSS receivers
(interference & authentication) over MAVLink.
  • Loading branch information
@flyingthingsintothings
flyingthingsintothings committed May 6, 2024
1 parent 45114d8 commit e62e725
Show file tree
Hide file tree
Showing 4 changed files with 231 additions and 1 deletion.
30 changes: 29 additions & 1 deletion libraries/AP_GPS/AP_GPS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,6 +106,30 @@ static_assert((uint32_t)AP_GPS::GPS_Status::GPS_OK_FIX_3D == (uint32_t)GPS_FIX_T
static_assert((uint32_t)AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS == (uint32_t)GPS_FIX_TYPE_DGPS, "FIX_DGPS incorrect");
static_assert((uint32_t)AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT == (uint32_t)GPS_FIX_TYPE_RTK_FLOAT, "FIX_RTK_FLOAT incorrect");
static_assert((uint32_t)AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED == (uint32_t)GPS_FIX_TYPE_RTK_FIXED, "FIX_RTK_FIXED incorrect");

static_assert((uint32_t)AP_GPS::GPS_Errors::NONE == (uint32_t)GPS_SYSTEM_ERROR_NONE);
static_assert((uint32_t)AP_GPS::GPS_Errors::INCOMING_CORRECTIONS == (uint32_t)GPS_SYSTEM_ERROR_INCOMING_CORRECTIONS);
static_assert((uint32_t)AP_GPS::GPS_Errors::CONFIGURATION == (uint32_t)GPS_SYSTEM_ERROR_CONFIGURATION);
static_assert((uint32_t)AP_GPS::GPS_Errors::SOFTWARE == (uint32_t)GPS_SYSTEM_ERROR_SOFTWARE);
static_assert((uint32_t)AP_GPS::GPS_Errors::ANTENNA == (uint32_t)GPS_SYSTEM_ERROR_ANTENNA);
static_assert((uint32_t)AP_GPS::GPS_Errors::EVENT_CONGESTION == (uint32_t)GPS_SYSTEM_ERROR_EVENT_CONGESTION);
static_assert((uint32_t)AP_GPS::GPS_Errors::CPU_OVERLOAD == (uint32_t)GPS_SYSTEM_ERROR_CPU_OVERLOAD);
static_assert((uint32_t)AP_GPS::GPS_Errors::OUTPUT_CONGESTION == (uint32_t)GPS_SYSTEM_ERROR_OUTPUT_CONGESTION);

static_assert((uint8_t)AP_GPS::GPS_Authentication::AUTHENTICATION_UNKNOWN == (uint8_t)GPS_AUTHENTICATION_STATE_UNKNOWN);
static_assert((uint8_t)AP_GPS::GPS_Authentication::AUTHENTICATION_INITIALIZING == (uint8_t)GPS_AUTHENTICATION_STATE_INITIALIZING);
static_assert((uint8_t)AP_GPS::GPS_Authentication::AUTHENTICATION_ERROR == (uint8_t)GPS_AUTHENTICATION_STATE_ERROR);
static_assert((uint8_t)AP_GPS::GPS_Authentication::AUTHENTICATION_OK == (uint8_t)GPS_AUTHENTICATION_STATE_OK);
static_assert((uint8_t)AP_GPS::GPS_Authentication::AUTHENTICATION_DISABLED == (uint8_t)GPS_AUTHENTICATION_STATE_DISABLED);

static_assert((uint8_t)AP_GPS::GPS_Jamming::JAMMING_UNKNOWN == (uint8_t)GPS_JAMMING_STATE_UNKNOWN);
static_assert((uint8_t)AP_GPS::GPS_Jamming::JAMMING_OK == (uint8_t)GPS_JAMMING_STATE_OK);
static_assert((uint8_t)AP_GPS::GPS_Jamming::JAMMING_DETECTED == (uint8_t)GPS_JAMMING_STATE_DETECTED);

static_assert((uint8_t)AP_GPS::GPS_Spoofing::SPOOFING_UNKNOWN == (uint8_t)GPS_SPOOFING_STATE_UNKNOWN);
static_assert((uint8_t)AP_GPS::GPS_Spoofing::SPOOFING_OK == (uint8_t)GPS_SPOOFING_STATE_OK);
static_assert((uint8_t)AP_GPS::GPS_Spoofing::SPOOFING_DETECTED == (uint8_t)GPS_SPOOFING_STATE_DETECTED);
static_assert((uint8_t)AP_GPS::GPS_Spoofing::SPOOFING_MITIGATED == (uint8_t)GPS_SPOOFING_STATE_MITIGATED);
#endif

// ensure that our own enum-class status is equivalent to the
Expand Down Expand Up @@ -1395,7 +1419,11 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
vacc * 1000, // one-sigma standard deviation in mm
sacc * 1000, // one-sigma standard deviation in mm/s
0, // TODO one-sigma heading accuracy standard deviation
gps_yaw_cdeg(0));
gps_yaw_cdeg(0),
get_system_errors(0),
get_authentication_state(0),
get_jamming_state(0),
get_spoofing_state(0));
}

#if GPS_MAX_RECEIVERS > 1
Expand Down
80 changes: 80 additions & 0 deletions libraries/AP_GPS/AP_GPS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -155,6 +155,46 @@ class AP_GPS
GPS_OK_FIX_3D_RTK_FIXED = 6, ///< Receiving valid messages and 3D RTK Fixed
};

/// GPS error bits. These are kept aligned with MAVLink by static_assert
/// in AP_GPS.cpp
enum GPS_Errors {
NONE = 0,
INCOMING_CORRECTIONS = 1 << 0,
CONFIGURATION = 1 << 1,
SOFTWARE = 1 << 2,
ANTENNA = 1 << 3,
EVENT_CONGESTION = 1 << 4,
CPU_OVERLOAD = 1 << 5,
OUTPUT_CONGESTION = 1 << 6,
};

/// GPS authentication status. These are kept aligned with MAVLink by static_assert
/// in AP_GPS.cpp
enum GPS_Authentication {
AUTHENTICATION_UNKNOWN = 0,
AUTHENTICATION_INITIALIZING = 1,
AUTHENTICATION_ERROR = 2,
AUTHENTICATION_OK = 3,
AUTHENTICATION_DISABLED = 4,
};

/// GPS jamming status. These are kept aligned with MAVLink by static_assert
/// in AP_GPS.cpp
enum GPS_Jamming {
JAMMING_UNKNOWN = 0,
JAMMING_OK = 1,
JAMMING_DETECTED = 2
};

/// GPS spoofing status. These are kept aligned with MAVLink by static_assert
/// in AP_GPS.cpp
enum GPS_Spoofing {
SPOOFING_UNKNOWN = 0,
SPOOFING_OK = 1,
SPOOFING_DETECTED = 2,
SPOOFING_MITIGATED = 3,
};

// GPS navigation engine settings. Not all GPS receivers support
// this
enum GPS_Engine_Setting {
Expand Down Expand Up @@ -222,6 +262,10 @@ class AP_GPS
uint64_t last_corrected_gps_time_us;///< the system time we got the last corrected GPS timestamp, microseconds
bool corrected_timestamp_updated; ///< true if the corrected timestamp has been updated
uint32_t lagged_sample_count; ///< number of samples with 50ms more lag than expected
uint32_t system_errors; ///< system errors
uint8_t authentication_state; ///< authentication state of GNSS signals
uint8_t jamming_state; ///< jamming state of GNSS signals
uint8_t spoofing_state; ///< spoofing state of GNSS signals

// all the following fields must only all be filled by RTK capable backend drivers
uint32_t rtk_time_week_ms; ///< GPS Time of Week of last baseline in milliseconds
Expand Down Expand Up @@ -429,6 +473,42 @@ class AP_GPS
return get_hdop(primary_instance);
}

// general errors in the GPS system
uint32_t get_system_errors(uint8_t instance) const {
return state[instance].system_errors;
}

uint32_t get_system_errors() const {
return get_system_errors(primary_instance);
}

// authentication state of GNSS signals
uint8_t get_authentication_state(uint8_t instance) const {
return state[instance].authentication_state;
}

uint8_t get_authentication_state() const {
return get_authentication_state(primary_instance);
}

// jamming state of GNSS signals
uint8_t get_jamming_state(uint8_t instance) const {
return state[instance].jamming_state;
}

uint8_t get_jamming_state() const {
return get_jamming_state(primary_instance);
}

// spoofing state of GNSS signals
uint8_t get_spoofing_state(uint8_t instance) const {
return state[instance].spoofing_state;
}

uint8_t get_spoofing_state() const {
return get_spoofing_state(primary_instance);
}

// vertical dilution of precision
uint16_t get_vdop(uint8_t instance) const {
return state[instance].vdop;
Expand Down
81 changes: 81 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_STATUS_STREAM_NUMBER
#define GPS_SBF_STATUS_STREAM_NUMBER 3
#endif

#define SBF_EXCESS_COMMAND_BYTES 5 // 2 start bytes + validity byte + space byte + endline byte

Expand Down Expand Up @@ -138,6 +141,13 @@ AP_GPS_SBF::read(void)
config_string = nullptr;
}
break;
case Config_State::SSO_Status:
if (asprintf(&config_string, "sso,Stream%d,COM%d,GalAuthStatus+RFStatus+QualityInd+ReceiverStatus,sec1\n",
(int)GPS_SBF_STATUS_STREAM_NUMBER,
(int)params.com_port) == -1) {
config_string = nullptr;
}
break;
case Config_State::Blob:
if (asprintf(&config_string, "%s\n", _initialisation_blob[_init_blob_index]) == -1) {
config_string = nullptr;
Expand Down Expand Up @@ -364,6 +374,9 @@ AP_GPS_SBF::parse(uint8_t temp)
config_step = Config_State::SSO;
break;
case Config_State::SSO:
config_step = Config_State::SSO_Status;
break;
case Config_State::SSO_Status:
config_step = Config_State::Blob;
break;
case Config_State::Blob:
Expand Down Expand Up @@ -519,6 +532,74 @@ AP_GPS_SBF::process_message(void)
(unsigned int)(RxError & RX_ERROR_MASK), (unsigned int)(temp.RxError & RX_ERROR_MASK));
}
RxError = temp.RxError;
ExtError = temp.ExtError;
state.system_errors = AP_GPS::GPS_Errors::NONE;
if (ExtError & CORRECTION)
state.system_errors |= AP_GPS::GPS_Errors::INCOMING_CORRECTIONS;
if (RxError & INVALIDCONFIG)
state.system_errors |= AP_GPS::GPS_Errors::CONFIGURATION;
if (RxError & SOFTWARE)
state.system_errors |= AP_GPS::GPS_Errors::SOFTWARE;
if (RxError & ANTENNA)
state.system_errors |= AP_GPS::GPS_Errors::ANTENNA;
if (RxError & MISSEDEVENT)
state.system_errors |= AP_GPS::GPS_Errors::EVENT_CONGESTION;
if (RxError & CPUOVERLOAD)
state.system_errors |= AP_GPS::GPS_Errors::CPU_OVERLOAD;
if (RxError & CONGESTION)
state.system_errors |= AP_GPS::GPS_Errors::OUTPUT_CONGESTION;
break;
}
case RFStatus:
{
const msg4092 &temp = sbf_msg.data.msg4092u;
check_new_itow(temp.TOW, sbf_msg.length);

state.spoofing_state = AP_GPS::GPS_Spoofing::SPOOFING_OK;
state.jamming_state = AP_GPS::GPS_Jamming::JAMMING_OK;

for (int i = 0; i < temp.N; i++) {
RFStatusRFBandSubBlock* rf_band_data = (RFStatusRFBandSubBlock*)(&temp.RFBand + i * temp.SBLength);
switch (rf_band_data->Info & (uint8_t)0b111) {
case 2:
// As long as there is indicated but unmitigated spoofing in one band, don't report the overall state as mitigated
if (state.spoofing_state == AP_GPS::GPS_Spoofing::SPOOFING_OK) {
state.jamming_state = AP_GPS::GPS_Jamming::JAMMING_OK;
state.spoofing_state = AP_GPS::GPS_Spoofing::SPOOFING_MITIGATED;
}
break;
case 8:
state.jamming_state = AP_GPS::GPS_Jamming::JAMMING_DETECTED;
state.spoofing_state = AP_GPS::GPS_Spoofing::SPOOFING_DETECTED;
}
}

break;
}
case GALAuthStatus:
{
const msg4245 &temp = sbf_msg.data.msg4245u;
check_new_itow(temp.TOW, sbf_msg.length);
switch (temp.OSNMAStatus & (uint16_t)0b111) {
case 0:
state.authentication_state = AP_GPS::GPS_Authentication::AUTHENTICATION_DISABLED;
break;
case 1:
case 2:
state.authentication_state = AP_GPS::GPS_Authentication::AUTHENTICATION_INITIALIZING;
break;
case 3:
case 4:
case 5:
state.authentication_state = AP_GPS::GPS_Authentication::AUTHENTICATION_ERROR;
break;
case 6:
state.authentication_state = AP_GPS::GPS_Authentication::AUTHENTICATION_OK;
break;
default:
state.authentication_state = AP_GPS::GPS_Authentication::AUTHENTICATION_UNKNOWN;
break;
}
break;
}
case VelCovGeodetic:
Expand Down
41 changes: 41 additions & 0 deletions libraries/AP_GPS/AP_GPS_SBF.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,7 @@ class AP_GPS_SBF : public AP_GPS_Backend
enum class Config_State {
Baud_Rate,
SSO,
SSO_Status,
Blob,
SBAS,
SGA,
Expand Down Expand Up @@ -102,6 +103,7 @@ class AP_GPS_SBF : public AP_GPS_Backend
uint32_t crc_error_counter = 0;
uint32_t RxState;
uint32_t RxError;
uint8_t ExtError;

void mount_disk(void) const;
void unmount_disk(void) const;
Expand All @@ -112,6 +114,8 @@ class AP_GPS_SBF : public AP_GPS_Backend
PVTGeodetic = 4007,
ReceiverStatus = 4014,
BaseVectorGeod = 4028,
RFStatus = 4092,
GALAuthStatus = 4245,
VelCovGeodetic = 5908,
AttEulerCov = 5939,
AuxAntPositions = 5942,
Expand Down Expand Up @@ -177,6 +181,36 @@ class AP_GPS_SBF : public AP_GPS_Backend
// remaining data is AGCData, which we don't have a use for, don't extract the data
};

struct PACKED RFStatusRFBandSubBlock
{
uint32_t Frequency;
uint16_t Bandwidth;
uint8_t Info;
};

struct PACKED msg4092 // RFStatus
{
uint32_t TOW;
uint16_t WNc;
uint8_t N;
uint8_t SBLength;
uint8_t Flags;
uint8_t Reserved[3];
uint8_t RFBand; // So we can use address and parse sub-blocks manually
};

struct PACKED msg4245 // GALAuthStatus
{
uint32_t TOW;
uint16_t WNc;
uint16_t OSNMAStatus;
float TrustedTimeDelta;
uint64_t GalActiveMask;
uint64_t GalAuthenticMask;
uint64_t GpsActiveMask;
uint64_t GpsAuthenticMask;
};

struct PACKED VectorInfoGeod {
uint8_t NrSV;
uint8_t Error;
Expand Down Expand Up @@ -264,6 +298,8 @@ class AP_GPS_SBF : public AP_GPS_Backend
msg4001 msg4001u;
msg4014 msg4014u;
msg4028 msg4028u;
msg4092 msg4092u;
msg4245 msg4245u;
msg5908 msg5908u;
msg5939 msg5939u;
msg5942 msg5942u;
Expand Down Expand Up @@ -296,13 +332,18 @@ class AP_GPS_SBF : public AP_GPS_Backend
enum {
SOFTWARE = (1 << 3), // set upon detection of a software warning or error. This bit is reset by the command lif, error
WATCHDOG = (1 << 4), // set when the watch-dog expired at least once since the last power-on.
ANTENNA = (1 << 5), // set when antenna overcurrent condition is detected
CONGESTION = (1 << 6), // set when an output data congestion has been detected on at least one of the communication ports of the receiver during the last second.
MISSEDEVENT = (1 << 8), // set when an external event congestion has been detected during the last second. It indicates that the receiver is receiving too many events on its EVENTx pins.
CPUOVERLOAD = (1 << 9), // set when the CPU load is larger than 90%. If this bit is set, receiver operation may be unreliable and the user must decrease the processing load by following the recommendations in the User Manual.
INVALIDCONFIG = (1 << 10), // set if one or more configuration file (permission or channel configuration) is invalid or absent.
OUTOFGEOFENCE = (1 << 11), // set if the receiver is currently out of its permitted region of operation (geo-fencing).
};

enum {
CORRECTION = (1 << 1), // : set when an anomaly has been detected in an incoming differential correction stream
};

static constexpr const char *portIdentifiers[] = { "COM", "USB", "IP1", "NTR", "IPS", "IPR" };
char portIdentifier[5];
uint8_t portLength;
Expand Down

0 comments on commit e62e725

Please sign in to comment.