control_allocator: handle thrust allocation for VTOL's properly

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

src/modules/vtol_att_control/standard.cpp

@@ -413,6 +413,30 @@ void Standard::fill_actuator_outputs()
 		break;
 	}
 
+	_torque_setpoint_0->timestamp = hrt_absolute_time();
+	_torque_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
+	_torque_setpoint_0->xyz[0] = mc_out[actuator_controls_s::INDEX_ROLL];
+	_torque_setpoint_0->xyz[1] = mc_out[actuator_controls_s::INDEX_PITCH];
+	_torque_setpoint_0->xyz[2] = mc_out[actuator_controls_s::INDEX_YAW];
+
+	_torque_setpoint_1->timestamp = hrt_absolute_time();
+	_torque_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
+	_torque_setpoint_1->xyz[0] = fw_out[actuator_controls_s::INDEX_ROLL];
+	_torque_setpoint_1->xyz[1] = fw_out[actuator_controls_s::INDEX_PITCH];
+	_torque_setpoint_1->xyz[2] = fw_out[actuator_controls_s::INDEX_YAW];
+
+	_thrust_setpoint_0->timestamp = hrt_absolute_time();
+	_thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
+	_thrust_setpoint_0->xyz[0] = 0.f;
+	_thrust_setpoint_0->xyz[1] = 0.f;
+	_thrust_setpoint_0->xyz[2] = -mc_in[actuator_controls_s::INDEX_THROTTLE];
+
+	_thrust_setpoint_1->timestamp = hrt_absolute_time();
+	_thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
+	_thrust_setpoint_1->xyz[0] = fw_in[actuator_controls_s::INDEX_THROTTLE];
+	_thrust_setpoint_1->xyz[1] = 0.f;
+	_thrust_setpoint_1->xyz[2] = 0.f;
+
 	_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
 	_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
 

src/modules/vtol_att_control/tiltrotor.cpp

@@ -462,21 +462,57 @@ void Tiltrotor::fill_actuator_outputs()
 	auto &mc_out = _actuators_out_0->control;
 	auto &fw_out = _actuators_out_1->control;
 
+	_torque_setpoint_0->timestamp = hrt_absolute_time();
+	_torque_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
+	_torque_setpoint_0->xyz[0] = 0.f;
+	_torque_setpoint_0->xyz[1] = 0.f;
+	_torque_setpoint_0->xyz[2] = 0.f;
+
+	_torque_setpoint_1->timestamp = hrt_absolute_time();
+	_torque_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
+	_torque_setpoint_1->xyz[0] = 0.f;
+	_torque_setpoint_1->xyz[1] = 0.f;
+	_torque_setpoint_1->xyz[2] = 0.f;
+
+	_thrust_setpoint_0->timestamp = hrt_absolute_time();
+	_thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
+	_thrust_setpoint_0->xyz[0] = 0.f;
+	_thrust_setpoint_0->xyz[1] = 0.f;
+	_thrust_setpoint_0->xyz[2] = 0.f;
+
+	_thrust_setpoint_1->timestamp = hrt_absolute_time();
+	_thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
+	_thrust_setpoint_1->xyz[0] = 0.f;
+	_thrust_setpoint_1->xyz[1] = 0.f;
+	_thrust_setpoint_1->xyz[2] = 0.f;
+
 	// Multirotor output
 	mc_out[actuator_controls_s::INDEX_ROLL]  = mc_in[actuator_controls_s::INDEX_ROLL]  * _mc_roll_weight;
 	mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
 	mc_out[actuator_controls_s::INDEX_YAW]   = mc_in[actuator_controls_s::INDEX_YAW]   * _mc_yaw_weight;
 
+	_torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL]  * _mc_roll_weight;
+	_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;
+
 	if (_vtol_schedule.flight_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];
 
+		// FW thrust is allocated on mc_thrust_sp[0] for tiltrotor with dynamic control allocation
+		_thrust_setpoint_0->xyz[0] = fw_in[actuator_controls_s::INDEX_THROTTLE];
+
 		/* allow differential thrust if enabled */
 		if (_params->diff_thrust == 1) {
 			mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
+			_torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
 		}
 
 	} else {
+
 		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;
 	}
 
 	// Fixed wing output
@@ -491,6 +527,9 @@ void Tiltrotor::fill_actuator_outputs()
 		fw_out[actuator_controls_s::INDEX_ROLL]  = fw_in[actuator_controls_s::INDEX_ROLL];
 		fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH];
 		fw_out[actuator_controls_s::INDEX_YAW]   = fw_in[actuator_controls_s::INDEX_YAW];
