From 28bb9556cbbd5b105a61bdcac0d5df72b3a56ae7 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Thu, 13 Aug 2026 16:27:52 +0200 Subject: [PATCH] feat(failure-injection): implement GNSS failure injection handling and add GPS wrong-failure fix type parameter (#28139) --- docs/en/debug/failure_injection.md | 5 +- src/drivers/gnss/septentrio/septentrio.cpp | 3 +- src/drivers/gps/gps.cpp | 3 +- src/drivers/uavcan/sensors/gnss.cpp | 3 +- .../failure_injection/FailureInjection.cpp | 24 +++ .../failure_injection/FailureInjection.hpp | 14 ++ .../FailureInjectionConfigTest.cpp | 150 ++++++++++++++++++ .../FailureInjectionManager.hpp | 5 +- .../failure_injection_manager_params.yaml | 18 +++ .../sensor_gps_sim/SensorGpsSim.cpp | 27 +--- .../sensor_gps_sim/SensorGpsSim.hpp | 17 +- .../simulator_mavlink/SimulatorMavlink.cpp | 139 +++++++--------- .../simulator_mavlink/SimulatorMavlink.hpp | 9 +- 13 files changed, 292 insertions(+), 125 deletions(-) diff --git a/docs/en/debug/failure_injection.md b/docs/en/debug/failure_injection.md index cc7357e17bc..5b05f45caea 100644 --- a/docs/en/debug/failure_injection.md +++ b/docs/en/debug/failure_injection.md @@ -5,7 +5,7 @@ This enables easier testing of [safety failsafe](../config/safety.md) behaviour, Failure injection is disabled by default, and can be enabled using the [SYS_FAILURE_EN](../advanced_config/parameter_reference.md#SYS_FAILURE_EN) parameter. -Failures can be injected both in simulation and on real hardware. In simulation the available failures depend on the simulator. On hardware the `off` (stop publishing) and `stuck` (freeze the last value) types are supported for the `gyro`, `accel`, `mag`, `baro`, `distance_sensor` and `gps` components; this requires firmware built with the failure-injection module. In addition, the `battery` component supports `off` (report a depleted pack, triggering the battery failsafe). +Failures can be injected both in simulation and on real hardware. In simulation the available failures depend on the simulator. On hardware the `off` (stop publishing) and `stuck` (freeze the last value) types are supported for the `gyro`, `accel`, `mag`, `baro`, `distance_sensor` and `gps` components; this requires firmware built with the failure-injection module. In addition, the `battery` component supports `off` (report a depleted pack, triggering the battery failsafe), and the `gps` component supports `wrong` (report the fix type selected by [SYS_FAIL_GPS_WRG](../advanced_config/parameter_reference.md#SYS_FAIL_GPS_WRG), leaving the reported position untouched). ::: info PX4 may accept a command to set a particular failure mode even it that mode is not supported by your simulator. @@ -61,7 +61,8 @@ where: - _instance bitmask_ (optional): address several instances at once (bit 0 = first instance, bit 1 = second, …; decimal or `0x` hex). Used only when `-i` is omitted. Example: `-m 0x5` targets instances 1 and 3. ::: info -The simulated GPS (SITL) implements only the `off`, `stuck`, and `wrong` failure modes; the other failure types have no effect on it. +GPS implements only the `off`, `stuck`, and `wrong` failure modes; the other failure types have no effect on it. +`gps wrong` makes the addressed receiver report the fix type selected by [SYS_FAIL_GPS_WRG](../advanced_config/parameter_reference.md#SYS_FAIL_GPS_WRG) and leaves the reported position untouched. ::: ## RC Switch Trigger diff --git a/src/drivers/gnss/septentrio/septentrio.cpp b/src/drivers/gnss/septentrio/septentrio.cpp index df62c2fb198..5764b142dae 100644 --- a/src/drivers/gnss/septentrio/septentrio.cpp +++ b/src/drivers/gnss/septentrio/septentrio.cpp @@ -1739,8 +1739,7 @@ void SeptentrioDriver::publish() _failure_config.update(); - if (!failure_injection::process(_failure_config, failure_injection_s::FAILURE_UNIT_SENSOR_GPS, - _sensor_gps_pub.get_instance(), _sensor_gps, _stuck)) { + if (!failure_injection::process_gnss(_failure_config, _sensor_gps_pub.get_instance(), _sensor_gps, _stuck)) { return; } diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 9e5e4d52da2..619c7612b1d 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -1609,8 +1609,7 @@ GPS::publish() _failure_config.update(); - if (!failure_injection::process(_failure_config, failure_injection_s::FAILURE_UNIT_SENSOR_GPS, - _sensor_gps_pub.get_instance(), _sensor_gps, _stuck)) { + if (!failure_injection::process_gnss(_failure_config, _sensor_gps_pub.get_instance(), _sensor_gps, _stuck)) { return; } diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index 0e5c75e00b8..7f907176289 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -622,8 +622,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure const int instance = get_orb_instance_for_node(node_id); if (channel >= 0 && instance >= 0 - && !failure_injection::process(_failure_config, failure_injection_s::FAILURE_UNIT_SENSOR_GPS, - instance, sensor_gps, _stuck[channel])) { + && !failure_injection::process_gnss(_failure_config, instance, sensor_gps, _stuck[channel])) { return; } diff --git a/src/lib/failure_injection/FailureInjection.cpp b/src/lib/failure_injection/FailureInjection.cpp index 5c2ea87f082..5586992cc1a 100644 --- a/src/lib/failure_injection/FailureInjection.cpp +++ b/src/lib/failure_injection/FailureInjection.cpp @@ -35,7 +35,9 @@ #if defined(CONFIG_MODULES_FAILURE_INJECTION_MANAGER) +#include #include +#include namespace failure_injection { @@ -91,6 +93,28 @@ void process_battery(const Config &config, uint8_t instance, battery_status_s &b battery_status.warning = battery_status_s::WARNING_EMERGENCY; } +bool process_gnss(const Config &config, uint8_t uorb_instance, sensor_gps_s &sensor_gps, + Stuck &stuck) +{ + const Mode mode = config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_GPS, uorb_instance + 1); + + // Off and Stuck are message-agnostic; run them first so the Stuck cache keeps the + // uncorrupted sample and a later Stuck replays a healthy fix. + if (!process(mode, sensor_gps, stuck)) { + return false; + } + + if (mode == Mode::Wrong) { + static const param_t fix_type_handle = param_find("SYS_FAIL_GPS_WRG"); + + 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; + } + + return true; +} + esc_status_s process_esc(const Config &config, const esc_status_s &status) { esc_status_s result = status; diff --git a/src/lib/failure_injection/FailureInjection.hpp b/src/lib/failure_injection/FailureInjection.hpp index 1d8e8cdea63..f8246f9bd6c 100644 --- a/src/lib/failure_injection/FailureInjection.hpp +++ b/src/lib/failure_injection/FailureInjection.hpp @@ -53,6 +53,7 @@ #include struct battery_status_s; +struct sensor_gps_s; namespace failure_injection { @@ -196,6 +197,17 @@ inline bool process(const Config &config, uint8_t unit, uint8_t uorb_instance) */ void process_battery(const Config &config, uint8_t instance, battery_status_s &battery_status); +/** + * 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. + * + * @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. + */ +bool process_gnss(const Config &config, uint8_t uorb_instance, sensor_gps_s &sensor_gps, + Stuck &stuck); + /** * ESC counterpart to process(): apply the active FAILURE_UNIT_SYSTEM_ESC failures to a copy of * status (matched per ESC by actuator_function). Off zeroes the ESC's telemetry (keeping only @@ -233,6 +245,8 @@ inline bool process(const Config &, uint8_t, uint8_t) { return true; } inline void process_battery(const Config &, uint8_t, battery_status_s &) {} +inline bool process_gnss(const Config &, uint8_t, sensor_gps_s &, Stuck &) { return true; } + inline esc_status_s process_esc(const Config &, const esc_status_s &status) { return status; } #endif // CONFIG_MODULES_FAILURE_INJECTION_MANAGER diff --git a/src/modules/failure_injection_manager/FailureInjectionConfigTest.cpp b/src/modules/failure_injection_manager/FailureInjectionConfigTest.cpp index 3c1c3de834d..d1f204d8b56 100644 --- a/src/modules/failure_injection_manager/FailureInjectionConfigTest.cpp +++ b/src/modules/failure_injection_manager/FailureInjectionConfigTest.cpp @@ -39,7 +39,9 @@ #include #include +#include #include +#include // FailureInjection.hpp transitively pulls in px4_platform_common/defines.h (via // uORB Subscription), which defines an OK macro that would clash with the local @@ -76,6 +78,28 @@ failure_injection_s make_config(uint8_t unit, uint16_t instance_mask, uint8_t fa return cfg; } +// Healthy 3D-fix sample, as a GNSS driver would publish it. +sensor_gps_s clean_gps() +{ + sensor_gps_s gps{}; + gps.timestamp = 1000; + gps.timestamp_sample = 900; + gps.fix_type = sensor_gps_s::FIX_TYPE_3D; + gps.latitude_deg = 47.0; + gps.longitude_deg = 8.0; + gps.altitude_msl_m = 500.0; + gps.altitude_ellipsoid_m = 500.0; + gps.eph = 0.9f; + gps.epv = 1.78f; + gps.satellites_used = 25; + gps.vel_n_m_s = 1.f; + gps.vel_e_m_s = 2.f; + gps.vel_d_m_s = 0.5f; + gps.vel_m_s = 2.236f; + gps.vel_ned_valid = true; + return gps; +} + constexpr uint8_t GYRO = failure_injection_s::FAILURE_UNIT_SENSOR_GYRO; constexpr uint8_t GPS = failure_injection_s::FAILURE_UNIT_SENSOR_GPS; constexpr uint8_t MOTOR = failure_injection_s::FAILURE_UNIT_SYSTEM_MOTOR; @@ -269,6 +293,132 @@ TEST(FailureInjectionConfig, ProcessMessageLessSuppressesOnlyOff) EXPECT_TRUE(process(config, GYRO, 0)); } +// =========================================================================== +// process_gnss() +// =========================================================================== + +TEST(FailureInjectionConfig, ProcessGnssWrongSetsConfiguredFixType) +{ + // The test runner has no work queue for the autosave param_set would schedule. + param_control_autosave(false); + + Config config; + config.set(make_config(GPS, 0x1, WRONG)); // uORB instance 0 -> failure instance 1 + + const int32_t fix_types[] = { + sensor_gps_s::FIX_TYPE_2D, + sensor_gps_s::FIX_TYPE_NONE, + sensor_gps_s::FIX_TYPE_RTK_FLOAT, + sensor_gps_s::FIX_TYPE_RTK_FIXED, + }; + + for (const int32_t expected : fix_types) { + ASSERT_EQ(param_set(param_find("SYS_FAIL_GPS_WRG"), &expected), 0); + + Stuck stuck; + sensor_gps_s gps = clean_gps(); + + EXPECT_TRUE(process_gnss(config, 0, gps, stuck)); + EXPECT_EQ(gps.fix_type, (uint8_t)expected); + } + + const int32_t default_fix_type = sensor_gps_s::FIX_TYPE_2D; + param_set(param_find("SYS_FAIL_GPS_WRG"), &default_fix_type); +} + +TEST(FailureInjectionConfig, ProcessGnssWrongLeavesPositionUntouched) +{ + param_control_autosave(false); + + const int32_t fix_type = sensor_gps_s::FIX_TYPE_2D; + ASSERT_EQ(param_set(param_find("SYS_FAIL_GPS_WRG"), &fix_type), 0); + + Config config; + config.set(make_config(GPS, 0x1, WRONG)); + + Stuck stuck; + const sensor_gps_s truth = clean_gps(); + sensor_gps_s gps = truth; + + EXPECT_TRUE(process_gnss(config, 0, gps, stuck)); + + // Only the fix type changes: the reported solution stays coherent with the truth. + EXPECT_EQ(gps.fix_type, (uint8_t)sensor_gps_s::FIX_TYPE_2D); + EXPECT_DOUBLE_EQ(gps.latitude_deg, truth.latitude_deg); + EXPECT_DOUBLE_EQ(gps.longitude_deg, truth.longitude_deg); + EXPECT_DOUBLE_EQ(gps.altitude_msl_m, truth.altitude_msl_m); + EXPECT_DOUBLE_EQ(gps.altitude_ellipsoid_m, truth.altitude_ellipsoid_m); + EXPECT_FLOAT_EQ(gps.vel_n_m_s, truth.vel_n_m_s); + EXPECT_FLOAT_EQ(gps.vel_e_m_s, truth.vel_e_m_s); + EXPECT_FLOAT_EQ(gps.vel_d_m_s, truth.vel_d_m_s); + EXPECT_FLOAT_EQ(gps.vel_m_s, truth.vel_m_s); + EXPECT_FLOAT_EQ(gps.eph, truth.eph); + EXPECT_EQ(gps.satellites_used, truth.satellites_used); +} + +TEST(FailureInjectionConfig, ProcessGnssWrongLeavesUnselectedInstanceUntouched) +{ + param_control_autosave(false); + + const int32_t fix_type = sensor_gps_s::FIX_TYPE_2D; + ASSERT_EQ(param_set(param_find("SYS_FAIL_GPS_WRG"), &fix_type), 0); + + Config config; + config.set(make_config(GPS, 0x2, WRONG)); // failure instance 2 -> uORB instance 1 + + Stuck stuck; + sensor_gps_s gps = clean_gps(); + + EXPECT_TRUE(process_gnss(config, 0, gps, stuck)); + EXPECT_EQ(gps.fix_type, (uint8_t)sensor_gps_s::FIX_TYPE_3D); + + // The addressed instance is degraded. + Stuck stuck_1; + sensor_gps_s gps_1 = clean_gps(); + + EXPECT_TRUE(process_gnss(config, 1, gps_1, stuck_1)); + EXPECT_EQ(gps_1.fix_type, (uint8_t)sensor_gps_s::FIX_TYPE_2D); +} + +TEST(FailureInjectionConfig, ProcessGnssOffSuppressesPublication) +{ + Config config; + config.set(make_config(GPS, 0x1, OFF)); + + Stuck stuck; + sensor_gps_s gps = clean_gps(); + + EXPECT_FALSE(process_gnss(config, 0, gps, stuck)); + // Off suppresses instead of mutating. + EXPECT_EQ(gps.fix_type, (uint8_t)sensor_gps_s::FIX_TYPE_3D); +} + +TEST(FailureInjectionConfig, ProcessGnssStuckReplaysLastGoodSample) +{ + Config config; + Stuck stuck; + + // A healthy cycle records the last good sample. + sensor_gps_s good = clean_gps(); + EXPECT_TRUE(process_gnss(config, 0, good, stuck)); + + config.set(make_config(GPS, 0x1, STUCK)); + + sensor_gps_s moved = clean_gps(); + moved.timestamp = 2000; + moved.timestamp_sample = 1900; + moved.latitude_deg = 48.0; + moved.longitude_deg = 9.0; + + EXPECT_TRUE(process_gnss(config, 0, moved, stuck)); + + // The frozen position comes back, with the live timestamps. + EXPECT_DOUBLE_EQ(moved.latitude_deg, good.latitude_deg); + EXPECT_DOUBLE_EQ(moved.longitude_deg, good.longitude_deg); + EXPECT_EQ(moved.timestamp, 2000u); + EXPECT_EQ(moved.timestamp_sample, 1900u); +} + // =========================================================================== // process_esc(): ESC Off / Wrong on the multi-instance esc_status // =========================================================================== diff --git a/src/modules/failure_injection_manager/FailureInjectionManager.hpp b/src/modules/failure_injection_manager/FailureInjectionManager.hpp index 1e8d3706c69..970523577ea 100644 --- a/src/modules/failure_injection_manager/FailureInjectionManager.hpp +++ b/src/modules/failure_injection_manager/FailureInjectionManager.hpp @@ -102,6 +102,9 @@ private: (ParamInt) _param_sys_fail_rc_src, (ParamInt) _param_sys_fail_rc_unit, (ParamInt) _param_sys_fail_rc_mode, - (ParamInt) _param_sys_fail_rc_inst + (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 ) }; 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 273218f036c..94180afb91a 100644 --- a/src/modules/failure_injection_manager/failure_injection_manager_params.yaml +++ b/src/modules/failure_injection_manager/failure_injection_manager_params.yaml @@ -80,3 +80,21 @@ parameters: max: 16 default: 1 reboot_required: true + SYS_FAIL_GPS_WRG: + description: + short: GPS Wrong-failure fix type + long: |- + GNSS fix type reported by the addressed receiver while a GPS 'wrong' + failure injection is active. The reported position is left untouched. + The default 2D fix is rejected by the estimator, which requires a 3D + fix, and makes the GNSS redundancy check report a lost fix, while the + receiver stays eligible for GPS blending. Values above 3D fix report a + better solution than the receiver really has. + type: enum + default: 2 + values: + 1: "Fix: None" + 2: "Fix: 2D" + 3: "Fix: 3D" + 5: "Fix: RTK float" + 6: "Fix: RTK fixed" diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp index 2d75b44afdd..ca9a3a5f1cb 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp @@ -198,7 +198,7 @@ void SensorGpsSim::Run() sensor_gps.vel_ned_valid = true; sensor_gps.satellites_used = _sim_gps_used.get(); - publishWithFailures(0, sensor_gps, _last_gps0, _sensor_gps_pub); + publishWithFailures(0, sensor_gps, _sensor_gps_pub); const float gps1_offx = _param_gps1_offx.get(); const float gps1_offy = _param_gps1_offy.get(); @@ -212,33 +212,22 @@ void SensorGpsSim::Run() gps1.latitude_deg = latitude + (double)gps1_offx / CONSTANTS_RADIUS_OF_EARTH * (180.0 / M_PI); gps1.longitude_deg = longitude + (double)gps1_offy / CONSTANTS_RADIUS_OF_EARTH * (180.0 / M_PI) / cos(latitude * M_PI / 180.0); - publishWithFailures(1, gps1, _last_gps1, _sensor_gps_pub2); + publishWithFailures(1, gps1, _sensor_gps_pub2); } } perf_end(_loop_perf); } -void SensorGpsSim::publishWithFailures(int instance, sensor_gps_s gps, sensor_gps_s &snapshot, - uORB::PublicationMulti &pub) +void SensorGpsSim::publishWithFailures(int instance, sensor_gps_s gps, uORB::PublicationMulti &pub) { - // Precedence when multiple failure masks are set: BLOCKED > STUCK > WRONG. - if (!isBlocked(instance)) { - if (isStuck(instance)) { - snapshot.timestamp = hrt_absolute_time(); - pub.publish(snapshot); + gps.timestamp = hrt_absolute_time(); - } else { - if (isWrong(instance)) { - gps.latitude_deg += 1.0; - gps.longitude_deg += 1.0; - } - - gps.timestamp = hrt_absolute_time(); - snapshot = gps; - pub.publish(gps); - } + if (!failure_injection::process_gnss(_failure_config, instance, gps, _stuck[instance])) { + return; } + + pub.publish(gps); } void SensorGpsSim::updateFailureConfig() diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp index 3a8b361b890..8b595b335da 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp @@ -77,17 +77,7 @@ private: void updateFailureConfig(); - void publishWithFailures(int instance, sensor_gps_s gps, sensor_gps_s &snapshot, - uORB::PublicationMulti &pub); - - // instance is 0-based here; the failure_injection topic addresses 1-based instances. - failure_injection::Mode failureMode(int instance) const - { - return _failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_GPS, instance + 1); - } - bool isBlocked(int instance) const { return failureMode(instance) == failure_injection::Mode::Off; } - bool isStuck(int instance) const { return failureMode(instance) == failure_injection::Mode::Stuck; } - bool isWrong(int instance) const { return failureMode(instance) == failure_injection::Mode::Wrong; } + void publishWithFailures(int instance, sensor_gps_s gps, uORB::PublicationMulti &pub); // generate white Gaussian noise sample with std=1 static float generate_wgn(); @@ -104,10 +94,9 @@ private: perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + // Failure injection (FAILURE_UNIT_SENSOR_GPS): active config + per-instance last-good sample. failure_injection::Config _failure_config; - - sensor_gps_s _last_gps0{}; - sensor_gps_s _last_gps1{}; + failure_injection::Stuck _stuck[GPS_MAX_INSTANCES]; // GPS Markov process noise state float _gps_pos_noise_n{0.0f}; diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 7b16572f316..810c4e0dffd 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -391,99 +391,87 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg) mavlink_hil_gps_t hil_gps; mavlink_msg_hil_gps_decode(msg, &hil_gps); - if (!_gps_blocked) { - sensor_gps_s gps{}; + sensor_gps_s gps{}; - if (!_gps_stuck) { - if (!_gps_wrong) { - gps.latitude_deg = hil_gps.lat / 1e7; - gps.longitude_deg = hil_gps.lon / 1e7; - gps.altitude_msl_m = hil_gps.alt / 1e3; - gps.altitude_ellipsoid_m = hil_gps.alt / 1e3; + gps.latitude_deg = hil_gps.lat / 1e7; + gps.longitude_deg = hil_gps.lon / 1e7; + gps.altitude_msl_m = hil_gps.alt / 1e3; + gps.altitude_ellipsoid_m = hil_gps.alt / 1e3; - } else { - gps.latitude_deg = hil_gps.lat / 1e7 + 1.0; - gps.longitude_deg = hil_gps.lon / 1e7 + 1.0; - gps.altitude_msl_m = hil_gps.alt / 1e3 + 100.0; - gps.altitude_ellipsoid_m = hil_gps.alt / 1e3 - 100.0; - } + gps.s_variance_m_s = 0.25f; + gps.c_variance_rad = 0.5f; + gps.fix_type = hil_gps.fix_type; - gps.s_variance_m_s = 0.25f; - gps.c_variance_rad = 0.5f; - gps.fix_type = hil_gps.fix_type; + gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m + gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m - gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m - gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m + gps.hdop = 0; // TODO + gps.vdop = 0; // TODO - gps.hdop = 0; // TODO - gps.vdop = 0; // TODO + gps.noise_per_ms = 0; + gps.automatic_gain_control = 0; + gps.jamming_indicator = 0; + gps.jamming_state = 0; + gps.spoofing_state = 0; - gps.noise_per_ms = 0; - gps.automatic_gain_control = 0; - gps.jamming_indicator = 0; - gps.jamming_state = 0; - gps.spoofing_state = 0; + gps.vel_m_s = (float)(hil_gps.vel) / 100.0f; // cm/s -> m/s + gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; // cm/s -> m/s + gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f; // cm/s -> m/s + gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f; // cm/s -> m/s - if (!_gps_wrong) { - gps.vel_m_s = (float)(hil_gps.vel) / 100.0f; // cm/s -> m/s - gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; // cm/s -> m/s - gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f; // cm/s -> m/s - gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f; // cm/s -> m/s + gps.cog_rad = ((hil_gps.cog == 65535) ? NAN : matrix::wrap_2pi(math::radians(hil_gps.cog * 1e-2f))); // cdeg -> rad + gps.vel_ned_valid = true; - } else { - gps.vel_m_s = (float)(hil_gps.vel) / 100.0f - 1.f; // cm/s -> m/s - gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f + 5.f; // cm/s -> m/s - gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f - 8.f; // cm/s -> m/s - gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f + 2.f; // cm/s -> m/s - } + gps.timestamp_time_relative = 0; + gps.time_utc_usec = hil_gps.time_usec; - gps.cog_rad = ((hil_gps.cog == 65535) ? NAN : matrix::wrap_2pi(math::radians(hil_gps.cog * 1e-2f))); // cdeg -> rad - gps.vel_ned_valid = true; + gps.satellites_used = hil_gps.satellites_visible; - gps.timestamp_time_relative = 0; - gps.time_utc_usec = hil_gps.time_usec; + gps.heading = NAN; + gps.heading_offset = NAN; - gps.satellites_used = hil_gps.satellites_visible; + gps.timestamp = hrt_absolute_time(); - gps.heading = NAN; - gps.heading_offset = NAN; + // Resolve the uORB instance for this HIL_GPS id first, so the failure injection state can be + // kept per instance. New publishers are created based on the HIL_GPS ID's being different or not. + int instance = -1; - _gps_prev = gps; - - } else { - gps = _gps_prev; + for (size_t i = 0; i < sizeof(_gps_ids) / sizeof(_gps_ids[0]); i++) { + if (_sensor_gps_pubs[i] && _gps_ids[i] == hil_gps.id) { + instance = i; + break; } - gps.timestamp = hrt_absolute_time(); - - // New publishers will be created based on the HIL_GPS ID's being different or not - for (size_t i = 0; i < sizeof(_gps_ids) / sizeof(_gps_ids[0]); i++) { - if (_sensor_gps_pubs[i] && _gps_ids[i] == hil_gps.id) { - _sensor_gps_pubs[i]->publish(gps); - break; - } + if (_sensor_gps_pubs[i] == nullptr) { + _sensor_gps_pubs[i] = new uORB::PublicationMulti {ORB_ID(sensor_gps)}; if (_sensor_gps_pubs[i] == nullptr) { - _sensor_gps_pubs[i] = new uORB::PublicationMulti {ORB_ID(sensor_gps)}; - - if (_sensor_gps_pubs[i] == nullptr) { - break; - } - - _gps_ids[i] = hil_gps.id; - - device::Device::DeviceId device_id; - device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; - device_id.devid_s.bus = 0; - device_id.devid_s.address = i; - device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; - gps.device_id = device_id.devid; - - _sensor_gps_pubs[i]->publish(gps); - break; + return; } + + _gps_ids[i] = hil_gps.id; + + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + device_id.devid_s.bus = 0; + device_id.devid_s.address = i; + device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; + gps.device_id = device_id.devid; + + instance = i; + break; } } + + if (instance < 0) { + return; + } + + if (!failure_injection::process_gnss(_failure_config, (uint8_t)instance, gps, _gps_stuck[instance])) { + return; + } + + _sensor_gps_pubs[instance]->publish(gps); } void SimulatorMavlink::handle_message_hil_sensor(const mavlink_message_t *msg) @@ -1463,11 +1451,6 @@ void SimulatorMavlink::updateFailureConfig() { _failure_config.update(); - const failure_injection::Mode gps_mode = _failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_GPS, 1); - _gps_blocked = (gps_mode == failure_injection::Mode::Off); - _gps_stuck = (gps_mode == failure_injection::Mode::Stuck); - _gps_wrong = (gps_mode == failure_injection::Mode::Wrong); - const failure_injection::Mode airspeed_mode = _failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_AIRSPEED, 1); _airspeed_disconnected = (airspeed_mode == failure_injection::Mode::Off); diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp index 1cd9f82de3b..9716ca320f8 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp @@ -311,11 +311,10 @@ private: hrt_abstime _last_accel_fifo_timestamp{0}; hrt_abstime _last_gyro_fifo_timestamp{0}; - // GPS, airspeed and VIO failure injection (no PX4* helper class applies these). - bool _gps_blocked{false}; - bool _gps_stuck{false}; - bool _gps_wrong{false}; - sensor_gps_s _gps_prev{}; + // Per-HIL_GPS-instance last-good sample, for the Stuck failure. + failure_injection::Stuck _gps_stuck[MAX_GPS]; + + // airspeed and VIO failure injection (no PX4* helper class applies these). bool _airspeed_disconnected{false}; hrt_abstime _airspeed_blocked_timestamp{0}; bool _vio_blocked{false};