fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

vmount: fix AUX gimbal output for v2 mavlink input

Browse Source 
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.
This commit is contained in:
Julian Oes 2020-11-16 16:23:26 +01:00 committed by Daniel Agar
parent 17dce18b32
commit cd0509d2ef
1 changed files with 12 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/modules/vmount/output.cpp
View File

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