diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 1d5351c2f6..b60f4969ab 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -71,6 +71,7 @@ uint8 vehicle_type # Type of vehicle (fixed-wing, rotary wing, ground) # and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing bool is_vtol # True if the system is VTOL capable +bool is_vtol_tailsitter # True if the system performs a 90° pitch down rotation during transition from MC to FW bool vtol_fw_permanent_stab # True if VTOL should stabilize attitude for fw in manual mode bool in_transition_mode # True if VTOL is doing a transition bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index d2d6730e26..f48e1893f2 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -50,6 +50,15 @@ get_rot_matrix(enum Rotation rot) math::radians((float)rot_lookup[rot].yaw)}}; } +__EXPORT matrix::Quatf +get_rot_quaternion(enum Rotation rot) +{ + return matrix::Quatf{matrix::Eulerf{ + math::radians((float)rot_lookup[rot].roll), + math::radians((float)rot_lookup[rot].pitch), + math::radians((float)rot_lookup[rot].yaw)}}; +} + __EXPORT void rotate_3f(enum Rotation rot, float &x, float &y, float &z) { diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index ce62e8d667..71d36ed62f 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -136,8 +136,14 @@ const rot_lookup_t rot_lookup[] = { /** * Get the rotation matrix */ +__EXPORT matrix::Dcmf +get_rot_matrix(enum Rotation rot); -__EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot); +/** + * Get the rotation quaternion + */ +__EXPORT matrix::Quatf +get_rot_quaternion(enum Rotation rot); /** * rotate a 3 element float vector in-place diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index c168f13a93..03a308287e 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1378,6 +1378,7 @@ Commander::run() } status.is_vtol = is_vtol(&status); + status.is_vtol_tailsitter = is_vtol_tailsitter(&status); commander_boot_timestamp = hrt_absolute_time(); @@ -1498,6 +1499,7 @@ Commander::run() /* set vehicle_status.is_vtol flag */ status.is_vtol = is_vtol(&status); + status.is_vtol_tailsitter = is_vtol_tailsitter(&status); /* check and update system / component ID */ int32_t sys_id = 0; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 0fd8144500..3f02123107 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -110,6 +110,12 @@ bool is_vtol(const struct vehicle_status_s *current_status) current_status->system_type == VEHICLE_TYPE_VTOL_RESERVED5); } +bool is_vtol_tailsitter(const struct vehicle_status_s *current_status) +{ + return (current_status->system_type == VEHICLE_TYPE_VTOL_DUOROTOR || + current_status->system_type == VEHICLE_TYPE_VTOL_QUADROTOR); +} + bool is_fixed_wing(const struct vehicle_status_s *current_status) { return current_status->system_type == VEHICLE_TYPE_FIXED_WING; diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 52528c3a66..44f3cb396d 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -53,6 +53,7 @@ bool is_multirotor(const struct vehicle_status_s *current_status); bool is_rotary_wing(const struct vehicle_status_s *current_status); bool is_vtol(const struct vehicle_status_s *current_status); +bool is_vtol_tailsitter(const struct vehicle_status_s *current_status); bool is_fixed_wing(const struct vehicle_status_s *current_status); bool is_ground_rover(const struct vehicle_status_s *current_status); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index f691649a10..a5d2eaba44 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -47,6 +47,7 @@ #include #include +#include #include #include #include @@ -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;