From 22950a94ec173655dfc517e10f9bd7e48e3c94e1 Mon Sep 17 00:00:00 2001 From: Michael Schaeuble Date: Fri, 3 Dec 2021 12:41:11 +0100 Subject: [PATCH] Override vehicle attitude send to gimbals when HIL mode is enabled The simulated attitude can disturb the gimbal estimator and lead to strange behavior of the gimbal. So, since the hardware is not moving in HIL/SIH, we fake a static attitude towards the gimbal. --- .../AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp b/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp index 5e4dea2a03d1..ae5f229cfb07 100644 --- a/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp +++ b/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp @@ -70,6 +70,7 @@ class MavlinkStreamAutopilotStateForGimbalDevice : public MavlinkStream uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _landed_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; bool send() override { @@ -78,14 +79,22 @@ class MavlinkStreamAutopilotStateForGimbalDevice : public MavlinkStream if (_att_sub.update(&att)) { mavlink_autopilot_state_for_gimbal_device_t msg{}; + bool hil_state = false; + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + hil_state = vehicle_status.hil_state; + } + //msg.target_system = 0; // TODO //msg.target_component = 0; // TODO msg.time_boot_us = att.timestamp; - msg.q[0] = att.q[0]; - msg.q[1] = att.q[1]; - msg.q[2] = att.q[2]; - msg.q[3] = att.q[3]; + // In HIL mode the gimbal is not moving. Send a static attitude to not disturb the gimbal + msg.q[0] = !hil_state ? att.q[0] : 1; + msg.q[1] = !hil_state ? att.q[1] : 0; + msg.q[2] = !hil_state ? att.q[2] : 0; + msg.q[3] = !hil_state ? att.q[3] : 0; msg.q_estimated_delay_us = 0; // I don't know. { @@ -102,7 +111,7 @@ class MavlinkStreamAutopilotStateForGimbalDevice : public MavlinkStream { vehicle_attitude_setpoint_s att_sp; - if (_att_sp_sub.copy(&att_sp)) { + if (!hil_state && _att_sp_sub.copy(&att_sp)) { msg.feed_forward_angular_velocity_z = att_sp.yaw_sp_move_rate; } }