fix gimbal compensation when output is set to rc

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));
 		}
 	}
 }

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:

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();