From a4a9d50a9774ab6e1cd089153fb620c7d2151f15 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Sat, 7 Dec 2019 15:47:07 +0100
Subject: [PATCH] ControlMath: refactor thrustToAttitude calculation

---
 .../PositionControl/ControlMath.cpp           | 51 +++++++++----------
 .../PositionControl/ControlMath.hpp           | 16 ++++--
 2 files changed, 36 insertions(+), 31 deletions(-)

diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp
index 495954430d..a0638d3415 100644
--- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp
+++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp
@@ -1,7 +1,6 @@
-
 /****************************************************************************
  *
- *   Copyright (C) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2018 - 2019 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
@@ -42,46 +41,46 @@
 #include <mathlib/mathlib.h>
 
 using namespace matrix;
+
 namespace ControlMath
 {
 void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
 {
-	att_sp.yaw_body = yaw_sp;
+	bodyzToAttitude(-thr_sp, yaw_sp, att_sp);
+	att_sp.thrust_body[2] = -thr_sp.length();
+}
 
-	// desired body_z axis = -normalize(thrust_vector)
-	Vector3f body_x, body_y, body_z;
-
-	if (thr_sp.length() > 0.00001f) {
-		body_z = -thr_sp.normalized();
-
-	} else {
-		// no thrust, set Z axis to safe value
-		body_z = Vector3f(0.f, 0.f, 1.f);
+void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
+{
+	// zero vector, no direction, set safe level value
+	if (body_z.norm_squared() < FLT_EPSILON) {
+		body_z(2) = 1.f;
 	}
 
+	body_z.normalize();
+
 	// vector of desired yaw direction in XY plane, rotated by PI/2
-	Vector3f y_C(-sinf(att_sp.yaw_body), cosf(att_sp.yaw_body), 0.0f);
+	Vector3f y_C(-sinf(yaw_sp), cosf(yaw_sp), 0.0f);
 
-	if (fabsf(body_z(2)) > 0.000001f) {
-		// desired body_x axis, orthogonal to body_z
-		body_x = y_C % body_z;
+	// 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.0f) {
-			body_x = -body_x;
-		}
+	// keep nose to front while inverted upside down
+	if (body_z(2) < 0.0f) {
+		body_x = -body_x;
+	}
 
-		body_x.normalize();
-
-	} else {
+	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.0f;
 	}
 
+	body_x.normalize();
+
 	// desired body_y axis
-	body_y = body_z % body_x;
+	Vector3f body_y = body_z % body_x;
 
 	Dcmf R_sp;
 
@@ -92,7 +91,7 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitu
 		R_sp(i, 2) = body_z(i);
 	}
 
-	//copy quaternion setpoint to attitude setpoint topic
+	// copy quaternion setpoint to attitude setpoint topic
 	Quatf q_sp = R_sp;
 	q_sp.copyTo(att_sp.q_d);
 	att_sp.q_d_valid = true;
@@ -101,7 +100,7 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitu
 	Eulerf euler = R_sp;
 	att_sp.roll_body = euler(0);
 	att_sp.pitch_body = euler(1);
-	att_sp.thrust_body[2] = -thr_sp.length();
+	att_sp.yaw_body = euler(2);
 }
 
 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 abc0e98234..eed3e483c3 100644
--- a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp
+++ b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2018 - 2019 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
@@ -47,12 +47,18 @@ namespace ControlMath
 {
 /**
  * Converts thrust vector and yaw set-point to a desired attitude.
- * @param thr_sp a 3D vector
+ * @param thr_sp desired 3D thrust vector
  * @param yaw_sp the desired yaw
- * @return vehicle_attitude_setpoints_s structure
+ * @param att_sp attitude setpoint to fill
  */
-void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp,
-		      vehicle_attitude_setpoint_s &attitude_setpoint);
+void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
+/**
+ * Converts a body z vector and yaw set-point to a desired attitude.
+ * @param body_z a world frame 3D vector in direction of the desired body z axis
+ * @param yaw_sp the desired yaw setpoint
+ * @param att_sp attitude setpoint to fill
+ */
+void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
 
 /**
  * Outputs the sum of two vectors but respecting the limits and priority.