dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

simulator_mavlink: Add basic vio failure injection (#20577) 

* simulator_mavlink: Add basic vio failure injection

Signed-off-by: Paul Frivold <paul@kefrobotics.com>

* simulator_mavlink: Rm failure not supported warning

Failures commands are also handled in other files,
so warning here could be confusing.

Signed-off-by: Paul Frivold <paul@kefrobotics.com>

Signed-off-by: Paul Frivold <paul@kefrobotics.com>
This commit is contained in:
Paul Erik Frivold 2022-11-13 19:36:23 -05:00 committed by GitHub
parent acd8f20a85
commit 8e5efb0131
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 20 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
View File

@ -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) {

1
src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp
View File

@ -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] {};