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

Improve readability

Browse Source
This commit is contained in:
Pernilla
2025-02-03 15:40:52 +01:00
parent 5afc6a921d
commit babea4bffa
1 changed files with 14 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/gimbal/input_mavlink.cpp
+14 -14
View File
@@ -43,6 +43,8 @@
#include <math.h>
#include <matrix/matrix/math.hpp>
using namespace matrix;
namespace gimbal
{
@@ -936,21 +938,19 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_manual_con
if (set_manual_control.origin_sysid == control_data.sysid_primary_control &&
set_manual_control.origin_compid == control_data.compid_primary_control) {
const matrix::Quatf q =
(PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw))
?
matrix::Quatf(
matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw))
:
matrix::Quatf(NAN, NAN, NAN, NAN);
Quatf q(NAN, NAN, NAN, NAN);
const matrix::Vector3f angular_velocity =
(PX4_ISFINITE(set_manual_control.pitch_rate) &&
PX4_ISFINITE(set_manual_control.yaw_rate)) ?
matrix::Vector3f(0.0f,
math::radians(_parameters.mnt_rate_pitch) * set_manual_control.pitch_rate,
math::radians(_parameters.mnt_rate_yaw) * set_manual_control.yaw_rate) :
matrix::Vector3f(NAN, NAN, NAN);
if (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) {
q = Quatf(matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw));
}
Vector3f angular_velocity(NAN, NAN, NAN);
if (PX4_ISFINITE(set_manual_control.pitch_rate) && PX4_ISFINITE(set_manual_control.yaw_rate)) {
angular_velocity = Vector3f(0.0f,
set_manual_control.pitch_rate * math::radians(_parameters.mnt_rate_pitch),
set_manual_control.yaw_rate * math::radians(_parameters.mnt_rate_yaw));
}
_set_control_data_from_set_attitude(control_data, set_manual_control.flags, q,
angular_velocity, set_manual_control.timestamp);