port #1752 to multiplatform

1 files changed, 24 insertions, 16 deletions

diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
index e6a7ee8a6..1fe75b90c 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
@@ -620,22 +620,6 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 			control_auto(dt);
 		}
 
-		/* fill local position setpoint */
-		_local_pos_sp_msg.data().timestamp = get_time_micros();
-		_local_pos_sp_msg.data().x = _pos_sp(0);
-		_local_pos_sp_msg.data().y = _pos_sp(1);
-		_local_pos_sp_msg.data().z = _pos_sp(2);
-		_local_pos_sp_msg.data().yaw = _att_sp_msg.data().yaw_body;
-
-		/* publish local position setpoint */
-		if (_local_pos_sp_pub != nullptr) {
-			_local_pos_sp_pub->publish(_local_pos_sp_msg);
-
-		} else {
-			_local_pos_sp_pub = _n.advertise<px4_vehicle_local_position_setpoint>();
-		}
-
-
 		if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_IDLE) {
 			/* idle state, don't run controller and set zero thrust */
 			_R.identity();
@@ -935,6 +919,11 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 
 				_att_sp_msg.data().thrust = thrust_abs;
 
+				/* save thrust setpoint for logging */
+				_local_pos_sp_msg.data().acc_x = thrust_sp(0);
+				_local_pos_sp_msg.data().acc_x = thrust_sp(1);
+				_local_pos_sp_msg.data().acc_x = thrust_sp(2);
+
 				_att_sp_msg.data().timestamp = get_time_micros();
 
 				/* publish attitude setpoint */
@@ -950,6 +939,25 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 			}
 		}
 
+		/* fill local position setpoint */
+		_local_pos_sp_msg.data().timestamp = get_time_micros();
+		_local_pos_sp_msg.data().x = _pos_sp(0);
+		_local_pos_sp_msg.data().y = _pos_sp(1);
+		_local_pos_sp_msg.data().z = _pos_sp(2);
+		_local_pos_sp_msg.data().yaw = _att_sp_msg.data().yaw_body;
+		_local_pos_sp_msg.data().vx = _vel_sp(0);
+		_local_pos_sp_msg.data().vy = _vel_sp(1);
+		_local_pos_sp_msg.data().vz = _vel_sp(2);
+
+		/* publish local position setpoint */
+		if (_local_pos_sp_pub != nullptr) {
+			_local_pos_sp_pub->publish(_local_pos_sp_msg);
+
+		} else {
+			_local_pos_sp_pub = _n.advertise<px4_vehicle_local_position_setpoint>();
+		}
+
+
 	} else {
 		/* position controller disabled, reset setpoints */
 		_reset_alt_sp = true;