AHRS: added AHRS_OPTIONS and jamming simulator

libraries/AP_AHRS/AP_AHRS.cpp

@@ -178,6 +178,13 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = {
 
     // index 17
 
+    // @Param: OPTIONS
+    // @DisplayName: Optional AHRS behaviour
+    // @Description: This controls optional AHRS behaviour. Setting DisableDCMFallbackFW will change the AHRS behaviour for fixed wing aircraft in fly-forward flight to not fall back to DCM when the EKF stops navigating. Setting DisableDCMFallbackVTOL will change the AHRS behaviour for fixed wing aircraft in non fly-forward (VTOL) flight to not fall back to DCM when the EKF stops navigating. 
+    // @Bitmask: 0:DisableDCMFallbackFW, 1:DisableDCMFallbackVTOL
+    // @User: Advanced
+    AP_GROUPINFO("OPTIONS",  18, AP_AHRS, _options, 0),
+
     AP_GROUPEND
 };
 
@@ -1890,21 +1897,11 @@ AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const
     }
 
 #if AP_AHRS_DCM_ENABLED
-    /*
-      fixed wing and rover will fall back to DCM if the EKF doesn't
-      have GPS. This is the safest option as DCM is very robust. Note
-      that we also check the filter status when fly_forward is false
-      and we are disarmed. This is to ensure that the arming checks do
-      wait for good GPS position on fixed wing and rover
-     */
+    // Handle fallback for fixed wing planes (including VTOL's) and ground vehicles.
     if (ret != EKFType::DCM &&
         (_vehicle_class == VehicleClass::FIXED_WING ||
          _vehicle_class == VehicleClass::GROUND)) {
-        if (!dcm.yaw_source_available() && !fly_forward) {
-            // if we don't have a DCM yaw source available and we are
-            // in a non-fly-forward mode then we are best off using the EKF
-            return ret;
-        }
+
         bool should_use_gps = true;
         nav_filter_status filt_state;
 #if HAL_NAVEKF2_AVAILABLE
@@ -1930,26 +1927,53 @@ AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const
             should_use_gps = true;
         }
 #endif
+
+        // Handle fallback for the case where the DCM or EKF is unable to provide attitude or height data.
+        const bool can_use_dcm = dcm.yaw_source_available() || fly_forward;
+        const bool can_use_ekf = filt_state.flags.attitude && filt_state.flags.vert_vel && filt_state.flags.vert_pos;
+        if (!can_use_dcm && can_use_ekf) {
+            // no choice - continue to use EKF
+            return ret;
+        } else if (!can_use_ekf) {
+            // No choice - we have to use DCM
+            return EKFType::DCM;
+        }
+
+        const bool disable_dcm_fallback = fly_forward?
+            option_set(Options::DISABLE_DCM_FALLBACK_FW) : option_set(Options::DISABLE_DCM_FALLBACK_VTOL);
+        if (disable_dcm_fallback) {
+            // don't fallback
+            return ret;
+        }
+
+        // Handle loss of global position when we still have a GPS fix
         if (hal.util->get_soft_armed() &&
-            (!filt_state.flags.using_gps ||
-             !filt_state.flags.horiz_pos_abs) &&
             should_use_gps &&
-            AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
-            // if the EKF is not fusing GPS or doesn't have a 2D fix
-            // and we have a 3D lock, then plane and rover would
-            // prefer to use the GPS position from DCM. This is a
-            // safety net while some issues with the EKF get sorted
-            // out
+            AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D &&
+            (!filt_state.flags.using_gps || !filt_state.flags.horiz_pos_abs)) {
+            /*
+               If the EKF is not fusing GPS or doesn't have a 2D fix and we have a 3D GPS lock,
+               then plane and rover would prefer to use the GPS position from DCM unless the
+               fallback has been inhibited by the user.
+               Note: The aircraft could be dead reckoning with acceptable accuracy and rejecting a bad GPS
+               Note: This is a last resort fallback and makes the navigation highly vulnerable to GPS noise.
+               Note: When operating in a VTOL flight mode that actively controls height such as QHOVER,
+               the EKF gives better vertical velocity and position estimates and height control characteristics.
+            */
             return EKFType::DCM;
         }
+
+        // Handle complete loss of navigation
         if (hal.util->get_soft_armed() && filt_state.flags.const_pos_mode) {
+            /*
+               Provided the EKF has been configured to use GPS, ie should_use_gps is true, then the
+               key difference to the case handled above is only the absence of a GPS fix which means
+               that DCM will not be able to navigate either so we are primarily concerned with
+               providing an attitude, vertical position and vertical velocity estimate.
+            */
             return EKFType::DCM;
         }
