From 198605d258c603b89aa82d0e7e7c02b46d77c57d Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 22 Oct 2019 18:38:47 +0200
Subject: [PATCH] PositionControl: add getter for output position setpoint

---
 .../PositionControl/PositionControl.cpp       | 16 +++++++-
 .../PositionControl/PositionControl.hpp       | 37 +++----------------
 .../mc_pos_control/mc_pos_control_main.cpp    |  7 +---
 3 files changed, 21 insertions(+), 39 deletions(-)

diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
index 8dfd8d5c07..73a939c716 100644
--- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
@@ -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
@@ -351,6 +351,20 @@ void PositionControl::updateConstraints(const vehicle_constraints_s &constraints
 	}
 }
 
+void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint)
+{
+	local_position_setpoint.x = _pos_sp(0);
+	local_position_setpoint.y = _pos_sp(1);
+	local_position_setpoint.z = _pos_sp(2);
+	local_position_setpoint.yaw = _yaw_sp;
+	local_position_setpoint.yawspeed = _yawspeed_sp;
+	local_position_setpoint.vx = _vel_sp(0);
+	local_position_setpoint.vy = _vel_sp(1);
+	local_position_setpoint.vz = _vel_sp(2);
+	_acc_sp.copyTo(local_position_setpoint.acceleration);
+	_thr_sp.copyTo(local_position_setpoint.thrust);
+}
+
 void PositionControl::updateParams()
 {
 	ModuleParams::updateParams();
diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
index 2b0eb74a3c..17d92445da 100644
--- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
@@ -128,20 +128,6 @@ public:
 	 */
 	void resetIntegralZ() { _thr_int(2) = 0.0f; }
 
-	/**
-	 * 	Get the
-	 * 	@see _thr_sp
-	 * 	@return The thrust set-point member.
-	 */
-	const matrix::Vector3f &getThrustSetpoint() { return _thr_sp; }
-
-	/**
-	 * 	Get the
-	 * 	@see _yaw_sp
-	 * 	@return The yaw set-point member.
-	 */
-	const float &getYawSetpoint() { return _yaw_sp; }
-
 	/**
 	 * 	Get the
 	 * 	@see _yawspeed_sp
@@ -171,25 +157,12 @@ public:
 	}
 
 	/**
-	 * 	Get the
-	 * 	@see _pos_sp
-	 * 	@return The position set-point that was executed in the control-loop. Nan if the position control-loop was skipped.
+	 * Get the controllers output local position setpoint
+	 * These setpoints are the ones which were executed on including PID output and feed-forward.
+	 * The acceleration or thrust setpoints can be used for attitude control.
+	 * @param local_position_setpoint reference to struct to fill up
 	 */
-	const matrix::Vector3f getPosSp()
-	{
-		matrix::Vector3f pos_sp{};
-
-		for (int i = 0; i <= 2; i++) {
-			if (_ctrl_pos[i]) {
-				pos_sp(i) = _pos_sp(i);
-
-			} else {
-				pos_sp(i) = NAN;
-			}
-		}
-
-		return pos_sp;
-	}
+	void getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint);
 
 protected:
 
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 2052b9a787..a8ea009207 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -668,15 +668,10 @@ MulticopterPositionControl::Run()
 			// PositionController.
 			vehicle_local_position_setpoint_s local_pos_sp{};
 			local_pos_sp.timestamp = hrt_absolute_time();
-			local_pos_sp.x = setpoint.x;
-			local_pos_sp.y = setpoint.y;
-			local_pos_sp.z = setpoint.z;
-			local_pos_sp.yaw = _control.getYawSetpoint();
-			local_pos_sp.yawspeed = _control.getYawspeedSetpoint();
+			_control.getLocalPositionSetpoint(local_pos_sp);
 			local_pos_sp.vx = PX4_ISFINITE(_control.getVelSp()(0)) ? _control.getVelSp()(0) : setpoint.vx;
 			local_pos_sp.vy = PX4_ISFINITE(_control.getVelSp()(1)) ? _control.getVelSp()(1) : setpoint.vy;
 			local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz;
-			_control.getThrustSetpoint().copyTo(local_pos_sp.thrust);
 
 			// Publish local position setpoint
 			// This message will be used by other modules (such as Landdetector) to determine