From 28e995ede25bf9a4348b2686089c31bee9b0293d 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:52:23 +0100
Subject: [PATCH] fw_att_control: publish vehicle_thrust_setpoint &
 vehicle_torque_setpoint

---
 .../FixedwingAttitudeControl.cpp              | 29 +++++++++++++++++++
 .../FixedwingAttitudeControl.hpp              |  7 +++++
 2 files changed, 36 insertions(+)

diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
index cc8789f4e7..5f1b6bb07d 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
@@ -660,6 +660,11 @@ void FixedwingAttitudeControl::Run()
 		    _vcontrol_mode.flag_control_attitude_enabled ||
 		    _vcontrol_mode.flag_control_manual_enabled) {
 			_actuators_0_pub.publish(_actuators);
+
+			if (!_vehicle_status.is_vtol) {
+				publishTorqueSetpoint(angular_velocity.timestamp_sample);
+				publishThrustSetpoint(angular_velocity.timestamp_sample);
+			}
 		}
 
 		updateActuatorControlsStatus(dt);
@@ -668,6 +673,30 @@ void FixedwingAttitudeControl::Run()
 	perf_end(_loop_perf);
 }
 
+void FixedwingAttitudeControl::publishTorqueSetpoint(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.control[actuator_controls_s::INDEX_ROLL];
+	v_torque_sp.xyz[1] = _actuators.control[actuator_controls_s::INDEX_PITCH];
+	v_torque_sp.xyz[2] = _actuators.control[actuator_controls_s::INDEX_YAW];
+
+	_vehicle_torque_setpoint_pub.publish(v_torque_sp);
+}
+
+void FixedwingAttitudeControl::publishThrustSetpoint(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;
+	v_thrust_sp.xyz[0] = _actuators.control[actuator_controls_s::INDEX_THROTTLE];
+	v_thrust_sp.xyz[1] = 0.0f;
+	v_thrust_sp.xyz[2] = 0.0f;
+
+	_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
+}
+
 void FixedwingAttitudeControl::control_flaps(const float dt)
 {
 	/* default flaps to center */
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
index 9d843cebe3..722c10dbd8 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
@@ -68,6 +68,8 @@
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_thrust_setpoint.h>
+#include <uORB/topics/vehicle_torque_setpoint.h>
 
 using matrix::Eulerf;
 using matrix::Quatf;
@@ -97,6 +99,9 @@ public:
 private:
 	void Run() override;
 
+	void publishTorqueSetpoint(const hrt_abstime &timestamp_sample);
+	void publishThrustSetpoint(const hrt_abstime &timestamp_sample);
+
 	uORB::SubscriptionCallbackWorkItem _att_sub{this, ORB_ID(vehicle_attitude)};	/**< vehicle attitude */
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@@ -119,6 +124,8 @@ private:
 	uORB::Publication<vehicle_attitude_setpoint_s>	_attitude_sp_pub;
 	uORB::Publication<vehicle_rates_setpoint_s>	_rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};
 	uORB::PublicationMulti<rate_ctrl_status_s>	_rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
+	uORB::Publication<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
+	uORB::Publication<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
 
 	actuator_controls_s			_actuators {};		/**< actuator control inputs */
 	manual_control_setpoint_s		_manual_control_setpoint {};		/**< r/c channel data */