Remove INDEX_COLLECTIVE_TILT from actuator_controls and instead use new topic tiltrotor_extra_controls

Tiltrotor_extra_controls also contains collective thrust beside collective tilt, as passing a 3D
thrust setpoint vector beside the tilt is not feasible.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

msg/ActuatorControls.msg

@@ -11,7 +11,6 @@ uint8 INDEX_AIRBRAKES = 6
 uint8 INDEX_LANDING_GEAR = 7
 uint8 INDEX_GIMBAL_SHUTTER = 3
 uint8 INDEX_CAMERA_ZOOM = 4
-uint8 INDEX_COLLECTIVE_TILT = 8
 
 uint8 GROUP_INDEX_ATTITUDE = 0
 uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1

msg/CMakeLists.txt

@@ -178,6 +178,7 @@ set(msg_files
 	TaskStackInfo.msg
 	TecsStatus.msg
 	TelemetryStatus.msg
+	TiltrotorExtraControls.msg
 	TimesyncStatus.msg
 	TrajectoryBezier.msg
 	TrajectorySetpoint.msg

msg/TiltrotorExtraControls.msg

@@ -0,0 +1,4 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 collective_tilt_normalized_setpoint	# Collective tilt angle of motors of tiltrotor, 0: vertical, 1: horizontal [0, 1]
+float32 collective_thrust_normalized_setpoint 	# Collective thrust setpoint [0, 1]

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp

@@ -43,7 +43,6 @@
 #include "ActuatorEffectivenessRotors.hpp"
 #include "ActuatorEffectivenessControlSurfaces.hpp"
 
-#include <uORB/topics/actuator_controls.h>
 #include <uORB/Subscription.hpp>
 
 class ActuatorEffectivenessTailsitterVTOL : public ModuleParams, public ActuatorEffectiveness
@@ -81,6 +80,4 @@ protected:
 	uint32_t _stopped_motors{}; ///< currently stopped motors
 
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
-
-	uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -110,10 +110,11 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 
 	// apply tilt
 	if (matrix_index == 0) {
-		actuator_controls_s actuator_controls_1;
 
-		if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
+		tiltrotor_extra_controls_s tiltrotor_extra_controls;
-			float control_collective_tilt = actuator_controls_1.control[actuator_controls_s::INDEX_COLLECTIVE_TILT] * 2.f - 1.f;
+
+		if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls)) {
+			float control_collective_tilt = tiltrotor_extra_controls.collective_tilt_normalized_setpoint * 2.f - 1.f;
 
 			// set control_collective_tilt to exactly -1 or 1 if close to these end points
 			control_collective_tilt = control_collective_tilt < -0.99f ? -1.f : control_collective_tilt;
@@ -133,16 +134,12 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 					actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
 				}
 			}
-		}
 
-		// in FW directly use throttle sp
+			// in FW directly use throttle sp
-		if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
+			if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
 
-			actuator_controls_s actuator_controls_0;
-
-			if (_actuator_controls_0_sub.copy(&actuator_controls_0)) {
 				for (int i = 0; i < _first_tilt_idx; ++i) {
-					actuator_sp(i) = actuator_controls_0.control[actuator_controls_s::INDEX_THROTTLE];
+					actuator_sp(i) = tiltrotor_extra_controls.collective_thrust_normalized_setpoint;
 				}
 			}
 		}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -46,6 +46,7 @@
 #include "ActuatorEffectivenessControlSurfaces.hpp"
 #include "ActuatorEffectivenessTilts.hpp"
 
+#include <uORB/topics/tiltrotor_extra_controls.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/Subscription.hpp>
 
@@ -107,4 +108,6 @@ protected:
 	};
 
 	YawTiltSaturationFlags _yaw_tilt_saturation_flags{};
+
+	uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)};
 };

src/modules/logger/logged_topics.cpp

@@ -105,6 +105,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("system_power", 500);
 	add_optional_topic("takeoff_status", 1000);
 	add_optional_topic("tecs_status", 200);
