AP_GPS: support GNSS receiver resilience information over MAVLink

ArduCopter/GCS_Mavlink.cpp

@@ -514,6 +514,7 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
     MSG_CURRENT_WAYPOINT, // MISSION_CURRENT
     MSG_GPS_RAW,
     MSG_GPS_RTK,
+    MSG_GNSS_INTEGRITY,
 #if GPS_MAX_RECEIVERS > 1
     MSG_GPS2_RAW,
     MSG_GPS2_RTK,

ArduPlane/GCS_Mavlink.cpp

@@ -625,6 +625,7 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
     MSG_CURRENT_WAYPOINT,
     MSG_GPS_RAW,
     MSG_GPS_RTK,
+    MSG_GNSS_INTEGRITY,
 #if GPS_MAX_RECEIVERS > 1
     MSG_GPS2_RAW,
     MSG_GPS2_RTK,

Tools/AP_Periph/GCS_MAVLink.cpp

@@ -35,6 +35,7 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
 #if AP_GPS_ENABLED
     MSG_GPS_RAW,
     MSG_GPS_RTK,
+    MSG_GNSS_INTEGRITY,
 #endif
 };
 

libraries/AP_GPS/AP_GPS.cpp

@@ -111,6 +111,31 @@ 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::GPS_SYSTEM_ERROR_NONE == (uint32_t)0x00, "GPS_SYSTEM_ERROR_NONE incorrect"); //not ideal but state doesn't exist in mavlink
+static_assert((uint32_t)AP_GPS::GPS_Errors::INCOMING_CORRECTIONS == (uint32_t)GPS_SYSTEM_ERROR_INCOMING_CORRECTIONS, "INCOMING_CORRECTIONS incorrect");
+static_assert((uint32_t)AP_GPS::GPS_Errors::CONFIGURATION == (uint32_t)GPS_SYSTEM_ERROR_CONFIGURATION, "CONFIGURATION incorrect");
+static_assert((uint32_t)AP_GPS::GPS_Errors::SOFTWARE == (uint32_t)GPS_SYSTEM_ERROR_SOFTWARE, "SOFTWARE incorrect");
+static_assert((uint32_t)AP_GPS::GPS_Errors::ANTENNA == (uint32_t)GPS_SYSTEM_ERROR_ANTENNA, "ANTENNA incorrect");
+static_assert((uint32_t)AP_GPS::GPS_Errors::EVENT_CONGESTION == (uint32_t)GPS_SYSTEM_ERROR_EVENT_CONGESTION, "EVENT_CONGESTION incorrect");
+static_assert((uint32_t)AP_GPS::GPS_Errors::CPU_OVERLOAD == (uint32_t)GPS_SYSTEM_ERROR_CPU_OVERLOAD, "CPU_OVERLOAD incorrect");
+static_assert((uint32_t)AP_GPS::GPS_Errors::OUTPUT_CONGESTION == (uint32_t)GPS_SYSTEM_ERROR_OUTPUT_CONGESTION, "OUTPUT_CONGESTION inccorect");
+
+static_assert((uint8_t)AP_GPS::GPS_Authentication::AUTHENTICATION_UNKNOWN == (uint8_t)GPS_AUTHENTICATION_STATE_UNKNOWN, "AUTHENTICATION_UNKNOWN incorrect");
+static_assert((uint8_t)AP_GPS::GPS_Authentication::AUTHENTICATION_INITIALIZING == (uint8_t)GPS_AUTHENTICATION_STATE_INITIALIZING, "AUTHENTICATION_INITIALIZING incorrect");
+static_assert((uint8_t)AP_GPS::GPS_Authentication::AUTHENTICATION_ERROR == (uint8_t)GPS_AUTHENTICATION_STATE_ERROR, "AUTHENTICATION_ERROR incorrect");
+static_assert((uint8_t)AP_GPS::GPS_Authentication::AUTHENTICATION_OK == (uint8_t)GPS_AUTHENTICATION_STATE_OK, "GPS_AUTHENTICATION_STATE_OK incorrect");
+static_assert((uint8_t)AP_GPS::GPS_Authentication::AUTHENTICATION_DISABLED == (uint8_t)GPS_AUTHENTICATION_STATE_DISABLED, "AUTHENTICATION_DISABLED incorrect");
+
+static_assert((uint8_t)AP_GPS::GPS_Jamming::JAMMING_UNKNOWN == (uint8_t)GPS_JAMMING_STATE_UNKNOWN, "JAMMING_UNKNOWN incorrect");
+static_assert((uint8_t)AP_GPS::GPS_Jamming::JAMMING_OK == (uint8_t)GPS_JAMMING_STATE_OK, "JAMMING_OK incorrect");
+static_assert((uint8_t)AP_GPS::GPS_Jamming::JAMMING_DETECTED == (uint8_t)GPS_JAMMING_STATE_DETECTED, "JAMMING_DETECTED incorrect");
+static_assert((uint8_t)AP_GPS::GPS_Jamming::JAMMING_MITIGATED == (uint8_t)GPS_JAMMING_STATE_MITIGATED, "JAMMING_MITIGATED incorrect");
+
+static_assert((uint8_t)AP_GPS::GPS_Spoofing::SPOOFING_UNKNOWN == (uint8_t)GPS_SPOOFING_STATE_UNKNOWN, "SPOOFING_UNKNOWN incorrect");
+static_assert((uint8_t)AP_GPS::GPS_Spoofing::SPOOFING_OK == (uint8_t)GPS_SPOOFING_STATE_OK, "SPOOFING_OK incorrect");
+static_assert((uint8_t)AP_GPS::GPS_Spoofing::SPOOFING_DETECTED == (uint8_t)GPS_SPOOFING_STATE_DETECTED, "SPOOFING_DETECTED incorrect");
+static_assert((uint8_t)AP_GPS::GPS_Spoofing::SPOOFING_MITIGATED == (uint8_t)GPS_SPOOFING_STATE_MITIGATED, "SPOOFING_MITIGATED incorrect");
 #endif
 
 // ensure that our own enum-class status is equivalent to the
