fw_att_control: publich torque&thrust sp for VTOL during fixed wing mode

PX4 · Nov 19, 2019 · 09bac11 · 09bac11
1 parent 522d0de
commit 09bac11
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 0 deletions.
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
@@ -656,6 +656,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
@@ -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 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
 	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 */