From cd0509d2efebb174ff4b4f28191a8331ca2169af Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 16 Nov 2020 16:23:26 +0100 Subject: [PATCH] vmount: fix AUX gimbal output for v2 mavlink input This should fix the case where AUX gimbals don't work anymore when gimbal input is mavlink gimbal v2. The fix is mostly to take care against NAN inputs. --- src/modules/vmount/output.cpp | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/modules/vmount/output.cpp b/src/modules/vmount/output.cpp index 29c03e05fa..9fd7fa4373 100644 --- a/src/modules/vmount/output.cpp +++ b/src/modules/vmount/output.cpp @@ -198,13 +198,6 @@ void OutputBase::_handle_position_update(bool force_update) void OutputBase::_calculate_output_angles(const hrt_abstime &t) { - //take speed into account - float dt = (t - _last_update) / 1.e6f; - - for (int i = 0; i < 3; ++i) { - _angle_setpoints[i] += dt * _angle_speeds[i]; - } - //get the output angles and stabilize if necessary vehicle_attitude_s vehicle_attitude{}; matrix::Eulerf euler; @@ -224,14 +217,22 @@ void OutputBase::_calculate_output_angles(const hrt_abstime &t) euler = matrix::Quatf(vehicle_attitude.q); } - for (int i = 0; i < 3; ++i) { - if (compensate[i]) { - _angle_outputs[i] = _angle_setpoints[i] - euler(i); + float dt = (t - _last_update) / 1.e6f; - } else { + for (int i = 0; i < 3; ++i) { + + if (PX4_ISFINITE(_angle_setpoints[i])) { _angle_outputs[i] = _angle_setpoints[i]; } + if (PX4_ISFINITE(_angle_speeds[i])) { + _angle_outputs[i] += dt * _angle_speeds[i]; + } + + if (compensate[i]) { + _angle_outputs[i] -= euler(i); + } + //bring angles into proper range [-pi, pi] while (_angle_outputs[i] > M_PI_F) { _angle_outputs[i] -= 2.f * M_PI_F; }