Fix unit tests

src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp

@@ -108,28 +108,31 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
 	float yaw = 0.f;
 	vehicle_attitude_setpoint_s att{};
 	thrustToAttitude(thr, yaw, att);
-	EXPECT_FLOAT_EQ(att.roll_body, 0.f);
-	EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
-	EXPECT_FLOAT_EQ(att.yaw_body, 0.f);
+	EXPECT_FLOAT_EQ(att.q_d[0], 1.0);
+	EXPECT_FLOAT_EQ(att.q_d[1], 0.0);
+	EXPECT_FLOAT_EQ(att.q_d[2], 0.0);
+	EXPECT_FLOAT_EQ(att.q_d[3], 0.0);
 	EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
 
 	/* expected: same as before but with 90 yaw
 	 * reason: only yaw changed */
 	yaw = M_PI_2_F;
 	thrustToAttitude(thr, yaw, att);
-	EXPECT_FLOAT_EQ(att.roll_body, 0.f);
-	EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
-	EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
+	EXPECT_FLOAT_EQ(att.q_d[0], std::cos(0.5 * M_PI_2_F));
+	EXPECT_FLOAT_EQ(att.q_d[1], 0.0);
+	EXPECT_FLOAT_EQ(att.q_d[2], 0.0);
+	EXPECT_FLOAT_EQ(att.q_d[3], std::sin(0.5 * M_PI_2_F));
 	EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
 
 	/* expected: same as before but roll 180
 	 * reason: thrust points straight down and order Euler
 	 * order is: 1. roll, 2. pitch, 3. yaw */
 	thr = Vector3f(0.f, 0.f, 1.f);
 	thrustToAttitude(thr, yaw, att);
-	EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F);
-	EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
-	EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
+	Quatf quat_att(att.q_d);
+	EXPECT_FLOAT_EQ(quat_att.phi(), -M_PI_F);
+	EXPECT_FLOAT_EQ(quat_att.theta(), 0.f);
+	EXPECT_FLOAT_EQ(quat_att.yaw(), M_PI_2_F);
 	EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
 }