diff --git a/src/modules/vmount/input_mavlink.cpp b/src/modules/vmount/input_mavlink.cpp index e6fee59e94..07518537a9 100644 --- a/src/modules/vmount/input_mavlink.cpp +++ b/src/modules/vmount/input_mavlink.cpp @@ -270,11 +270,11 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con _control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; // vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0 - const float roll = vehicle_command.param2 * M_DEG_TO_RAD_F; + const float roll = math::radians(vehicle_command.param2); // vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1 - const float pitch = vehicle_command.param1 * M_DEG_TO_RAD_F; + const float pitch = math::radians(vehicle_command.param1); // both specs have yaw on channel 2 - float yaw = vehicle_command.param3 * M_DEG_TO_RAD_F; + float yaw = math::radians(vehicle_command.param3); matrix::Eulerf euler(roll, pitch, yaw); @@ -639,11 +639,11 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con _control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; // vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0 - const float roll = vehicle_command.param2 * M_DEG_TO_RAD_F; + const float roll = math::radians(vehicle_command.param2); // vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1 - const float pitch = vehicle_command.param1 * M_DEG_TO_RAD_F; + const float pitch = math::radians(vehicle_command.param1); // both specs have yaw on channel 2 - float yaw = vehicle_command.param3 * M_DEG_TO_RAD_F; + float yaw = math::radians(vehicle_command.param3); // We expect angle of [-pi..+pi]. If the input range is [0..2pi] we can fix that. if (yaw > M_PI_F) { @@ -795,7 +795,7 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con if (vehicle_command.source_system == _sys_id_primary_control && vehicle_command.source_component == _comp_id_primary_control) { - const matrix::Eulerf euler(0.0f, vehicle_command.param1 * M_DEG_TO_RAD_F, vehicle_command.param2 * M_DEG_TO_RAD_F); + const matrix::Eulerf euler(0.0f, math::radians(vehicle_command.param1), math::radians(vehicle_command.param2)); const matrix::Quatf q(euler); const matrix::Vector3f angular_velocity(0.0f, vehicle_command.param3, vehicle_command.param4); const uint32_t flags = vehicle_command.param5; @@ -832,8 +832,8 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con const matrix::Vector3f angular_velocity = (PX4_ISFINITE(set_manual_control.pitch_rate) && PX4_ISFINITE(set_manual_control.yaw_rate)) ? matrix::Vector3f(0.0f, - _mnt_rate_pitch * M_DEG_TO_RAD_F * set_manual_control.pitch_rate, - _mnt_rate_yaw * M_DEG_TO_RAD_F * set_manual_control.yaw_rate) : + math::radians(_mnt_rate_pitch) * set_manual_control.pitch_rate, + math::radians(_mnt_rate_yaw) * set_manual_control.yaw_rate) : matrix::Vector3f(NAN, NAN, NAN); _set_control_data_from_set_attitude(set_manual_control.flags, q, angular_velocity); diff --git a/src/modules/vmount/input_test.cpp b/src/modules/vmount/input_test.cpp index bbe0e49cc3..dc979a0d3d 100644 --- a/src/modules/vmount/input_test.cpp +++ b/src/modules/vmount/input_test.cpp @@ -43,6 +43,7 @@ #include #include +#include namespace vmount @@ -71,9 +72,9 @@ int InputTest::update(unsigned int timeout_ms, ControlData **control_data, bool _control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; matrix::Eulerf euler( - _angles[0] * M_DEG_TO_RAD_F, - _angles[1] * M_DEG_TO_RAD_F, - _angles[2] * M_DEG_TO_RAD_F); + math::radians(_angles[0]), + math::radians(_angles[1]), + math::radians(_angles[2])); matrix::Quatf q(euler); _control_data.type_data.angle.q[0] = q(0); diff --git a/src/modules/vmount/output_mavlink.cpp b/src/modules/vmount/output_mavlink.cpp index b91ffa4851..c8ea6555de 100644 --- a/src/modules/vmount/output_mavlink.cpp +++ b/src/modules/vmount/output_mavlink.cpp @@ -101,9 +101,9 @@ int OutputMavlinkV1::update(const ControlData *control_data) // vmount spec has roll, pitch on channels 0, 1, respectively; MAVLink spec has roll, pitch on channels 1, 0, respectively // vmount uses radians, MAVLink uses degrees - vehicle_command.param1 = (_angle_outputs[1] + _config.pitch_offset) * M_RAD_TO_DEG_F; - vehicle_command.param2 = (_angle_outputs[0] + _config.roll_offset) * M_RAD_TO_DEG_F; - vehicle_command.param3 = (_angle_outputs[2] + _config.yaw_offset) * M_RAD_TO_DEG_F; + vehicle_command.param1 = math::degrees(_angle_outputs[1] + _config.pitch_offset); + vehicle_command.param2 = math::degrees(_angle_outputs[0] + _config.roll_offset); + vehicle_command.param3 = math::degrees(_angle_outputs[2] + _config.yaw_offset); vehicle_command.param7 = 2.0f; // MAV_MOUNT_MODE_MAVLINK_TARGETING; _vehicle_command_pub.publish(vehicle_command); diff --git a/src/modules/vmount/vmount.cpp b/src/modules/vmount/vmount.cpp index 161f3ae9a2..046b76b2c9 100644 --- a/src/modules/vmount/vmount.cpp +++ b/src/modules/vmount/vmount.cpp @@ -233,12 +233,12 @@ static int vmount_thread_main(int argc, char *argv[]) output_config.gimbal_normal_mode_value = params.mnt_ob_norm_mode; output_config.gimbal_retracted_mode_value = params.mnt_ob_lock_mode; - output_config.pitch_scale = 1.0f / ((params.mnt_range_pitch / 2.0f) * M_DEG_TO_RAD_F); - output_config.roll_scale = 1.0f / ((params.mnt_range_roll / 2.0f) * M_DEG_TO_RAD_F); - output_config.yaw_scale = 1.0f / ((params.mnt_range_yaw / 2.0f) * M_DEG_TO_RAD_F); - output_config.pitch_offset = params.mnt_off_pitch * M_DEG_TO_RAD_F; - output_config.roll_offset = params.mnt_off_roll * M_DEG_TO_RAD_F; - output_config.yaw_offset = params.mnt_off_yaw * M_DEG_TO_RAD_F; + output_config.pitch_scale = 1.0f / (math::radians(params.mnt_range_pitch / 2.0f)); + output_config.roll_scale = 1.0f / (math::radians(params.mnt_range_roll / 2.0f)); + output_config.yaw_scale = 1.0f / (math::radians(params.mnt_range_yaw / 2.0f)); + output_config.pitch_offset = math::radians(params.mnt_off_pitch); + output_config.roll_offset = math::radians(params.mnt_off_roll); + output_config.yaw_offset = math::radians(params.mnt_off_yaw); output_config.mavlink_sys_id_v1 = params.mnt_mav_sys_id_v1; output_config.mavlink_comp_id_v1 = params.mnt_mav_comp_id_v1;