diff --git a/msg/versioned/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg index 8f20ec2d85..789ecd6cf4 100644 --- a/msg/versioned/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -140,6 +140,7 @@ uint8 FAILURE_UNIT_SENSOR_OPTICAL_FLOW = 5 uint8 FAILURE_UNIT_SENSOR_VIO = 6 uint8 FAILURE_UNIT_SENSOR_DISTANCE_SENSOR = 7 uint8 FAILURE_UNIT_SENSOR_AIRSPEED = 8 +uint8 FAILURE_UNIT_SENSOR_GPS_ALT = 9 uint8 FAILURE_UNIT_SYSTEM_BATTERY = 100 uint8 FAILURE_UNIT_SYSTEM_MOTOR = 101 uint8 FAILURE_UNIT_SYSTEM_SERVO = 102 @@ -155,6 +156,7 @@ uint8 FAILURE_TYPE_WRONG = 4 uint8 FAILURE_TYPE_SLOW = 5 uint8 FAILURE_TYPE_DELAYED = 6 uint8 FAILURE_TYPE_INTERMITTENT = 7 +uint8 FAILURE_TYPE_DRIFT = 8 # Used as param1 in DO_CHANGE_SPEED command. uint8 SPEED_TYPE_AIRSPEED = 0 diff --git a/src/modules/simulation/failure_injection/failureInjection.cpp b/src/modules/simulation/failure_injection/failureInjection.cpp new file mode 100644 index 0000000000..1c15ef2c31 --- /dev/null +++ b/src/modules/simulation/failure_injection/failureInjection.cpp @@ -0,0 +1,439 @@ + +#include + +void FailureInjection::test() +{ + printf("test\n"); +} + +bool FailureInjection::handle_gps_failure(sensor_gps_s &gps) +{ + if (_gps_blocked) { + return false; + } + + if (!_gps_stuck) { + + if (_gps_wrong) { + gps.latitude_deg += 1.0; + gps.longitude_deg += 1.0; + gps.altitude_msl_m += 100.0; + gps.altitude_ellipsoid_m += 100.0; + + gps.vel_m_s -= 1.f; + gps.vel_n_m_s += 5.f; + gps.vel_e_m_s -= 8.f; + gps.vel_d_m_s += 2.f; + } + + + if (_gps_drift) { + if (!_has_drift_ref) { + + _mp.initReference(gps.latitude_deg, gps.longitude_deg); + // Project current LL to local meters to anchor the reference point + _mp.project(gps.latitude_deg, gps.longitude_deg, _gps_drift_pos(0), _gps_drift_pos(1)); + _has_drift_ref = true; + } + + matrix::Vector2f vel_gps(gps.vel_n_m_s, gps.vel_e_m_s); + const float vel_norm = vel_gps.norm(); + + // directional drift by 5% + if (vel_norm > 1.f) { + vel_gps *= 1.f + RELATIVE_GPS_DRIFT; + + } else if (vel_norm > 0.01f) { + vel_gps *= 0.5f / vel_norm; + + } else { + vel_gps(0) = 0.5f; + } + + // perpendicular drift by 5% + matrix::Vector2f vel_normal = vel_gps.normalized(); + vel_gps(0) += vel_norm * RELATIVE_GPS_DRIFT * vel_normal(1); + vel_gps(1) -= vel_norm * RELATIVE_GPS_DRIFT * vel_normal(0); + + gps.vel_n_m_s = vel_gps(0); + gps.vel_e_m_s = vel_gps(1); + + if (_last_gps_timestamp > 0) { + float dt = 1e-6f * (gps.timestamp - _last_gps_timestamp); + _gps_drift_pos += vel_gps * dt; + _mp.reproject(_gps_drift_pos(0), _gps_drift_pos(1), gps.latitude_deg, gps.longitude_deg); + } + + } + + + } else { + gps = _gps_prev; + } + + _last_gps_timestamp = gps.timestamp; + _gps_prev = gps; + + + return true; + +} + +bool FailureInjection::handle_gps_alt_failure(sensor_gps_s &gps) +{ + if (_gps_alt_blocked) { + return false; + } + + if (!_gps_alt_stuck) { + + if (_gps_alt_wrong) { + gps.altitude_msl_m += 100.0; + gps.altitude_ellipsoid_m += 100.0; + + } + + if (_gps_alt_drift) { + if (_t0 == 0) { + _t0 = gps.timestamp; + } + + if (_gps_alt_offset < 0.01) { _gps_alt_offset = gps.altitude_msl_m; } + + gps.vel_d_m_s = 5. * sin(2 * M_PI / 20. * 1e-6 * (gps.timestamp - _t0)); + + if (_last_gps_timestamp > 0) { + _gps_alt_offset += -(double)(gps.vel_d_m_s * (gps.timestamp - _last_gps_timestamp) / 1.e6f); + } + + gps.altitude_msl_m = _gps_alt_offset; + + } + + + } else { + gps = _gps_prev; + } + + _last_gps_timestamp = gps.timestamp; + _gps_prev = gps; + + return true; + +} + + +void FailureInjection::check_failure_injections() +{ + vehicle_command_s vehicle_command; + + while (_vehicle_command_sub.update(&vehicle_command)) { + if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) { + continue; + } + + bool handled = false; + bool supported = false; + + const int failure_unit = static_cast(vehicle_command.param1 + 0.5f); + const int failure_type = static_cast(vehicle_command.param2 + 0.5f); + const int instance = static_cast(vehicle_command.param3 + 0.5f); + + if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GPS) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, GPS off"); + supported = true; + _gps_blocked = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, GPS ok"); + supported = true; + _gps_blocked = false; + _gps_stuck = false; + _gps_wrong = false; + _gps_drift = false; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + supported = true; + _gps_stuck = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) { + supported = true; + _gps_wrong = true; + // } + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_DRIFT) { + PX4_INFO("CMD_INJECT_FAILURE, GPsS drift"); + supported = true; + _gps_drift = true; + + } + + } if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GPS_ALT) { + + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, GPS off"); + supported = true; + _gps_blocked = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, GPS ok"); + supported = true; + _gps_blocked = false; + _gps_stuck = false; + _gps_wrong = false; + _gps_drift = false; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + supported = true; + _gps_stuck = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) { + supported = true; + _gps_wrong = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_DRIFT) { + PX4_INFO("CMD_INJECT_FAILURE, GPS drift"); + supported = true; + _gps_drift = true; + } + + + + } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_ACCEL) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + supported = true; + + // 0 to signal all + if (instance == 0) { + for (int i = 0; i < ACCEL_COUNT_MAX; i++) { + PX4_WARN("CMD_INJECT_FAILURE, accel %d off", i); + _accel_blocked[i] = true; + _accel_stuck[i] = false; + } + + } else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) { + PX4_WARN("CMD_INJECT_FAILURE, accel %d off", instance - 1); + _accel_blocked[instance - 1] = true; + _accel_stuck[instance - 1] = false; + } + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + supported = true; + + // 0 to signal all + if (instance == 0) { + for (int i = 0; i < ACCEL_COUNT_MAX; i++) { + PX4_WARN("CMD_INJECT_FAILURE, accel %d stuck", i); + _accel_blocked[i] = false; + _accel_stuck[i] = true; + } + + } else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) { + PX4_WARN("CMD_INJECT_FAILURE, accel %d stuck", instance - 1); + _accel_blocked[instance - 1] = false; + _accel_stuck[instance - 1] = true; + } + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + supported = true; + + // 0 to signal all + if (instance == 0) { + for (int i = 0; i < ACCEL_COUNT_MAX; i++) { + PX4_INFO("CMD_INJECT_FAILURE, accel %d ok", i); + _accel_blocked[i] = false; + _accel_stuck[i] = false; + } + + } else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) { + PX4_INFO("CMD_INJECT_FAILURE, accel %d ok", instance - 1); + _accel_blocked[instance - 1] = false; + _accel_stuck[instance - 1] = false; + } + } + } + + // } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GYRO) { + // handled = true; + + // if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + // supported = true; + + // // 0 to signal all + // if (instance == 0) { + // for (int i = 0; i < GYRO_COUNT_MAX; i++) { + // PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", i); + // _gyro_blocked[i] = true; + // _gyro_stuck[i] = false; + // } + + // } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { + // PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", instance - 1); + // _gyro_blocked[instance - 1] = true; + // _gyro_stuck[instance - 1] = false; + // } + + // } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + // supported = true; + + // // 0 to signal all + // if (instance == 0) { + // for (int i = 0; i < GYRO_COUNT_MAX; i++) { + // PX4_WARN("CMD_INJECT_FAILURE, gyro %d stuck", i); + // _gyro_blocked[i] = false; + // _gyro_stuck[i] = true; + // } + + // } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { + // PX4_INFO("CMD_INJECT_FAILURE, gyro %d stuck", instance - 1); + // _gyro_blocked[instance - 1] = false; + // _gyro_stuck[instance - 1] = true; + // } + + // } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + // supported = true; + + // // 0 to signal all + // if (instance == 0) { + // for (int i = 0; i < GYRO_COUNT_MAX; i++) { + // PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", i); + // _gyro_blocked[i] = false; + // _gyro_stuck[i] = false; + // } + + // } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { + // PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", instance - 1); + // _gyro_blocked[instance - 1] = false; + // _gyro_stuck[instance - 1] = false; + // } + // } + + // } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_MAG) { + // handled = true; + + // if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + // supported = true; + + // // 0 to signal all + // if (instance == 0) { + // for (int i = 0; i < MAG_COUNT_MAX; i++) { + // PX4_WARN("CMD_INJECT_FAILURE, mag %d off", i); + // _mag_blocked[i] = true; + // _mag_stuck[i] = false; + // } + + // } else if (instance >= 1 && instance <= MAG_COUNT_MAX) { + // PX4_WARN("CMD_INJECT_FAILURE, mag %d off", instance - 1); + // _mag_blocked[instance - 1] = true; + // _mag_stuck[instance - 1] = false; + // } + + // } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + // supported = true; + + // // 0 to signal all + // if (instance == 0) { + // for (int i = 0; i < MAG_COUNT_MAX; i++) { + // PX4_WARN("CMD_INJECT_FAILURE, mag %d stuck", i); + // _mag_blocked[i] = false; + // _mag_stuck[i] = true; + // } + + // } else if (instance >= 1 && instance <= MAG_COUNT_MAX) { + // PX4_WARN("CMD_INJECT_FAILURE, mag %d stuck", instance - 1); + // _mag_blocked[instance - 1] = false; + // _mag_stuck[instance - 1] = true; + // } + + // } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + // supported = true; + + // // 0 to signal all + // if (instance == 0) { + // for (int i = 0; i < MAG_COUNT_MAX; i++) { + // PX4_WARN("CMD_INJECT_FAILURE, mag %d ok", i); + // _mag_blocked[i] = false; + // _mag_stuck[i] = false; + // } + + // } else if (instance >= 1 && instance <= MAG_COUNT_MAX) { + // PX4_WARN("CMD_INJECT_FAILURE, mag %d ok", instance - 1); + // _mag_blocked[instance - 1] = false; + // _mag_stuck[instance - 1] = false; + // } + // } + + // } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_BARO) { + // handled = true; + + // if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + // PX4_WARN("CMD_INJECT_FAILURE, baro off"); + // supported = true; + // _baro_blocked = true; + + // } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + // PX4_WARN("CMD_INJECT_FAILURE, baro stuck"); + // supported = true; + // _baro_stuck = true; + // _baro_blocked = false; + + // } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + // PX4_INFO("CMD_INJECT_FAILURE, baro ok"); + // supported = true; + // _baro_blocked = false; + // } + + // } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_AIRSPEED) { + // handled = true; + + // if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + // PX4_WARN("CMD_INJECT_FAILURE, airspeed off"); + // supported = true; + // _airspeed_disconnected = true; + + // } else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) { + // PX4_WARN("CMD_INJECT_FAILURE, airspeed wrong (simulate pitot blockage)"); + // supported = true; + // _airspeed_blocked_timestamp = hrt_absolute_time(); + + // } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + // PX4_INFO("CMD_INJECT_FAILURE, airspeed ok"); + // supported = true; + // _airspeed_disconnected = false; + // _airspeed_blocked_timestamp = 0; + // } + + // } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_VIO) { + // handled = true; + + // if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + // PX4_WARN("CMD_INJECT_FAILURE, vio off"); + // supported = true; + // _vio_blocked = true; + + // } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + // PX4_INFO("CMD_INJECT_FAILURE, vio ok"); + // supported = true; + // _vio_blocked = false; + // } + // } + + if (handled) { + vehicle_command_ack_s ack{}; + ack.command = vehicle_command.command; + ack.from_external = false; + ack.result = supported ? + vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : + vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + ack.timestamp = hrt_absolute_time(); + _command_ack_pub.publish(ack); + } + } +} diff --git a/src/modules/simulation/failure_injection/failureInjection.hpp b/src/modules/simulation/failure_injection/failureInjection.hpp new file mode 100644 index 0000000000..9f0db0b6ed --- /dev/null +++ b/src/modules/simulation/failure_injection/failureInjection.hpp @@ -0,0 +1,76 @@ + + +// #include +// #include +// #include +// #include +// #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +static constexpr uint8_t ACCEL_COUNT_MAX = 4; +static constexpr uint8_t GYRO_COUNT_MAX = 4; +static constexpr uint8_t MAG_COUNT_MAX = 4; +static constexpr uint8_t BARO_COUNT_MAX = 4; + + +static constexpr float RELATIVE_GPS_DRIFT = 0.1f; + +class FailureInjection +{ + +public: + FailureInjection() {}; + void test(); + + + void check_failure_injections(); + bool handle_gps_failure(sensor_gps_s &gps); + bool handle_gps_alt_failure(sensor_gps_s &gps); + + bool is_accel_blocked(const int instance=0) { return _accel_blocked[instance]; } + bool is_accel_stuck(const int instance=0) { return _accel_stuck[instance]; } + +private: + + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + + + + // GPS + bool _gps_drift{false}; + bool _gps_alt_drift{false}; + MapProjection _mp; + bool _has_drift_ref{false}; + matrix::Vector2f _gps_drift_pos; + double _gps_alt_offset{0}; + hrt_abstime _last_gps_timestamp{0}; + hrt_abstime _t0{0}; + + bool _gps_blocked{false}; + bool _gps_alt_blocked{false}; + bool _gps_stuck{false}; + bool _gps_alt_stuck{false}; + bool _gps_wrong{false}; + bool _gps_alt_wrong{false}; + sensor_gps_s _gps_prev{}; + + + // accel + bool _accel_blocked[ACCEL_COUNT_MAX] {}; + bool _accel_stuck[ACCEL_COUNT_MAX] {}; + +}; diff --git a/src/modules/simulation/simulator_mavlink/CMakeLists.txt b/src/modules/simulation/simulator_mavlink/CMakeLists.txt index f5f39d7106..ec065bd16a 100644 --- a/src/modules/simulation/simulator_mavlink/CMakeLists.txt +++ b/src/modules/simulation/simulator_mavlink/CMakeLists.txt @@ -41,9 +41,11 @@ px4_add_module( INCLUDES ${CMAKE_BINARY_DIR}/mavlink ${CMAKE_BINARY_DIR}/mavlink/${CONFIG_MAVLINK_DIALECT} + ${CMAKE_CURRENT_SOURCE_DIR}/../failure_injection SRCS SimulatorMavlink.cpp SimulatorMavlink.hpp + ${CMAKE_CURRENT_SOURCE_DIR}/../failure_injection/failureInjection.cpp DEPENDS mavlink_c_generate conversion diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 342cfcd4e7..ef3748bacf 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -410,70 +410,49 @@ 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.timestamp = hrt_absolute_time(); + 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; + gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; + gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f; + gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f; - 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 - } 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.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.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(); + if (_failure_injection.handle_gps_failure(gps) && _failure_injection.handle_gps_alt_failure(gps)) { // 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++) { @@ -1056,7 +1035,7 @@ void SimulatorMavlink::send() if (fds_actuator_outputs[0].revents & POLLIN) { // Got new data to read, update all topics. parameters_update(false); - check_failure_injections(); + _failure_injection.check_failure_injections(); _vehicle_status_sub.update(&_vehicle_status); _battery_status_sub.update(&_battery_status); @@ -1244,279 +1223,6 @@ void SimulatorMavlink::run() } } -void SimulatorMavlink::check_failure_injections() -{ - vehicle_command_s vehicle_command; - - while (_vehicle_command_sub.update(&vehicle_command)) { - if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) { - continue; - } - - bool handled = false; - bool supported = false; - - const int failure_unit = static_cast(vehicle_command.param1 + 0.5f); - const int failure_type = static_cast(vehicle_command.param2 + 0.5f); - const int instance = static_cast(vehicle_command.param3 + 0.5f); - - if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GPS) { - handled = true; - - if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - PX4_WARN("CMD_INJECT_FAILURE, GPS off"); - supported = true; - _gps_blocked = true; - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - PX4_INFO("CMD_INJECT_FAILURE, GPS ok"); - supported = true; - _gps_blocked = false; - _gps_stuck = false; - _gps_wrong = false; - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { - supported = true; - _gps_stuck = true; - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) { - supported = true; - _gps_wrong = true; - } - - } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_ACCEL) { - handled = true; - - if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < ACCEL_COUNT_MAX; i++) { - PX4_WARN("CMD_INJECT_FAILURE, accel %d off", i); - _accel_blocked[i] = true; - _accel_stuck[i] = false; - } - - } else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) { - PX4_WARN("CMD_INJECT_FAILURE, accel %d off", instance - 1); - _accel_blocked[instance - 1] = true; - _accel_stuck[instance - 1] = false; - } - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < ACCEL_COUNT_MAX; i++) { - PX4_WARN("CMD_INJECT_FAILURE, accel %d stuck", i); - _accel_blocked[i] = false; - _accel_stuck[i] = true; - } - - } else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) { - PX4_WARN("CMD_INJECT_FAILURE, accel %d stuck", instance - 1); - _accel_blocked[instance - 1] = false; - _accel_stuck[instance - 1] = true; - } - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < ACCEL_COUNT_MAX; i++) { - PX4_INFO("CMD_INJECT_FAILURE, accel %d ok", i); - _accel_blocked[i] = false; - _accel_stuck[i] = false; - } - - } else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) { - PX4_INFO("CMD_INJECT_FAILURE, accel %d ok", instance - 1); - _accel_blocked[instance - 1] = false; - _accel_stuck[instance - 1] = false; - } - } - - } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GYRO) { - handled = true; - - if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < GYRO_COUNT_MAX; i++) { - PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", i); - _gyro_blocked[i] = true; - _gyro_stuck[i] = false; - } - - } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { - PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", instance - 1); - _gyro_blocked[instance - 1] = true; - _gyro_stuck[instance - 1] = false; - } - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < GYRO_COUNT_MAX; i++) { - PX4_WARN("CMD_INJECT_FAILURE, gyro %d stuck", i); - _gyro_blocked[i] = false; - _gyro_stuck[i] = true; - } - - } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { - PX4_INFO("CMD_INJECT_FAILURE, gyro %d stuck", instance - 1); - _gyro_blocked[instance - 1] = false; - _gyro_stuck[instance - 1] = true; - } - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < GYRO_COUNT_MAX; i++) { - PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", i); - _gyro_blocked[i] = false; - _gyro_stuck[i] = false; - } - - } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { - PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", instance - 1); - _gyro_blocked[instance - 1] = false; - _gyro_stuck[instance - 1] = false; - } - } - - } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_MAG) { - handled = true; - - if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < MAG_COUNT_MAX; i++) { - PX4_WARN("CMD_INJECT_FAILURE, mag %d off", i); - _mag_blocked[i] = true; - _mag_stuck[i] = false; - } - - } else if (instance >= 1 && instance <= MAG_COUNT_MAX) { - PX4_WARN("CMD_INJECT_FAILURE, mag %d off", instance - 1); - _mag_blocked[instance - 1] = true; - _mag_stuck[instance - 1] = false; - } - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < MAG_COUNT_MAX; i++) { - PX4_WARN("CMD_INJECT_FAILURE, mag %d stuck", i); - _mag_blocked[i] = false; - _mag_stuck[i] = true; - } - - } else if (instance >= 1 && instance <= MAG_COUNT_MAX) { - PX4_WARN("CMD_INJECT_FAILURE, mag %d stuck", instance - 1); - _mag_blocked[instance - 1] = false; - _mag_stuck[instance - 1] = true; - } - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < MAG_COUNT_MAX; i++) { - PX4_WARN("CMD_INJECT_FAILURE, mag %d ok", i); - _mag_blocked[i] = false; - _mag_stuck[i] = false; - } - - } else if (instance >= 1 && instance <= MAG_COUNT_MAX) { - PX4_WARN("CMD_INJECT_FAILURE, mag %d ok", instance - 1); - _mag_blocked[instance - 1] = false; - _mag_stuck[instance - 1] = false; - } - } - - } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_BARO) { - handled = true; - - if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - PX4_WARN("CMD_INJECT_FAILURE, baro off"); - supported = true; - _baro_blocked = true; - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { - PX4_WARN("CMD_INJECT_FAILURE, baro stuck"); - supported = true; - _baro_stuck = true; - _baro_blocked = false; - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - PX4_INFO("CMD_INJECT_FAILURE, baro ok"); - supported = true; - _baro_blocked = false; - } - - } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_AIRSPEED) { - handled = true; - - if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - PX4_WARN("CMD_INJECT_FAILURE, airspeed off"); - supported = true; - _airspeed_disconnected = true; - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) { - PX4_WARN("CMD_INJECT_FAILURE, airspeed wrong (simulate pitot blockage)"); - supported = true; - _airspeed_blocked_timestamp = hrt_absolute_time(); - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - PX4_INFO("CMD_INJECT_FAILURE, airspeed ok"); - supported = true; - _airspeed_disconnected = false; - _airspeed_blocked_timestamp = 0; - } - - } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_VIO) { - handled = true; - - if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - PX4_WARN("CMD_INJECT_FAILURE, vio off"); - supported = true; - _vio_blocked = true; - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - PX4_INFO("CMD_INJECT_FAILURE, vio ok"); - supported = true; - _vio_blocked = false; - } - } - - if (handled) { - vehicle_command_ack_s ack{}; - ack.command = vehicle_command.command; - ack.from_external = false; - ack.result = supported ? - vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : - vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; - ack.timestamp = hrt_absolute_time(); - _command_ack_pub.publish(ack); - } - } -} - int SimulatorMavlink::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavlink) { distance_sensor_s dist{}; diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp index bf8c07bbf4..65243d6ce4 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp @@ -77,6 +77,7 @@ #include #include #include +#include #include @@ -293,10 +294,6 @@ private: bool _mag_blocked[MAG_COUNT_MAX] {}; bool _mag_stuck[MAG_COUNT_MAX] {}; - bool _gps_blocked{false}; - bool _gps_stuck{false}; - bool _gps_wrong{false}; - sensor_gps_s _gps_prev{}; bool _airspeed_disconnected{false}; hrt_abstime _airspeed_blocked_timestamp{0}; bool _vio_blocked{false}; @@ -308,6 +305,8 @@ private: float _last_baro_pressure{0.0f}; float _last_baro_temperature{0.0f}; + FailureInjection _failure_injection{}; + int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {}; #if defined(ENABLE_LOCKSTEP_SCHEDULER) diff --git a/src/systemcmds/failure/failure.cpp b/src/systemcmds/failure/failure.cpp index 2d764d9b91..6679afc362 100644 --- a/src/systemcmds/failure/failure.cpp +++ b/src/systemcmds/failure/failure.cpp @@ -57,6 +57,7 @@ static constexpr FailureUnit failure_units[] = { { "mag", vehicle_command_s::FAILURE_UNIT_SENSOR_MAG}, { "baro", vehicle_command_s::FAILURE_UNIT_SENSOR_BARO}, { "gps", vehicle_command_s::FAILURE_UNIT_SENSOR_GPS}, + { "gps_alt", vehicle_command_s::FAILURE_UNIT_SENSOR_GPS_ALT}, { "optical_flow", vehicle_command_s::FAILURE_UNIT_SENSOR_OPTICAL_FLOW}, { "vio", vehicle_command_s::FAILURE_UNIT_SENSOR_VIO}, { "distance_sensor", vehicle_command_s::FAILURE_UNIT_SENSOR_DISTANCE_SENSOR}, @@ -83,6 +84,7 @@ static constexpr FailureType failure_types[] = { { "slow", vehicle_command_s::FAILURE_TYPE_SLOW}, { "delayed", vehicle_command_s::FAILURE_TYPE_DELAYED}, { "intermittent", vehicle_command_s::FAILURE_TYPE_INTERMITTENT}, + { "drift", vehicle_command_s::FAILURE_TYPE_DRIFT}, }; static void print_usage()