From b6de83117e9cc638deb1bb2a2e4d5836e4a2c0f0 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Mon, 4 Nov 2019 19:11:54 +0100
Subject: [PATCH] PositionControl: fix attitude setpoint timestamp

The plot of the attitude setpoint in the log did not show any values
because the message timestamp that the position control module sets
was overwritten by the PositionControl attitude generation.
---
 src/modules/mc_pos_control/PositionControl/ControlMath.cpp  | 5 +----
 src/modules/mc_pos_control/PositionControl/ControlMath.hpp  | 3 ++-
 .../mc_pos_control/PositionControl/ControlMathTest.cpp      | 6 +++---
 .../mc_pos_control/PositionControl/PositionControl.cpp      | 2 +-
 4 files changed, 7 insertions(+), 9 deletions(-)

diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp
index 7bc9c6120f..495954430d 100644
--- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp
+++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp
@@ -44,9 +44,8 @@
 using namespace matrix;
 namespace ControlMath
 {
-vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp)
+void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
 {
-	vehicle_attitude_setpoint_s att_sp = {};
 	att_sp.yaw_body = yaw_sp;
 
 	// desired body_z axis = -normalize(thrust_vector)
@@ -103,8 +102,6 @@ vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float
 	att_sp.roll_body = euler(0);
 	att_sp.pitch_body = euler(1);
 	att_sp.thrust_body[2] = -thr_sp.length();
-
-	return att_sp;
 }
 
 Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp
index 47390004b1..abc0e98234 100644
--- a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp
+++ b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp
@@ -51,7 +51,8 @@ namespace ControlMath
  * @param yaw_sp the desired yaw
  * @return vehicle_attitude_setpoints_s structure
  */
-vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp);
+void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp,
+		      vehicle_attitude_setpoint_s &attitude_setpoint);
 
 /**
  * Outputs the sum of two vectors but respecting the limits and priority.
diff --git a/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp b/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp
index 0594d15bb5..daf009159b 100644
--- a/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp
+++ b/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp
@@ -46,7 +46,7 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
 	Vector3f thr{0.0f, 0.0f, -1.0f};
 	float yaw = 0.0f;
 	vehicle_attitude_setpoint_s att{};
-	att = thrustToAttitude(thr, yaw);
+	thrustToAttitude(thr, yaw, att);
 	EXPECT_EQ(att.roll_body, 0);
 	EXPECT_EQ(att.pitch_body, 0);
 	EXPECT_EQ(att.yaw_body, 0);
@@ -55,7 +55,7 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
 	/* expected: same as before but with 90 yaw
 	 * reason: only yaw changed */
 	yaw = M_PI_2_F;
-	att = thrustToAttitude(thr, yaw);
+	thrustToAttitude(thr, yaw, att);
 	EXPECT_EQ(att.roll_body, 0);
 	EXPECT_EQ(att.pitch_body, 0);
 	EXPECT_EQ(att.yaw_body, M_PI_2_F);
@@ -65,7 +65,7 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
 	 * reason: thrust points straight down and order Euler
 	 * order is: 1. roll, 2. pitch, 3. yaw */
 	thr = Vector3f(0.0f, 0.0f, 1.0f);
-	att = thrustToAttitude(thr, yaw);
+	thrustToAttitude(thr, yaw, att);
 	EXPECT_NEAR(att.roll_body, -M_PI_F, 1e-4);
 	EXPECT_EQ(att.pitch_body, 0);
 	EXPECT_EQ(att.yaw_body, M_PI_2_F);
diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
index c75c0f703f..3db656e877 100644
--- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
@@ -367,7 +367,7 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
 
 void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const
 {
-	attitude_setpoint = ControlMath::thrustToAttitude(_thr_sp, _yaw_sp);
+	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint);
 	attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
 }