SITL: added simple simulated jammer

libraries/SITL/SIM_GPS.cpp

@@ -129,6 +129,71 @@ void GPS_Backend::simulation_timeval(struct timeval *tv)
     tv->tv_usec = new_usec % 1000000ULL;
 }
 
+/*
+  simple simulation of jamming
+ */
+void GPS::simulate_jamming(struct GPS_Data &d)
+{
+    auto &jam = jamming[instance];
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - jam.last_jam_ms > 1000) {
+        jam.jam_start_ms = now_ms;
+        jam.latitude = d.latitude;
+        jam.longitude = d.longitude;
+    }
+    jam.last_jam_ms = now_ms;
+
+    // how often each of the key state variables change during jamming
+    const float vz_change_hz = 0.5;
+    const float vel_change_hz = 0.8;
+    const float pos_change_hz = 1.1;
+    const float sats_change_hz = 3;
+    const float acc_change_hz = 3;
+
+    if (now_ms - jam.jam_start_ms < unsigned(1000U+(get_random16()%5000))) {
+        // total loss of signal for a period at the start is common
+        d.num_sats = 0;
+        d.have_lock = false;
+    } else {
+        if ((now_ms - jam.last_sats_change_ms)*0.001 > 2*fabsf(rand_float())/sats_change_hz) {
+            jam.last_sats_change_ms = now_ms;
+            d.num_sats = 2 + (get_random16() % 15);
+            if (d.num_sats >= 4) {
+                if (get_random16() % 2 == 0) {
+                    d.have_lock = false;
+                } else {
+                    d.have_lock = true;
+                }
+            } else {
+                d.have_lock = false;
+            }
+        }
+        if ((now_ms - jam.last_vz_change_ms)*0.001 > 2*fabsf(rand_float())/vz_change_hz) {
+            jam.last_vz_change_ms = now_ms;
+            d.speedD = rand_float() * 400;
+        }
+        if ((now_ms - jam.last_vel_change_ms)*0.001 > 2*fabsf(rand_float())/vel_change_hz) {
+            jam.last_vel_change_ms = now_ms;
+            d.speedN = rand_float() * 400;
+            d.speedE = rand_float() * 400;
+        }
+        if ((now_ms - jam.last_pos_change_ms)*0.001 > 2*fabsf(rand_float())/pos_change_hz) {
+            jam.last_pos_change_ms = now_ms;
+            jam.latitude += rand_float()*200 * LATLON_TO_M_INV * 1e-7;
+            jam.longitude += rand_float()*200 * LATLON_TO_M_INV * 1e-7;
+        }
+        if ((now_ms - jam.last_acc_change_ms)*0.001 > 2*fabsf(rand_float())/acc_change_hz) {
+            jam.last_acc_change_ms = now_ms;
+            d.vertical_acc = fabsf(rand_float())*300;
+            d.horizontal_acc = fabsf(rand_float())*300;
+            d.speed_acc = fabsf(rand_float())*50;
+        }
+    }
+
+    d.latitude = constrain_float(jam.latitude, -90, 90);
+    d.longitude = constrain_float(jam.longitude, -180, 180);
+}
+
 /*
   return GPS time of week
  */
@@ -342,6 +407,10 @@ void GPS::update()
     d.longitude += glitch_offsets.y;
     d.altitude += glitch_offsets.z;
 
+        if (_sitl->gps_jam[idx] == 1) {
+            simulate_jamming(d);
+        }
+
     backend->publish(&d);
 }
 

libraries/SITL/SIM_GPS.h

@@ -44,6 +44,10 @@ struct GPS_Data {
     double roll_deg;
     double pitch_deg;
     bool have_lock;
+    float horizontal_acc;
+    float vertical_acc;
+    float speed_acc;
+    uint8_t num_sats;
 
     // Get heading [rad], where 0 = North in WGS-84 coordinate system
     float heading() const WARN_IF_UNUSED;
@@ -141,9 +145,24 @@ private:
     // last 20 samples, allowing for up to 20 samples of delay
     GPS_Data _gps_history[20];
 
+    // state of jamming simulation
+    struct {
+        uint32_t last_jam_ms;
+        uint32_t jam_start_ms;
+        uint32_t last_sats_change_ms;
+        uint32_t last_vz_change_ms;
+        uint32_t last_vel_change_ms;
+        uint32_t last_pos_change_ms;
+        uint32_t last_acc_change_ms;
+        double latitude;
+        double longitude;
+    } jamming[2];
+
     bool _gps_has_basestation_position;
     GPS_Data _gps_basestation_data;
 
+    void simulate_jamming(GPS_Data &d);
+
     // get delayed data
     GPS_Data interpolate_data(const GPS_Data &d, uint32_t delay_ms);
 

libraries/SITL/SITL.cpp

@@ -569,6 +569,12 @@ const AP_Param::GroupInfo SIM::var_gps[] = {
     // @Vector3Parameter: 1
     // @User: Advanced
     AP_GROUPINFO("GPS_VERR",      15, SIM,  gps_vel_err[0], 0),
+    // @Param: GPS_JAM
+    // @DisplayName: GPS jamming enable
+    // @Description: Enable simulated GPS jamming
+    // @User: Advanced
+    // @Values: 0:Disabled, 1:Enabled
+    AP_GROUPINFO("GPS_JAM",       16, SIM,  gps_jam[0], 0),
     // @Param: GPS2_DISABLE
     // @DisplayName: GPS 2 disable
     // @Description: Disables GPS 2
@@ -672,6 +678,13 @@ const AP_Param::GroupInfo SIM::var_gps[] = {
     // @Description: Log number for GPS:update_file()
     AP_GROUPINFO("GPS_LOG_NUM",   48, SIM,  gps_log_num, 0),
 
+    // @Param: GPS2_JAM
+    // @DisplayName: GPS jamming enable
+    // @Description: Enable simulated GPS jamming
+    // @User: Advanced
+    // @Values: 0:Disabled, 1:Enabled
+    AP_GROUPINFO("GPS2_JAM",      49, SIM,  gps_jam[1], 0),
+
     AP_GROUPEND
 };
 #endif  // HAL_SIM_GPS_ENABLED

libraries/SITL/SITL.h

@@ -206,6 +206,7 @@ public:
     AP_Vector3f gps_pos_offset[2];  // XYZ position of the GPS antenna phase centre relative to the body frame origin (m)
     AP_Float gps_accuracy[2];
     AP_Vector3f gps_vel_err[2]; // Velocity error offsets in NED (x = N, y = E, z = D)
+    AP_Int8 gps_jam[2]; // jamming simulation enable
 
     // initial offset on GPS lat/lon, used to shift origin
     AP_Float gps_init_lat_ofs;