diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 4e061a465ec5d..312471f0e9f5b 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/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()) && diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 5ea8ae2ed953d..0f5c985eeb1a7 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/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 { diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp index 2d8d6dbdb525d..53f858a94634a 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/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); } diff --git a/libraries/SITL/SIM_GPS.h b/libraries/SITL/SIM_GPS.h index 0cbda828ec1d6..63e4b99441c85 100644 --- a/libraries/SITL/SIM_GPS.h +++ b/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 @@ class GPS : public SerialDevice { // 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); diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index c516c5acb8b9d..2ff2220aa7ed7 100644 --- a/libraries/SITL/SITL.cpp +++ b/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 diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index ade17ae2e854b..24dbdac91a6ae 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -206,6 +206,7 @@ class SIM { 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;