@@ -1456,6 +1481,35 @@ void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst)
 }
 #endif
 
+void AP_GPS::send_mavlink_gnss_integrity(mavlink_channel_t chan, uint8_t instance) {
+    uint8_t id = instance; 
+    uint32_t system_error = get_system_errors(instance);
+    uint8_t authentication = get_authentication_state(instance);
+    uint8_t jamming = get_jamming_state(instance);
+    uint8_t spoofing = get_spoofing_state(instance);
+    uint8_t raim_state = 255; //not implemented yet
+    uint16_t raim_hfom = 255; //not implemented yet
+    uint16_t raim_vfom = 255; //not implemented yet
+    uint8_t corrections_quality = 255; //not implemented yet
+    uint8_t systems_status_summary = 255; //not implemented yet
+    uint8_t gnss_signal_quality = 255; //not implemented yet
+    uint8_t post_processing_quality = 255; //not implemented yet
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "gps %u with auth %u and spooof %u", id, authentication, spoofing);
+    mavlink_msg_gnss_integrity_send(chan, 
+        id,
+        system_error,
+        authentication,
+        jamming,
+        spoofing,
+        raim_state,
+        raim_hfom,
+        raim_vfom,
+        corrections_quality,
+        systems_status_summary,
+        gnss_signal_quality,
+        post_processing_quality);
+}
+
 bool AP_GPS::first_unconfigured_gps(uint8_t &instance) const
 {
     for (int i = 0; i < GPS_MAX_RECEIVERS; i++) {

libraries/AP_GPS/AP_GPS.h

@@ -156,6 +156,49 @@ 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 {
+        GPS_SYSTEM_ERROR_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_MITIGATED = 2,
+        JAMMING_DETECTED =  3,
+
+    };
+
+    /// 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_MITIGATED = 2,
+        SPOOFING_DETECTED =  3,
+
+    };
+
     // GPS navigation engine settings. Not all GPS receivers support
     // this
     enum GPS_Engine_Setting {
@@ -223,6 +266,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
@@ -430,6 +477,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;
@@ -505,6 +588,7 @@ class AP_GPS
     void send_mavlink_gps2_raw(mavlink_channel_t chan);
 
     void send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst);
+    void send_mavlink_gnss_integrity(mavlink_channel_t chan, uint8_t inst);
 
     // Returns true if there is an unconfigured GPS, and provides the instance number of the first non configured GPS
     bool first_unconfigured_gps(uint8_t &instance) const WARN_IF_UNUSED;

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_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
 
@@ -91,9 +94,9 @@ AP_GPS_SBF::read(void)
     uint32_t available_bytes = port->available();
     for (uint32_t i = 0; i < available_bytes; i++) {
         uint8_t temp = port->read();
-#if AP_GPS_DEBUG_LOGGING_ENABLED
+    #if AP_GPS_DEBUG_LOGGING_ENABLED
         log_data(&temp, 1);
-#endif
+    #endif
         ret |= parse(temp);
     }
 
@@ -139,16 +142,20 @@ AP_GPS_SBF::read(void)
                                 }
                                 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;
-                                    }
+                                //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;
+                                }*/
+                            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:
@@ -377,9 +384,17 @@ 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 {
+                                        config_step = Config_State::SSO_Status;
+                                    }
                                     break;
                                 case Config_State::Constellation:
+                                    config_step = Config_State::SSO_Status;
+                                    break;
+                                case Config_State::SSO_Status:
                                     config_step = Config_State::Blob;
                                     break;
                                 case Config_State::Blob:
@@ -548,6 +563,85 @@ 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::GPS_SYSTEM_ERROR_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);
+        #if HAL_GCS_ENABLED
+        if (temp.Flags==0) {
+            state.spoofing_state = AP_GPS::GPS_Spoofing::SPOOFING_OK;
+        }
+        else {
+            state.spoofing_state = AP_GPS::GPS_Spoofing::SPOOFING_DETECTED;
+        }
+        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 1:
+            case 2:
+                // As long as there is indicated but unmitigated spoofing in one band, don't report the overall state as mitigated
+                if (state.jamming_state == AP_GPS::GPS_Jamming::JAMMING_OK) {
+                    state.jamming_state = AP_GPS::GPS_Jamming::JAMMING_MITIGATED;
+                }
+                break;
+            case 8:
+                state.jamming_state = AP_GPS::GPS_Jamming::JAMMING_DETECTED;
+                break;
+            }
+        }
+        break;
+        #endif
+    }
+    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: