ATTITUDE_QUATERNION: fill repr_offset_q for tailsitters (#13249)

* Conversions lib: add quaternion getter

* vehicle_status: add flag is_vtol_tailsitter

* ATTITUDE_QUATERNION: fill repr_offset_q for tailsitters

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

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)
 {

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

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;

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;

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);
 

src/modules/mavlink/mavlink_messages.cpp

@@ -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;