From 5da67e2e28e09eeb4535ce3a71c47c6f62a81d14 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 29 Nov 2021 13:50:47 +0100
Subject: [PATCH] vtol_att_control: publish vehicle_thrust_setpoint &
 vehicle_torque_setpoint

---
 .../vtol_att_control_main.cpp                 | 39 +++++++++++++++++++
 .../vtol_att_control/vtol_att_control_main.h  |  9 +++++
 2 files changed, 48 insertions(+)

diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index ad17e7865d..bc50a6703a 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -118,6 +118,10 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	}
 
 	_vtol_vehicle_status_pub.advertise();
+	_vehicle_thrust_setpoint0_pub.advertise();
+	_vehicle_torque_setpoint0_pub.advertise();
+	_vehicle_thrust_setpoint1_pub.advertise();
+	_vehicle_torque_setpoint1_pub.advertise();
 }
 
 VtolAttitudeControl::~VtolAttitudeControl()
@@ -499,6 +503,8 @@ VtolAttitudeControl::Run()
 		_vtol_type->fill_actuator_outputs();
 		_actuators_0_pub.publish(_actuators_out_0);
 		_actuators_1_pub.publish(_actuators_out_1);
+		publishTorqueSetpoints(0);
+		publishThrustSetpoints(0);
 
 		// Advertise/Publish vtol vehicle status
 		_vtol_vehicle_status.timestamp = hrt_absolute_time();
@@ -508,6 +514,39 @@ VtolAttitudeControl::Run()
 	perf_end(_loop_perf);
 }
 
+void VtolAttitudeControl::publishTorqueSetpoints(const hrt_abstime &timestamp_sample)
+{
+	vehicle_torque_setpoint_s v_torque_sp = {};
+	v_torque_sp.timestamp = hrt_absolute_time();
+	v_torque_sp.timestamp_sample = timestamp_sample;
+	v_torque_sp.xyz[0] = _actuators_out_0.control[actuator_controls_s::INDEX_ROLL];
+	v_torque_sp.xyz[1] = _actuators_out_0.control[actuator_controls_s::INDEX_PITCH];
+	v_torque_sp.xyz[2] = _actuators_out_0.control[actuator_controls_s::INDEX_YAW];
+	_vehicle_torque_setpoint0_pub.publish(v_torque_sp);
+
+	v_torque_sp.xyz[0] = _actuators_out_1.control[actuator_controls_s::INDEX_ROLL];
+	v_torque_sp.xyz[1] = _actuators_out_1.control[actuator_controls_s::INDEX_PITCH];
+	v_torque_sp.xyz[2] = _actuators_out_1.control[actuator_controls_s::INDEX_YAW];
+	_vehicle_torque_setpoint1_pub.publish(v_torque_sp);
+}
+
+void VtolAttitudeControl::publishThrustSetpoints(const hrt_abstime &timestamp_sample)
+{
+	vehicle_thrust_setpoint_s v_thrust_sp = {};
+	v_thrust_sp.timestamp = hrt_absolute_time();
+	v_thrust_sp.timestamp_sample = timestamp_sample;
+	// TODO: separate thrust
+	v_thrust_sp.xyz[0] = _actuators_out_0.control[actuator_controls_s::INDEX_THROTTLE];
+	v_thrust_sp.xyz[1] = 0.0f;
+	v_thrust_sp.xyz[2] = -_actuators_out_0.control[actuator_controls_s::INDEX_THROTTLE];
+	_vehicle_thrust_setpoint0_pub.publish(v_thrust_sp);
+
+	v_thrust_sp.xyz[0] = _actuators_out_1.control[actuator_controls_s::INDEX_THROTTLE];
+	v_thrust_sp.xyz[1] = 0.0f;
+	v_thrust_sp.xyz[2] = 0.0f;
+	_vehicle_thrust_setpoint1_pub.publish(v_thrust_sp);
+}
+
 int
 VtolAttitudeControl::task_spawn(int argc, char *argv[])
 {
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h
index 88c42e7b05..8929cb3bb0 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.h
+++ b/src/modules/vtol_att_control/vtol_att_control_main.h
@@ -62,6 +62,7 @@
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/px4_work_queue/WorkItem.hpp>
 #include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/action_request.h>
@@ -80,6 +81,8 @@
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_thrust_setpoint.h>
+#include <uORB/topics/vehicle_torque_setpoint.h>
 #include "standard.h"
 #include "tailsitter.h"
 #include "tiltrotor.h"
@@ -142,6 +145,8 @@ public:
 	struct Params 					*get_params() {return &_params;}
 
 private:
+	void publishTorqueSetpoints(const hrt_abstime &timestamp_sample);
+	void publishThrustSetpoints(const hrt_abstime &timestamp_sample);
 
 	void Run() override;
 
@@ -168,6 +173,10 @@ private:
 	uORB::Publication<actuator_controls_s>		_actuators_1_pub{ORB_ID(actuator_controls_1)};
 	uORB::Publication<vehicle_attitude_setpoint_s>	_v_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Publication<vtol_vehicle_status_s>	_vtol_vehicle_status_pub{ORB_ID(vtol_vehicle_status)};
+	uORB::PublicationMulti<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint0_pub{ORB_ID(vehicle_thrust_setpoint)};
+	uORB::PublicationMulti<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint0_pub{ORB_ID(vehicle_torque_setpoint)};
+	uORB::PublicationMulti<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint1_pub{ORB_ID(vehicle_thrust_setpoint)};
+	uORB::PublicationMulti<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint1_pub{ORB_ID(vehicle_torque_setpoint)};
 
 	orb_advert_t	_mavlink_log_pub{nullptr};	// mavlink log uORB handle