fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 08:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink sim: add support of failure gps struck

Browse Source
This commit is contained in:
bresch
2025-07-15 09:24:40 +02:00
parent 83c7184f8c
commit e9b4bbf2a6
2 changed files with 41 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
+39 -27
View File
@@ -413,41 +413,48 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg)
if (!_gps_blocked) { if (!_gps_blocked) {
sensor_gps_s gps{}; sensor_gps_s gps{};
gps.latitude_deg = hil_gps.lat / 1e7; if (!_gps_stuck) {
gps.longitude_deg = hil_gps.lon / 1e7; gps.latitude_deg = hil_gps.lat / 1e7;
gps.altitude_msl_m = hil_gps.alt / 1e3; gps.longitude_deg = hil_gps.lon / 1e7;
gps.altitude_ellipsoid_m = hil_gps.alt / 1e3; gps.altitude_msl_m = hil_gps.alt / 1e3;
gps.altitude_ellipsoid_m = hil_gps.alt / 1e3;
gps.s_variance_m_s = 0.25f; gps.s_variance_m_s = 0.25f;
gps.c_variance_rad = 0.5f; gps.c_variance_rad = 0.5f;
gps.fix_type = hil_gps.fix_type; gps.fix_type = hil_gps.fix_type;
gps.eph = (float)hil_gps.eph * 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.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
gps.hdop = 0; // TODO gps.hdop = 0; // TODO
gps.vdop = 0; // TODO gps.vdop = 0; // TODO
gps.noise_per_ms = 0; gps.noise_per_ms = 0;
gps.automatic_gain_control = 0; gps.automatic_gain_control = 0;
gps.jamming_indicator = 0; gps.jamming_indicator = 0;
gps.jamming_state = 0; gps.jamming_state = 0;
gps.spoofing_state = 0; gps.spoofing_state = 0;
gps.vel_m_s = (float)(hil_gps.vel) / 100.0f; // cm/s -> m/s 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_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_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.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.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_ned_valid = true;
gps.timestamp_time_relative = 0; gps.timestamp_time_relative = 0;
gps.time_utc_usec = hil_gps.time_usec; 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 = NAN;
gps.heading_offset = NAN; gps.heading_offset = NAN;
_gps_prev = gps;
} else {
gps = _gps_prev;
}
gps.timestamp = hrt_absolute_time(); gps.timestamp = hrt_absolute_time();
@@ -1248,6 +1255,11 @@ void SimulatorMavlink::check_failure_injections()
PX4_INFO("CMD_INJECT_FAILURE, GPS ok"); PX4_INFO("CMD_INJECT_FAILURE, GPS ok");
supported = true; supported = true;
_gps_blocked = false; _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) { } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_ACCEL) {
src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp
+2
View File
@@ -294,6 +294,8 @@ private:
bool _mag_stuck[MAG_COUNT_MAX] {}; bool _mag_stuck[MAG_COUNT_MAX] {};
bool _gps_blocked{false}; bool _gps_blocked{false};
bool _gps_stuck{false};
sensor_gps_s _gps_prev{};
bool _airspeed_disconnected{false}; bool _airspeed_disconnected{false};
hrt_abstime _airspeed_blocked_timestamp{0}; hrt_abstime _airspeed_blocked_timestamp{0};
bool _vio_blocked{false}; bool _vio_blocked{false};