+		_torque_setpoint_1->xyz[0] = fw_in[actuator_controls_s::INDEX_ROLL];
+		_torque_setpoint_1->xyz[1] = fw_in[actuator_controls_s::INDEX_PITCH];
+		_torque_setpoint_1->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW];
 	}
 
 	_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -513,8 +513,11 @@ 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);
+
+		_vehicle_torque_setpoint0_pub.publish(_torque_setpoint_0);
+		_vehicle_torque_setpoint1_pub.publish(_torque_setpoint_1);
+		_vehicle_thrust_setpoint0_pub.publish(_thrust_setpoint_0);
+		_vehicle_thrust_setpoint1_pub.publish(_thrust_setpoint_1);
 
 		// Advertise/Publish vtol vehicle status
 		_vtol_vehicle_status.timestamp = hrt_absolute_time();
@@ -524,39 +527,6 @@ 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[])
 {

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -142,12 +142,14 @@ public:
 	struct vehicle_local_position_setpoint_s	*get_local_pos_sp() {return &_local_pos_sp;}
 	struct vtol_vehicle_status_s			*get_vtol_vehicle_status() {return &_vtol_vehicle_status;}
 
+	struct vehicle_torque_setpoint_s 		*get_torque_setpoint_0() {return &_torque_setpoint_0;}
+	struct vehicle_torque_setpoint_s 		*get_torque_setpoint_1() {return &_torque_setpoint_1;}
+	struct vehicle_thrust_setpoint_s 		*get_thrust_setpoint_0() {return &_thrust_setpoint_0;}
+	struct vehicle_thrust_setpoint_s 		*get_thrust_setpoint_1() {return &_thrust_setpoint_1;}
+
 	struct Params 					*get_params() {return &_params;}
 
 private:
-	void publishTorqueSetpoints(const hrt_abstime &timestamp_sample);
-	void publishThrustSetpoints(const hrt_abstime &timestamp_sample);
-
 	void Run() override;
 
 	uORB::SubscriptionCallbackWorkItem _actuator_inputs_fw{this, ORB_ID(actuator_controls_virtual_fw)};
@@ -189,6 +191,11 @@ private:
 	actuator_controls_s			_actuators_out_0{};	//actuator controls going to the mc mixer
 	actuator_controls_s			_actuators_out_1{};	//actuator controls going to the fw mixer (used for elevons)
 
+	vehicle_torque_setpoint_s		_torque_setpoint_0{};
+	vehicle_torque_setpoint_s		_torque_setpoint_1{};
+	vehicle_thrust_setpoint_s		_thrust_setpoint_0{};
+	vehicle_thrust_setpoint_s		_thrust_setpoint_1{};
+
 	airspeed_validated_s 				_airspeed_validated{};			// airspeed
 	position_setpoint_triplet_s		_pos_sp_triplet{};
 	tecs_status_s				_tecs_status{};

src/modules/vtol_att_control/vtol_type.cpp

@@ -65,6 +65,10 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	_actuators_out_1 = _attc->get_actuators_out1();
 	_actuators_mc_in = _attc->get_actuators_mc_in();
 	_actuators_fw_in = _attc->get_actuators_fw_in();
+	_torque_setpoint_0 = _attc->get_torque_setpoint_0();
+	_torque_setpoint_1 = _attc->get_torque_setpoint_1();
+	_thrust_setpoint_0 = _attc->get_thrust_setpoint_0();
+	_thrust_setpoint_1 = _attc->get_thrust_setpoint_1();
 	_local_pos = _attc->get_local_pos();
 	_local_pos_sp = _attc->get_local_pos_sp();
 	_airspeed_validated = _attc->get_airspeed();

src/modules/vtol_att_control/vtol_type.h

@@ -218,6 +218,11 @@ public:
 	struct tecs_status_s				*_tecs_status;
 	struct vehicle_land_detected_s			*_land_detected;
 
+	struct vehicle_torque_setpoint_s 		*_torque_setpoint_0;
+	struct vehicle_torque_setpoint_s 		*_torque_setpoint_1;
+	struct vehicle_thrust_setpoint_s 		*_thrust_setpoint_0;
+	struct vehicle_thrust_setpoint_s 		*_thrust_setpoint_1;
+
 	struct Params 					*_params;
 
 	bool _flag_idle_mc = false;		//false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"