-        if (!filt_state.flags.attitude ||
-            !filt_state.flags.vert_vel ||
-            !filt_state.flags.vert_pos) {
-            return EKFType::DCM;
-        }
+
         if (!filt_state.flags.horiz_vel ||
             (!filt_state.flags.horiz_pos_abs && !filt_state.flags.horiz_pos_rel)) {
             if ((!AP::compass().use_for_yaw()) &&

libraries/AP_AHRS/AP_AHRS.h

@@ -991,6 +991,15 @@ class AP_AHRS {
     struct AP_AHRS_Backend::Estimates external_estimates;
 #endif
 
+    enum class Options : uint16_t {
+        DISABLE_DCM_FALLBACK_FW=(1U<<0),
+        DISABLE_DCM_FALLBACK_VTOL=(1U<<1),
+    };
+    AP_Int16 _options;
+
+    bool option_set(Options option) const {
+        return (_options & uint16_t(option)) != 0;
+    }
 };
 
 namespace AP {

libraries/SITL/SIM_GPS.cpp

@@ -87,12 +87,12 @@ ssize_t GPS::write_to_autopilot(const char *p, size_t size) const
 
     size_t ret = 0;
     while (size--) {
-            float r = ((((unsigned)random()) % 1000000)) / 1.0e4;
-            if (r < byteloss) {
-                // lose the byte
-                p++;
-                continue;
-            }
+        float r = ((((unsigned)random()) % 1000000)) / 1.0e4;
+        if (r < byteloss) {
+            // lose the byte
+            p++;
+            continue;
+        }
 
         const ssize_t pret = SerialDevice::write_to_autopilot(p, 1);
         if (pret == 0) {
@@ -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
  */
@@ -262,85 +327,89 @@ void GPS::update()
 
     const uint8_t idx = instance;  // alias to avoid code churn
 
-        struct GPS_Data d {};
+    struct GPS_Data d {};
 
-        // simulate delayed lock times
-        bool have_lock = (!_sitl->gps_disable[idx] && now_ms >= _sitl->gps_lock_time[idx]*1000UL);
+    // simulate delayed lock times
+    bool have_lock = (!_sitl->gps_disable[idx] && now_ms >= _sitl->gps_lock_time[idx]*1000UL);
 
-        // Only let physics run and GPS write at configured GPS rate (default 5Hz).
-        if ((now_ms - last_write_update_ms) < (uint32_t)(1000/_sitl->gps_hertz[instance])) {
-            // Reading runs every iteration.
-            // Beware- physics don't update every iteration with this approach.
-            // Currently, none of the drivers rely on quickly updated physics.
-            backend->update_read();
-            return;
-        }
+    // Only let physics run and GPS write at configured GPS rate (default 5Hz).
+    if ((now_ms - last_write_update_ms) < (uint32_t)(1000/_sitl->gps_hertz[instance])) {
+        // Reading runs every iteration.
+        // Beware- physics don't update every iteration with this approach.
+        // Currently, none of the drivers rely on quickly updated physics.
+        backend->update_read();
+        return;
+    }
 
     last_write_update_ms = now_ms;
 
-        d.latitude = latitude;
-        d.longitude = longitude;
-        d.yaw_deg = _sitl->state.yawDeg;
-        d.roll_deg = _sitl->state.rollDeg;
-        d.pitch_deg = _sitl->state.pitchDeg;
-
-        // add an altitude error controlled by a slow sine wave
-        d.altitude = altitude + _sitl->gps_noise[idx] * sinf(now_ms * 0.0005f) + _sitl->gps_alt_offset[idx];
-
-        // Add offset to c.g. velocity to get velocity at antenna and add simulated error
-        Vector3f velErrorNED = _sitl->gps_vel_err[idx];
-        d.speedN = speedN + (velErrorNED.x * rand_float());
-        d.speedE = speedE + (velErrorNED.y * rand_float()); 
-        d.speedD = speedD + (velErrorNED.z * rand_float());
-        d.have_lock = have_lock;
-
-        if (_sitl->gps_drift_alt[idx] > 0) {
-            // add slow altitude drift controlled by a slow sine wave
-            d.altitude += _sitl->gps_drift_alt[idx]*sinf(now_ms*0.001f*0.02f);
-        }
+    d.latitude = latitude;
+    d.longitude = longitude;
+    d.yaw_deg = _sitl->state.yawDeg;
+    d.roll_deg = _sitl->state.rollDeg;
+    d.pitch_deg = _sitl->state.pitchDeg;
+
+    // add an altitude error controlled by a slow sine wave
+    d.altitude = altitude + _sitl->gps_noise[idx] * sinf(now_ms * 0.0005f) + _sitl->gps_alt_offset[idx];
+
+    // Add offset to c.g. velocity to get velocity at antenna and add simulated error
+    Vector3f velErrorNED = _sitl->gps_vel_err[idx];
+    d.speedN = speedN + (velErrorNED.x * rand_float());
+    d.speedE = speedE + (velErrorNED.y * rand_float());
+    d.speedD = speedD + (velErrorNED.z * rand_float());
+    d.have_lock = have_lock;
+
+    if (_sitl->gps_drift_alt[idx] > 0) {
+        // add slow altitude drift controlled by a slow sine wave
+        d.altitude += _sitl->gps_drift_alt[idx]*sinf(now_ms*0.001f*0.02f);
+    }
 
-        // correct the latitude, longitude, height and NED velocity for the offset between
-        // the vehicle c.g. and GPS antenna
-        Vector3f posRelOffsetBF = _sitl->gps_pos_offset[idx];
-        if (!posRelOffsetBF.is_zero()) {
-            // get a rotation matrix following DCM conventions (body to earth)
-            Matrix3f rotmat;
-            _sitl->state.quaternion.rotation_matrix(rotmat);
-
-            // rotate the antenna offset into the earth frame
-            Vector3f posRelOffsetEF = rotmat * posRelOffsetBF;
-
-            // Add the offset to the latitude, longitude and height using a spherical earth approximation
-            double const earth_rad_inv = 1.569612305760477e-7; // use Authalic/Volumetric radius
-            double lng_scale_factor = earth_rad_inv / cos(radians(d.latitude));
-            d.latitude += degrees(posRelOffsetEF.x * earth_rad_inv);
-            d.longitude += degrees(posRelOffsetEF.y * lng_scale_factor);
-            d.altitude -= posRelOffsetEF.z;
-
-            // calculate a velocity offset due to the antenna position offset and body rotation rate
-            // note: % operator is overloaded for cross product
-            Vector3f gyro(radians(_sitl->state.rollRate),
-                          radians(_sitl->state.pitchRate),
-                          radians(_sitl->state.yawRate));
-            Vector3f velRelOffsetBF = gyro % posRelOffsetBF;
-
-            // rotate the velocity offset into earth frame and add to the c.g. velocity
-            Vector3f velRelOffsetEF = rotmat * velRelOffsetBF;
-            d.speedN += velRelOffsetEF.x;
-            d.speedE += velRelOffsetEF.y;
-            d.speedD += velRelOffsetEF.z;
-        }
+    // correct the latitude, longitude, height and NED velocity for the offset between
+    // the vehicle c.g. and GPS antenna
+    Vector3f posRelOffsetBF = _sitl->gps_pos_offset[idx];
+    if (!posRelOffsetBF.is_zero()) {
+        // get a rotation matrix following DCM conventions (body to earth)
+        Matrix3f rotmat;
+        _sitl->state.quaternion.rotation_matrix(rotmat);
+
+        // rotate the antenna offset into the earth frame
+        Vector3f posRelOffsetEF = rotmat * posRelOffsetBF;
+
+        // Add the offset to the latitude, longitude and height using a spherical earth approximation
+        double const earth_rad_inv = 1.569612305760477e-7; // use Authalic/Volumetric radius
+        double lng_scale_factor = earth_rad_inv / cos(radians(d.latitude));
+        d.latitude += degrees(posRelOffsetEF.x * earth_rad_inv);
+        d.longitude += degrees(posRelOffsetEF.y * lng_scale_factor);
+        d.altitude -= posRelOffsetEF.z;
+
+        // calculate a velocity offset due to the antenna position offset and body rotation rate
+        // note: % operator is overloaded for cross product
+        Vector3f gyro(radians(_sitl->state.rollRate),
+                      radians(_sitl->state.pitchRate),
+                      radians(_sitl->state.yawRate));
+        Vector3f velRelOffsetBF = gyro % posRelOffsetBF;
+
+        // rotate the velocity offset into earth frame and add to the c.g. velocity
+        Vector3f velRelOffsetEF = rotmat * velRelOffsetBF;
+        d.speedN += velRelOffsetEF.x;
+        d.speedE += velRelOffsetEF.y;
+        d.speedD += velRelOffsetEF.z;
+    }
+
+    // get delayed data
+    d.timestamp_ms = now_ms;
+    d = interpolate_data(d, _sitl->gps_delay_ms[instance]);
 
-        // get delayed data
-        d.timestamp_ms = now_ms;
-        d = interpolate_data(d, _sitl->gps_delay_ms[instance]);
+    // Applying GPS glitch
+    // Using first gps glitch
+    Vector3f glitch_offsets = _sitl->gps_glitch[idx];
+    d.latitude += glitch_offsets.x;
+    d.longitude += glitch_offsets.y;
+    d.altitude += glitch_offsets.z;
 
-        // Applying GPS glitch
-        // Using first gps glitch
-        Vector3f glitch_offsets = _sitl->gps_glitch[idx];
-        d.latitude += glitch_offsets.x;
-        d.longitude += glitch_offsets.y;
-        d.altitude += glitch_offsets.z;
+        if (_sitl->gps_jam[idx] == 1) {
+            simulate_jamming(d);
+        }
 
     backend->publish(&d);
 }