From 0bbfc36f117714d2196ff6242bdc87dc14f0721e Mon Sep 17 00:00:00 2001
From: Julien Lecoeur <julien.lecoeur@gmail.com>
Date: Tue, 19 Nov 2019 16:01:40 +0100
Subject: [PATCH] fw_att_control: publich torque&thrust sp for VTOL during
 fixed wing mode

---
 .../FixedwingAttitudeControl.cpp              | 22 +++++++++++++++++++
 .../FixedwingAttitudeControl.hpp              |  4 ++++
 2 files changed, 26 insertions(+)

diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
index 5fcd970d64..e80b0e2a0f 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
@@ -655,6 +655,28 @@ void FixedwingAttitudeControl::Run()
 			}
 
 			_actuators_2_pub.publish(_actuators_airframe);
+
+			/* publish thrust and torque setpoint */
+			if (!_vehicle_status.is_vtol
+			    || (_vehicle_status.is_vtol && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING))
+			   ) {
+				vehicle_torque_setpoint_s v_torque_sp = {};
+				v_torque_sp.timestamp = hrt_absolute_time();
+				v_torque_sp.timestamp_sample = _att.timestamp;
+				v_torque_sp.xyz[0] = (PX4_ISFINITE(_actuators.control[0])) ? _actuators.control[0] : 0.0f;
+				v_torque_sp.xyz[1] = (PX4_ISFINITE(_actuators.control[1])) ? _actuators.control[1] : 0.0f;
+				v_torque_sp.xyz[2] = (PX4_ISFINITE(_actuators.control[2])) ? _actuators.control[2] : 0.0f;
+				_vehicle_torque_setpoint_pub.publish(v_torque_sp);
+
+				vehicle_thrust_setpoint_s v_thrust_sp = {};
+				v_thrust_sp.timestamp = hrt_absolute_time();
+				v_thrust_sp.timestamp_sample = _att.timestamp;
+				v_thrust_sp.xyz[0] = (PX4_ISFINITE(_actuators.control[3])) ? _actuators.control[3] : 0.0f;
+				v_thrust_sp.xyz[1] = 0.0f;
+				v_thrust_sp.xyz[2] = 0.0f;
+				_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
+
+			}
 		}
 	}
 
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
index 660d5c4d78..0a664187b1 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
@@ -66,6 +66,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;
@@ -115,6 +117,8 @@ private:
 	uORB::Publication<actuator_controls_s>		_actuators_2_pub{ORB_ID(actuator_controls_2)};		/**< actuator control group 1 setpoint (Airframe) */
 	uORB::Publication<vehicle_rates_setpoint_s>	_rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};		/**< rate setpoint publication */
 	uORB::PublicationMulti<rate_ctrl_status_s>	_rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};	/**< rate controller status publication */
+	uORB::Publication<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};		/**< vehicle_thrust_setpoint publication */
+	uORB::Publication<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};		/**< vehicle_torque_setpoint publication */
 
 	orb_id_t	_attitude_setpoint_id{nullptr};
 	orb_advert_t	_attitude_sp_pub{nullptr};	/**< attitude setpoint point */