TEMP

with simple quaternion calculation.
Difference: The body yaw axis when projected
onto the horizontal plane is not stying in line
with the wold frame heading. This alignment
was generating additional yaw control effort
for any combined roll and pitch rotation.

src/modules/ekf2/EKF2.cpp

@@ -1593,8 +1593,9 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	Quatf delta_q_reset;
 	_ekf.get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter);
 
-	lpos.heading = Eulerf(_ekf.getQuaternion()).psi();
-	lpos.unaided_heading = _ekf.getUnaidedYaw();
+	const Quatf q = _ekf.getQuaternion();
+	lpos.heading = 2.f * atan2f(q(3), q(0));
+	lpos.unaided_heading = _ekf.getUnaidedYaw(); // TODO: Needs to change as well
 	lpos.delta_heading = Eulerf(delta_q_reset).psi();
 	lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete();
 

src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp

@@ -138,3 +138,31 @@ TEST(AttitudeControlTest, YawWeightScaling)
 	// THEN: no actuation (also no NAN)
 	EXPECT_EQ(rate_setpoint, Vector3f());
 }
+
+TEST(AttitudeControlTest, HeadingCorrectness)
+{
+	//Quatf q(0.863f, 0.358f,  0.358f, 0.f);
+	Quatf q(0.733f, 0.462f, 0.191f, 0.462f);
+	q.normalize();
+	float yaw = matrix::Eulerf(q).psi();
+
+	Quatf q_red(Vector3f(0, 0, 1), q.dcm_z());
+	Quatf q_mix = q_red.inversed() * q;
+	q_mix.print();
+
+	Quatf q_mix2 = q;
+	q_mix2(1) = 0.f;
+	q_mix2(2) = 0.f;
+	q_mix2.normalize();
+	q_mix2.print();
+
+	q_mix2(3) = math::constrain(q_mix2(3), -1.f, 1.f);
+	float heading = 2.f * asinf(q_mix2(3));
+
+	float heading2 = 2.f * atan2f(q(3), q(0));
+	EXPECT_FLOAT_EQ(heading, heading2);
+
+	EXPECT_GT(fabsf(yaw), FLT_EPSILON);
+	EXPECT_LT(fabsf(heading), FLT_EPSILON);
+	EXPECT_TRUE(false);
+}

src/modules/mc_pos_control/PositionControl/CMakeLists.txt

@@ -41,3 +41,4 @@ target_include_directories(PositionControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 px4_add_unit_gtest(SRC ControlMathTest.cpp LINKLIBS PositionControl)
 px4_add_unit_gtest(SRC PositionControlTest.cpp LINKLIBS PositionControl)
+px4_add_unit_gtest(SRC PositionAttitudeTest.cpp LINKLIBS PositionControl AttitudeControl)

src/modules/mc_pos_control/PositionControl/ControlMath.cpp

@@ -44,9 +44,20 @@ using namespace matrix;
 
 namespace ControlMath
 {
+
 void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
 {
-	bodyzToAttitude(-thr_sp, yaw_sp, att_sp);
+	const Quatf q = bodyzToQuaternion(thr_sp, yaw_sp);
+
+	// copy quaternion setpoint to attitude setpoint topic
+	q.copyTo(att_sp.q_d);
+
+	// calculate euler angles, for logging only, must not be used for control
+	const Eulerf euler{q};
+	att_sp.roll_body = euler.phi();
+	att_sp.pitch_body = euler.theta();
+	att_sp.yaw_body = euler.psi();
+
 	att_sp.thrust_body[2] = -thr_sp.length();
 }
 
@@ -67,56 +78,16 @@ void limitTilt(Vector3f &body_unit, const Vector3f &world_unit, const float max_
 	body_unit = cosf(angle) * world_unit + sinf(angle) * rejection.unit();
 }
 
-void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
+Quatf bodyzToQuaternion(Vector3f body_z, const float yaw_sp)
 {
 	// zero vector, no direction, set safe level value
 	if (body_z.norm_squared() < FLT_EPSILON) {
-		body_z(2) = 1.f;
+		body_z(2) = -1.f;
 	}
 
-	body_z.normalize();
-
-	// vector of desired yaw direction in XY plane, rotated by PI/2
-	const Vector3f y_C{-sinf(yaw_sp), cosf(yaw_sp), 0.f};
-
-	// desired body_x axis, orthogonal to body_z
-	Vector3f body_x = y_C % body_z;
-
-	// keep nose to front while inverted upside down
-	if (body_z(2) < 0.f) {
-		body_x = -body_x;
-	}
-
-	if (fabsf(body_z(2)) < 0.000001f) {
-		// desired thrust is in XY plane, set X downside to construct correct matrix,
-		// but yaw component will not be used actually
-		body_x.zero();
-		body_x(2) = 1.f;
-	}
-
-	body_x.normalize();
-
-	// desired body_y axis
-	const Vector3f body_y = body_z % body_x;
-
-	Dcmf R_sp;
-
-	// fill rotation matrix
-	for (int i = 0; i < 3; i++) {
-		R_sp(i, 0) = body_x(i);
-		R_sp(i, 1) = body_y(i);
-		R_sp(i, 2) = body_z(i);
-	}
-
-	// copy quaternion setpoint to attitude setpoint topic
-	const Quatf q_sp{R_sp};
-	q_sp.copyTo(att_sp.q_d);
-
-	// calculate euler angles, for logging only, must not be used for control
-	const Eulerf euler{R_sp};
-	att_sp.roll_body = euler.phi();
-	att_sp.pitch_body = euler.theta();
-	att_sp.yaw_body = euler.psi();
+	const Quatf q{Vector3f(0.f, 0.f, -1.f), body_z};
+	const Quatf q_yaw{cosf(yaw_sp / 2.f), 0.f, 0.f, sinf(yaw_sp / 2.f)};
+	return q * q_yaw;
 }
 
 Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)

src/modules/mc_pos_control/PositionControl/ControlMath.hpp

@@ -69,6 +69,8 @@ void limitTilt(matrix::Vector3f &body_unit, const matrix::Vector3f &world_unit,
  */
 void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
 
+matrix::Quatf bodyzToQuaternion(matrix::Vector3f body_z, const float yaw_sp);
+
 /**
  * Outputs the sum of two vectors but respecting the limits and priority.
  * The sum of two vectors are constraint such that v0 has priority over v1.

src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp

@@ -100,37 +100,65 @@ TEST(ControlMathTest, LimitTilt10degree)
 	EXPECT_FLOAT_EQ(2.f * body(0), body(1));
 }
 
-TEST(ControlMathTest, ThrottleAttitudeMapping)
+class ControlMathAttitudeMappingTest : public ::testing::Test
+{
+public:
+	void checkDirection(const Vector3f thrust_setpoint, const float yaw)
+	{
+		thrustToAttitude(thrust_setpoint, yaw, _attitude_setpoint);
+		EXPECT_EQ(Quatf(_attitude_setpoint.q_d).dcm_z(), -thrust_setpoint.normalized());
+		EXPECT_EQ(_attitude_setpoint.thrust_body[2], -thrust_setpoint.length());
+	}
+
+	void checkEuler(const float roll, const float pitch, const float yaw)
+	{
+		EXPECT_FLOAT_EQ(_attitude_setpoint.roll_body, roll);
+		EXPECT_FLOAT_EQ(_attitude_setpoint.pitch_body, pitch);
+		EXPECT_FLOAT_EQ(_attitude_setpoint.yaw_body, yaw);
+	}
+
+	vehicle_attitude_setpoint_s _attitude_setpoint{};
+};
+
+TEST_F(ControlMathAttitudeMappingTest, AttitudeMappingNoRotation)
 {
 	/* expected: zero roll, zero pitch, zero yaw, full thr mag
 	 * reason: thrust pointing full upward */
-	Vector3f thr{0.f, 0.f, -1.f};
-	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.thrust_body[2], -1.f);
+	checkDirection(Vector3f(0.f, 0.f, -1.f), 0.f);
+	checkEuler(0.f, 0.f, 0.f);
+}
 
+TEST_F(ControlMathAttitudeMappingTest, AttitudeMappingYaw90)
+{
 	/* 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.thrust_body[2], -1.f);
+	checkDirection(Vector3f(0.f, 0.f, -1.f), M_PI_2_F);
+	checkEuler(0.f, 0.f, M_PI_2_F);
+}
 
+TEST_F(ControlMathAttitudeMappingTest, AttitudeMappingUpsideDown)
+{
 	/* 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);
-	EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
+	checkDirection(Vector3f(0.f, 0.f, 1.f), 0.f);
+	checkEuler(M_PI_F, 0.f, 0.f);
+}
+
+TEST_F(ControlMathAttitudeMappingTest, AttitudeMappingUpsideDownYaw90)
+{
+	/* expected: same as before but roll 180
+	 * reason: thrust points straight down and order Euler
+	 * order is: 1. roll, 2. pitch, 3. yaw */
+	checkDirection(Vector3f(0.f, 0.f, 1.f), M_PI_2_F);
+	checkEuler(M_PI_F, 0.f, -M_PI_2_F);
+}
+
+TEST_F(ControlMathAttitudeMappingTest, AttitudeMappingRandomDirections)
+{
+	checkDirection(Vector3f(0, .5f, -.5f), 1.f);
+	checkDirection(Vector3f(-2.f, 8.f, .1f), 2.f);
+	checkDirection(Vector3f(-.2f, -5.f, -30.f), 2.f);
 }
 
 TEST(ControlMathTest, ConstrainXYPriorities)

src/modules/mc_pos_control/PositionControl/PositionAttitudeTest.cpp

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <ControlMath.hpp>
+#include <AttitudeControl.hpp>
+#include <px4_defines.h>
+
+using namespace matrix;
+
+TEST(PositionControlTest, NavigationYawInfluence)
+{
+	const Vector3f thrust_setpoint(1.f, 1.f, -.5f);
+	const float yaw_setpoint = 0.f;
+
+	// Generate attitude
+	vehicle_attitude_setpoint_s attitude_setpoint{};
+	ControlMath::thrustToAttitude(thrust_setpoint, yaw_setpoint, attitude_setpoint);
+
+	// Set up attitude control
+	AttitudeControl attitude_control;
+	attitude_control.setProportionalGain(Vector3f(1.f, 1.f, 1.f), 1.f);
+	attitude_control.setRateLimit(Vector3f(100.f, 100.f, 100.f));
+
+	// Execute on attitude
+	Quatf qd(attitude_setpoint.q_d);
+	attitude_control.setAttitudeSetpoint(qd, 0.f);
+	const Vector3f rate_setpoint = attitude_control.update(Quatf());
+	rate_setpoint.print();
+
+	// expect symmetric angular rate command towards thrust direction without any body yaw correction
+	EXPECT_GT(rate_setpoint(0), 0.f);
+	EXPECT_LT(rate_setpoint(1), 0.f);
+	EXPECT_FLOAT_EQ(fabsf(rate_setpoint(0)), fabsf(rate_setpoint(1)));
+	EXPECT_FLOAT_EQ(rate_setpoint(2), 0.f);
+
+	qd.print();
+	Eulerf(qd).print();
+	EXPECT_FLOAT_EQ(2.f * atan2f(qd(3), qd(0)), yaw_setpoint);
+}