From 6340cc61567f80be5cfabfb49904e2cb7d588a94 Mon Sep 17 00:00:00 2001 From: DanielePettenuzzo Date: Thu, 22 Feb 2024 22:00:22 +0100 Subject: [PATCH] fix gimbal compensation when output is set to rc --- src/modules/gimbal/output.cpp | 15 ++++++++------- src/modules/gimbal/output.h | 1 + src/modules/gimbal/output_rc.cpp | 6 +++--- 3 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/modules/gimbal/output.cpp b/src/modules/gimbal/output.cpp index c43c3dfaa8..50624ec49c 100644 --- a/src/modules/gimbal/output.cpp +++ b/src/modules/gimbal/output.cpp @@ -231,23 +231,24 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t) _angle_outputs[i] += dt * _angle_velocity[i]; } - if (compensate[i] && PX4_ISFINITE(euler_vehicle(i))) { - _angle_outputs[i] -= euler_vehicle(i); - } + float compensation = (compensate[i] && PX4_ISFINITE(euler_vehicle(i)) ? euler_vehicle(i) : 0.f); + + _angle_outputs_compensated[i] = _angle_outputs[i] - compensation; if (PX4_ISFINITE(_angle_outputs[i])) { // bring angles into proper range [-pi, pi] _angle_outputs[i] = matrix::wrap_pi(_angle_outputs[i]); + _angle_outputs_compensated[i] = matrix::wrap_pi(_angle_outputs_compensated[i]); } } // constrain pitch to [MNT_LND_P_MIN, MNT_LND_P_MAX] if landed if (_landed) { - if (PX4_ISFINITE(_angle_outputs[1])) { - _angle_outputs[1] = math::constrain(_angle_outputs[1], - math::radians(_parameters.mnt_lnd_p_min), - math::radians(_parameters.mnt_lnd_p_max)); + if (PX4_ISFINITE(_angle_outputs_compensated[1])) { + _angle_outputs_compensated[1] = math::constrain(_angle_outputs_compensated[1], + math::radians(_parameters.mnt_lnd_p_min), + math::radians(_parameters.mnt_lnd_p_max)); } } } diff --git a/src/modules/gimbal/output.h b/src/modules/gimbal/output.h index f0dd983240..66866c9795 100644 --- a/src/modules/gimbal/output.h +++ b/src/modules/gimbal/output.h @@ -89,6 +89,7 @@ protected: void _calculate_angle_output(const hrt_abstime &t); float _angle_outputs[3] = { 0.f, 0.f, 0.f }; ///< calculated output angles (roll, pitch, yaw) [rad] + float _angle_outputs_compensated[3] = { 0.f, 0.f, 0.f }; ///< calculated output angles (roll, pitch, yaw) [rad] hrt_abstime _last_update; private: diff --git a/src/modules/gimbal/output_rc.cpp b/src/modules/gimbal/output_rc.cpp index a5878b3801..1c4ddad723 100644 --- a/src/modules/gimbal/output_rc.cpp +++ b/src/modules/gimbal/output_rc.cpp @@ -66,15 +66,15 @@ void OutputRC::update(const ControlData &control_data, bool new_setpoints, uint8 // _angle_outputs are in radians, gimbal_controls are in [-1, 1] gimbal_controls_s gimbal_controls{}; gimbal_controls.control[gimbal_controls_s::INDEX_ROLL] = constrain( - (_angle_outputs[0] + math::radians(_parameters.mnt_off_roll)) * + (_angle_outputs_compensated[0] + math::radians(_parameters.mnt_off_roll)) * (1.0f / (math::radians(_parameters.mnt_range_roll / 2.0f))), -1.f, 1.f); gimbal_controls.control[gimbal_controls_s::INDEX_PITCH] = constrain( - (_angle_outputs[1] + math::radians(_parameters.mnt_off_pitch)) * + (_angle_outputs_compensated[1] + math::radians(_parameters.mnt_off_pitch)) * (1.0f / (math::radians(_parameters.mnt_range_pitch / 2.0f))), -1.f, 1.f); gimbal_controls.control[gimbal_controls_s::INDEX_YAW] = constrain( - (_angle_outputs[2] + math::radians(_parameters.mnt_off_yaw)) * + (_angle_outputs_compensated[2] + math::radians(_parameters.mnt_off_yaw)) * (1.0f / (math::radians(_parameters.mnt_range_yaw / 2.0f))), -1.f, 1.f); gimbal_controls.timestamp = hrt_absolute_time();