diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 1517d7ed5a..71970b9490 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -811,8 +811,11 @@ void SimulatorMavlink::handle_message_odometry(const mavlink_message_t *msg) case MAV_ESTIMATOR_TYPE_NAIVE: case MAV_ESTIMATOR_TYPE_VISION: case MAV_ESTIMATOR_TYPE_VIO: - odom.timestamp = hrt_absolute_time(); - _visual_odometry_pub.publish(odom); + if (!_vio_blocked) { + odom.timestamp = hrt_absolute_time(); + _visual_odometry_pub.publish(odom); + } + break; case MAV_ESTIMATOR_TYPE_MOCAP: @@ -1432,6 +1435,20 @@ void SimulatorMavlink::check_failure_injections() supported = true; _airspeed_blocked = false; } + + } 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) { diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp index a84729e09a..ee4118a302 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp @@ -295,6 +295,7 @@ private: bool _gps_blocked{false}; bool _airspeed_blocked{false}; + bool _vio_blocked{false}; float _last_magx[MAG_COUNT_MAX] {}; float _last_magy[MAG_COUNT_MAX] {};