dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fix gimbal compensation when output is set to rc

This commit is contained in:
DanielePettenuzzo 2024-02-22 22:00:22 +01:00
parent b405d75553
commit 6340cc6156
3 changed files with 12 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/gimbal/output.cpp
View File

@ -231,23 +231,24 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t)
_angle_outputs[i] += dt * _angle_velocity[i]; _angle_outputs[i] += dt * _angle_velocity[i];
} }
if (compensate[i] && PX4_ISFINITE(euler_vehicle(i))) { float compensation = (compensate[i] && PX4_ISFINITE(euler_vehicle(i)) ? euler_vehicle(i) : 0.f);
_angle_outputs[i] -= euler_vehicle(i);
} _angle_outputs_compensated[i] = _angle_outputs[i] - compensation;
if (PX4_ISFINITE(_angle_outputs[i])) { if (PX4_ISFINITE(_angle_outputs[i])) {
// bring angles into proper range [-pi, pi] // bring angles into proper range [-pi, pi]
_angle_outputs[i] = matrix::wrap_pi(_angle_outputs[i]); _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 // constrain pitch to [MNT_LND_P_MIN, MNT_LND_P_MAX] if landed
if (_landed) { if (_landed) {
if (PX4_ISFINITE(_angle_outputs[1])) { if (PX4_ISFINITE(_angle_outputs_compensated[1])) {
_angle_outputs[1] = math::constrain(_angle_outputs[1], _angle_outputs_compensated[1] = math::constrain(_angle_outputs_compensated[1],
math::radians(_parameters.mnt_lnd_p_min), math::radians(_parameters.mnt_lnd_p_min),
math::radians(_parameters.mnt_lnd_p_max)); math::radians(_parameters.mnt_lnd_p_max));
} }
} }
} }

1
src/modules/gimbal/output.h
View File

@ -89,6 +89,7 @@ protected:
void _calculate_angle_output(const hrt_abstime &t); 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[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; hrt_abstime _last_update;
private: private:

6
src/modules/gimbal/output_rc.cpp
View File

@ -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] // _angle_outputs are in radians, gimbal_controls are in [-1, 1]
gimbal_controls_s gimbal_controls{}; gimbal_controls_s gimbal_controls{};
gimbal_controls.control[gimbal_controls_s::INDEX_ROLL] = constrain( 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.0f / (math::radians(_parameters.mnt_range_roll / 2.0f))),
-1.f, 1.f); -1.f, 1.f);
gimbal_controls.control[gimbal_controls_s::INDEX_PITCH] = constrain( 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.0f / (math::radians(_parameters.mnt_range_pitch / 2.0f))),
-1.f, 1.f); -1.f, 1.f);
gimbal_controls.control[gimbal_controls_s::INDEX_YAW] = constrain( 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.0f / (math::radians(_parameters.mnt_range_yaw / 2.0f))),
-1.f, 1.f); -1.f, 1.f);
gimbal_controls.timestamp = hrt_absolute_time(); gimbal_controls.timestamp = hrt_absolute_time();