+	add_optional_topic("tiltrotor_extra_controls", 100);
 	add_topic("trajectory_setpoint", 200);
 	add_topic("transponder_report");
 	add_topic("vehicle_acceleration", 50);

src/modules/vtol_att_control/tiltrotor.cpp

@@ -428,16 +428,16 @@ void Tiltrotor::fill_actuator_outputs()
 	_torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
 	_torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW]   * _mc_yaw_weight;
 
+	// Special case tiltrotor: instead of passing a 3D thrust vector (that would mostly have a x-component in FW, and z in MC),
+	// pass the vector magnitude as z-component, plus the collective tilt. Passing 3D thrust plus tilt is not feasible as they
+	// can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated
+	// by the allocator directly.
+	float collective_thrust_normalized_setpoint = 0.f;
+
 	if (_vtol_mode == vtol_mode::FW_MODE) {
 
-		// for the legacy mixing system pubish FW throttle on the MC output
-		mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
-
-		// Special case tiltrotor: instead of passing a 3D thrust vector (that would mostly have a x-component in FW, and z in MC),
-		// pass the vector magnitude as z-component, plus the collective tilt. Passing 3D thrust plus tilt is not feasible as they
-		// can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated
-		// by the allocator directly.
 		_thrust_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_THROTTLE];
+		collective_thrust_normalized_setpoint = fw_in[actuator_controls_s::INDEX_THROTTLE];
 
 		/* allow differential thrust if enabled */
 		if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::YAW_BIT)) {
@@ -446,10 +446,8 @@ void Tiltrotor::fill_actuator_outputs()
 		}
 
 	} else {
-
-		// see comment above for passing magnitude of thrust, not 3D thrust
-		mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
 		_thrust_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
+		collective_thrust_normalized_setpoint = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
 	}
 
 	// Landing gear
@@ -461,8 +459,6 @@ void Tiltrotor::fill_actuator_outputs()
 	}
 
 	// Fixed wing output
-	fw_out[actuator_controls_s::INDEX_COLLECTIVE_TILT] = _tilt_control;
-
 	if (_param_vt_elev_mc_lock.get()  && _vtol_mode == vtol_mode::MC_MODE) {
 		fw_out[actuator_controls_s::INDEX_ROLL]  = 0;
 		fw_out[actuator_controls_s::INDEX_PITCH] = 0;
@@ -485,6 +481,13 @@ void Tiltrotor::fill_actuator_outputs()
 	_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
 
 	_actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time();
+
+	// publish tiltrotor extra controls
+	tiltrotor_extra_controls_s tiltrotor_extra_controls = {};
+	tiltrotor_extra_controls.collective_tilt_normalized_setpoint = _tilt_control;
+	tiltrotor_extra_controls.collective_thrust_normalized_setpoint = collective_thrust_normalized_setpoint;
+	tiltrotor_extra_controls.timestamp = hrt_absolute_time();
+	_tiltrotor_extra_controls_pub.publish(tiltrotor_extra_controls);
 }
 
 void Tiltrotor::blendThrottleAfterFrontTransition(float scale)

src/modules/vtol_att_control/tiltrotor.h

@@ -44,6 +44,9 @@
 #include <parameters/param.h>
 #include <drivers/drv_hrt.h>
 
+#include <uORB/Publication.hpp>
+#include <uORB/topics/tiltrotor_extra_controls.h>
+
 class Tiltrotor : public VtolType
 {
 
@@ -77,6 +80,8 @@ private:
 
 	vtol_mode _vtol_mode{vtol_mode::MC_MODE};			/**< vtol flight mode, defined by enum vtol_mode */
 
+	uORB::Publication<tiltrotor_extra_controls_s>	_tiltrotor_extra_controls_pub{ORB_ID(tiltrotor_extra_controls)};
+
 	float _tilt_control{0.0f};		/**< actuator value for the tilt servo */
 
 	void parameters_update() override;