Skip to content

Commit 551897a

Browse files
committed
gimbal: mavlink streamed angular ranges should be radians
1 parent 42d0d5a commit 551897a

File tree

1 file changed

+6
-4
lines changed

1 file changed

+6
-4
lines changed

src/modules/gimbal/input_mavlink.cpp

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -508,10 +508,12 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_information(const ControlData
508508
gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK |
509509
gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL;
510510

511-
gimbal_manager_info.pitch_max = _parameters.mnt_max_pitch;
512-
gimbal_manager_info.pitch_min = _parameters.mnt_min_pitch;
513-
gimbal_manager_info.yaw_max = _parameters.mnt_range_yaw * 0.5f;
514-
gimbal_manager_info.yaw_min = -_parameters.mnt_range_yaw * 0.5f;
511+
gimbal_manager_info.pitch_max = math::radians(_parameters.mnt_max_pitch);
512+
gimbal_manager_info.pitch_min = math::radians(_parameters.mnt_min_pitch);
513+
gimbal_manager_info.yaw_max = math::radians(_parameters.mnt_range_yaw * 0.5f);
514+
gimbal_manager_info.yaw_min = math::radians(-_parameters.mnt_range_yaw * 0.5f);
515+
gimbal_manager_info.roll_max = math::radians(_parameters.mnt_range_roll * 0.5f);
516+
gimbal_manager_info.roll_min = math::radians(-_parameters.mnt_range_roll * 0.5f);
515517

516518
gimbal_manager_info.gimbal_device_id = control_data.device_compid;
517519

0 commit comments

Comments
 (0)