From e9b4bbf2a6cd46e7b5faaf1b00f1d5f8c49ff80d Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 4 Apr 2025 11:42:32 +0200 Subject: [PATCH] mavlink sim: add support of failure gps struck --- .../simulator_mavlink/SimulatorMavlink.cpp | 66 +++++++++++-------- .../simulator_mavlink/SimulatorMavlink.hpp | 2 + 2 files changed, 41 insertions(+), 27 deletions(-) diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 92c5fe73a5..a9a8269e9f 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -413,41 +413,48 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg) if (!_gps_blocked) { sensor_gps_s gps{}; - 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; + if (!_gps_stuck) { + 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.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 - 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.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; - gps.timestamp_time_relative = 0; - gps.time_utc_usec = hil_gps.time_usec; + gps.timestamp_time_relative = 0; + gps.time_utc_usec = hil_gps.time_usec; - gps.satellites_used = hil_gps.satellites_visible; + gps.satellites_used = hil_gps.satellites_visible; - gps.heading = NAN; - gps.heading_offset = NAN; + gps.heading = NAN; + gps.heading_offset = NAN; + + _gps_prev = gps; + + } else { + gps = _gps_prev; + } gps.timestamp = hrt_absolute_time(); @@ -1248,6 +1255,11 @@ void SimulatorMavlink::check_failure_injections() PX4_INFO("CMD_INJECT_FAILURE, GPS ok"); supported = true; _gps_blocked = false; + _gps_stuck = false; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + supported = true; + _gps_stuck = true; } } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_ACCEL) { diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp index 291148b9cb..9ef81fdd55 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp @@ -294,6 +294,8 @@ private: bool _mag_stuck[MAG_COUNT_MAX] {}; bool _gps_blocked{false}; + bool _gps_stuck{false}; + sensor_gps_s _gps_prev{}; bool _airspeed_disconnected{false}; hrt_abstime _airspeed_blocked_timestamp{0}; bool _vio_blocked{false};