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

ATTITUDE_QUATERNION: fill repr_offset_q for tailsitters (#13249)

Browse Source 
* Conversions lib: add quaternion getter

* vehicle_status: add flag is_vtol_tailsitter

* ATTITUDE_QUATERNION: fill repr_offset_q for tailsitters
This commit is contained in:
Julien Lecoeur
2019-10-28 09:27:53 +01:00
committed by Julian Oes
parent 380cae18d1
commit 8f910f8435
7 changed files with 50 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+24 -1
View File
@@ -47,6 +47,7 @@
#include <commander/px4_custom_mode.h>
#include <drivers/drv_pwm_output.h>
#include <lib/conversion/rotation.h>
#include <lib/ecl/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/matrix/matrix/math.hpp>
@@ -1263,6 +1264,7 @@ public:
private:
MavlinkOrbSubscription *_att_sub;
MavlinkOrbSubscription *_angular_velocity_sub;
MavlinkOrbSubscription *_status_sub;
uint64_t _att_time{0};
/* do not allow top copying this class */
@@ -1272,7 +1274,8 @@ private:
protected:
explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink),
_att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))),
_angular_velocity_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_angular_velocity)))
_angular_velocity_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_angular_velocity))),
_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status)))
{}
bool send(const hrt_abstime t)
@@ -1283,6 +1286,9 @@ protected:
vehicle_angular_velocity_s angular_velocity{};
_angular_velocity_sub->update(&angular_velocity);
vehicle_status_s status{};
_status_sub->update(&status);
mavlink_attitude_quaternion_t msg{};
msg.time_boot_ms = att.timestamp / 1000;
@@ -1294,6 +1300,23 @@ protected:
msg.pitchspeed = angular_velocity.xyz[1];
msg.yawspeed = angular_velocity.xyz[2];
if (status.is_vtol && status.is_vtol_tailsitter && (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
// This is a tailsitter VTOL flying in fixed wing mode:
// indicate that reported attitude should be rotated by
// 90 degrees upward pitch for user display
get_rot_quaternion(ROTATION_PITCH_90).copyTo(msg.repr_offset_q);
} else {
// Normal case
// zero rotation should be [1 0 0 0]:
// `get_rot_quaternion(ROTATION_NONE).copyTo(msg.repr_offset_q);`
// but to save bandwidth, we instead send [0, 0, 0, 0].
msg.repr_offset_q[0] = 0.0f;
msg.repr_offset_q[1] = 0.0f;
msg.repr_offset_q[2] = 0.0f;
msg.repr_offset_q[3] = 0.0f;
}
mavlink_msg_attitude_quaternion_send_struct(_mavlink->get_channel(), &msg);
return true;