diff --git a/src/lib/failure_injection/FailureInjection.cpp b/src/lib/failure_injection/FailureInjection.cpp index 5586992cc1a6..c084f691e787 100644 --- a/src/lib/failure_injection/FailureInjection.cpp +++ b/src/lib/failure_injection/FailureInjection.cpp @@ -110,6 +110,15 @@ bool process_gnss(const Config &config, uint8_t uorb_instance, sensor_gps_s &sen int32_t fix_type = sensor_gps_s::FIX_TYPE_2D; param_get(fix_type_handle, &fix_type); sensor_gps.fix_type = (uint8_t)fix_type; + + static const param_t jamming_state_handle = param_find("SYS_FAIL_GPS_JAM"); + + int32_t jamming_state = sensor_gps_s::JAMMING_STATE_UNKNOWN; + param_get(jamming_state_handle, &jamming_state); + + if (jamming_state != sensor_gps_s::JAMMING_STATE_UNKNOWN) { + sensor_gps.jamming_state = (uint8_t)jamming_state; + } } return true; diff --git a/src/lib/failure_injection/FailureInjection.hpp b/src/lib/failure_injection/FailureInjection.hpp index f8246f9bd6c6..7a0b5046c6ac 100644 --- a/src/lib/failure_injection/FailureInjection.hpp +++ b/src/lib/failure_injection/FailureInjection.hpp @@ -200,7 +200,8 @@ void process_battery(const Config &config, uint8_t instance, battery_status_s &b /** * GNSS counterpart to process(): on FAILURE_UNIT_SENSOR_GPS for the receiver publishing on the * given 0-based uORB instance, Off and Stuck behave as in the generic process() and Wrong reports - * the fix type selected by SYS_FAIL_GPS_WRG while leaving the position untouched. + * the fix type selected by SYS_FAIL_GPS_WRG and the jamming state selected by SYS_FAIL_GPS_JAM + * while leaving the position untouched. * * @param uorb_instance 0-based uORB instance of the publisher (not the 1-based failure instance). * @return false if the sensor_gps publication must be suppressed (Off), true otherwise. diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 0fb4c2194a92..526123a9a74b 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -678,7 +678,7 @@ void EstimatorChecks::checkGps(const Context &context, Report &reporter, const s */ reporter.armingCheckFailure(NavModes::None, health_component_t::gps, events::ID("check_estimator_gps_jamming_critical"), - events::Log::Warning, "GPS jamming detected"); + events::Log::Notice, "GPS jamming detected"); if (reporter.mavlink_log_pub()) { mavlink_log_warning(reporter.mavlink_log_pub(), "GPS jamming detected\t"); diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp index 0d2fe978e75e..e01d0e477643 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp @@ -68,6 +68,30 @@ class GnssChecks final uint16_t value; }; + /** + * Fail-status flags (gps_check_fail_status_u layout) of the checks enabled by EKF2_GPS_CHECK. + * The param bit order (GnssChecksMask) and the status bit order are not parallel, so they are + * mapped one by one: a positional shift silently misassigns every check that is appended to + * one of the two enums but not the other. + */ + uint16_t getEnabledChecksFailStatusMask() const + { + gps_check_fail_status_u mask{}; + mask.flags.fix = isCheckEnabled(GnssChecksMask::kFix); + mask.flags.nsats = isCheckEnabled(GnssChecksMask::kNsats); + mask.flags.pdop = isCheckEnabled(GnssChecksMask::kPdop); + mask.flags.hacc = isCheckEnabled(GnssChecksMask::kHacc); + mask.flags.vacc = isCheckEnabled(GnssChecksMask::kVacc); + mask.flags.sacc = isCheckEnabled(GnssChecksMask::kSacc); + mask.flags.hdrift = isCheckEnabled(GnssChecksMask::kHdrift); + mask.flags.vdrift = isCheckEnabled(GnssChecksMask::kVdrift); + mask.flags.hspeed = isCheckEnabled(GnssChecksMask::kHspd); + mask.flags.vspeed = isCheckEnabled(GnssChecksMask::kVspd); + mask.flags.spoofed = isCheckEnabled(GnssChecksMask::kSpoofed); + mask.flags.jammed = isCheckEnabled(GnssChecksMask::kJammed); + return mask.value; + } + void resetHard() { _initial_checks_passed = false; @@ -113,7 +137,7 @@ class GnssChecks final kJammed = (1 << 11) }; - bool isCheckEnabled(GnssChecksMask check) { return (_params.check_mask & static_cast(check)); } + bool isCheckEnabled(GnssChecksMask check) const { return (_params.check_mask & static_cast(check)); } bool runSimplifiedChecks(const gnssSample &gnss); bool runInitialFixChecks(const gnssSample &gnss); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 558e1dcf66d8..b73c16d09b7d 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -406,6 +406,7 @@ class Ekf final : public EstimatorInterface const GnssChecks::gps_check_fail_status_u &gps_check_fail_status() const { return _gnss_checks.getFailStatus(); } const decltype(GnssChecks::gps_check_fail_status_u::flags) &gps_check_fail_status_flags() const { return _gnss_checks.getFailStatus().flags; } + uint16_t gps_check_fail_status_enabled_mask() const { return _gnss_checks.getEnabledChecksFailStatusMask(); } bool gps_checks_passed() const { return _gnss_checks.passed(); }; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 5a1bbace04bd..091f473da0ff 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1948,9 +1948,8 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) _ekf.getOutputTrackingError().copyTo(status.output_tracking_error); #if defined(CONFIG_EKF2_GNSS) - // only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include - // the GPS Fix bit, which is always checked) - status.gps_check_fail_flags = _ekf.gps_check_fail_status().value & (((uint16_t)_params->ekf2_gps_check << 1) | 1); + // only report enabled GPS check failures + status.gps_check_fail_flags = _ekf.gps_check_fail_status().value & _ekf.gps_check_fail_status_enabled_mask(); #endif // CONFIG_EKF2_GNSS status.control_mode_flags = _ekf.control_status().value; diff --git a/src/modules/failure_injection_manager/FailureInjectionManager.hpp b/src/modules/failure_injection_manager/FailureInjectionManager.hpp index 970523577ead..4d63a31c3282 100644 --- a/src/modules/failure_injection_manager/FailureInjectionManager.hpp +++ b/src/modules/failure_injection_manager/FailureInjectionManager.hpp @@ -105,6 +105,7 @@ class FailureInjectionManager : public ModuleBase, public ModuleParams, public p (ParamInt) _param_sys_fail_rc_inst, // Consumed via param_find() in failure_injection::process_gnss(); registered // here so it is marked used and shows up in the GCS parameter list. - (ParamInt) _param_sys_fail_gps_wrg + (ParamInt) _param_sys_fail_gps_wrg, + (ParamInt) _param_sys_fail_gps_jam ) }; diff --git a/src/modules/failure_injection_manager/failure_injection_manager_params.yaml b/src/modules/failure_injection_manager/failure_injection_manager_params.yaml index 94180afb91a8..38bdfdd0a05e 100644 --- a/src/modules/failure_injection_manager/failure_injection_manager_params.yaml +++ b/src/modules/failure_injection_manager/failure_injection_manager_params.yaml @@ -98,3 +98,19 @@ parameters: 3: "Fix: 3D" 5: "Fix: RTK float" 6: "Fix: RTK fixed" + SYS_FAIL_GPS_JAM: + description: + short: GPS Wrong-failure jamming state + long: |- + GNSS jamming state reported by the addressed receiver while a GPS 'wrong' + failure injection is active. The reported position is left untouched, so + 'Detected' simulates a receiver that still delivers a valid fix while + reporting interference. Leave at 'Unchanged' to keep the simulated + receiver's own jamming state. + type: enum + default: 0 + values: + 0: Unchanged + 1: OK + 2: Mitigated + 3: Detected