Skip to content
Snippets Groups Projects
Commit 98de8c85 authored by Julian Oes's avatar Julian Oes Committed by Beat Küng
Browse files

vmount: accept mavlink input from [0..360] degrees

This enables mavlink user input not only in the range of [-180..180]
degrees but also [0..360] degrees.
parent 8ba6c209
No related branches found
No related tags found
No related merge requests found
......@@ -278,6 +278,11 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
// both specs have yaw on channel 2
_control_data.type_data.angle.angles[2] = vehicle_command.param3 * M_DEG_TO_RAD_F;
// We expect angle of [-pi..+pi]. If the input range is [0..2pi] we can fix that.
if (_control_data.type_data.angle.angles[2] > M_PI_F) {
_control_data.type_data.angle.angles[2] -= 2 * M_PI_F;
}
*control_data = &_control_data;
break;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment