Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

vmount: use math functions instead of macros

Browse Source
This commit is contained in:
Julian Oes
2021-02-04 08:04:40 +01:00
committed by Daniel Agar
parent 6672be040a
commit f39216b9c3
4 changed files with 22 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vmount/input_mavlink.cpp
+9 -9
View File
@@ -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);
src/modules/vmount/input_test.cpp
+4 -3
View File
@@ -43,6 +43,7 @@
#include <px4_platform_common/posix.h>
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/mathlib.h>
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);
src/modules/vmount/output_mavlink.cpp
+3 -3
View File
@@ -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);
src/modules/vmount/vmount.cpp
+6 -6
View File
@@ -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;