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 5e4dea2a03..ae5f229cfb 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 @@ private: 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 @@ private: 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 @@ private: { 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; } }