新增了利用额外通道屏蔽特定执行器的功能，使用的通道是aux2

src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp

@@ -48,6 +48,21 @@ ControlAllocation::ControlAllocation()
 	_actuator_max.setAll(1.f);
 }
 
+void
+ControlAllocation::zero_stopped_actuators(uint16_t stopped_bitmask)
+{
+	if(stopped_bitmask)
+	{
+		for(int i=0; i<16; i++)
+		{
+			if(stopped_bitmask & (1 << i))
+			{
+				_actuator_sp(i) = 0;
+			}
+		}
+	}
+}
+
 void
 ControlAllocation::setEffectivenessMatrix(
 	const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &effectiveness,

src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp

@@ -93,6 +93,8 @@ class ControlAllocation
 		THRUST_Z
 	};
 
+	void zero_stopped_actuators(uint16_t stopped_bitmask);
+
 	/**
 	 * Allocate control setpoint to actuators
 	 */

src/modules/control_allocator/ControlAllocator.cpp

@@ -46,6 +46,8 @@
 #include <mathlib/math/Limits.hpp>
 #include <mathlib/math/Functions.hpp>
 
+#include <include/HyModeName.hpp>
+
 using namespace matrix;
 using namespace time_literals;
 
@@ -135,6 +137,11 @@ ControlAllocator::parameters_updated()
 		_control_allocation[i]->updateParameters();
 	}
 
+	_stopped_actuator_bitmasks[(int)AllocaterHydroState::StopAll] = 		(uint16_t)0b1111111111111111;
+	_stopped_actuator_bitmasks[(int)AllocaterHydroState::WaterOnly] = 	(uint16_t)0b0000000100111100;
+	_stopped_actuator_bitmasks[(int)AllocaterHydroState::WaterAir] = 	(uint16_t)0b0000000000000000;
+	_stopped_actuator_bitmasks[(int)AllocaterHydroState::AirOnly] = 		(uint16_t)0b0000001011100011;
+
 	update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::CONFIGURATION_UPDATE);
 }
 
@@ -296,6 +303,50 @@ ControlAllocator::update_effectiveness_source()
 	return false;
 }
 
+void
+ControlAllocator::update_allocate_hydro_state()
+{
+	AllocaterHydroState new_state;
+	manual_control_setpoint_s manual_control_setpoint;
+	vehicle_status_s vehicle_status;
+
+	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+	_vehicle_status_sub.copy(&vehicle_status);
+
+	if(vehicle_status.nav_state == HYDRO_MODE_AUTO_DIVE)
+	{
+		new_state = AllocaterHydroState::WaterOnly;
+	}
+	else if(vehicle_status.nav_state == HYDRO_MODE_STABILIZED || vehicle_status.nav_state == HYDRO_MODE_ACRO || vehicle_status.nav_state == HYDRO_MODE_MANUAL)
+	{
+		if(manual_control_setpoint.aux2 < -0.5f)
+		{
+			new_state = AllocaterHydroState::WaterOnly;
+		}
+		else if(manual_control_setpoint.aux2 > 0.5f)
+		{
+			new_state = AllocaterHydroState::AirOnly;
+		}
+		else
+		{
+			new_state = AllocaterHydroState::WaterAir;
+		}
+	}
+	else
+	{
+		new_state = AllocaterHydroState::AirOnly;
+	}
+
+	if(_allocate_hydro_state == new_state)
+	{
+		return;
+	}
+
+	_allocate_hydro_state = new_state;
+	//update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::CONFIGURATION_UPDATE);
+	//PX4_INFO("update_allocate_hydro_state, new_state: %d", (int)new_state);
+}
+
 void
 ControlAllocator::Run()
 {
@@ -371,6 +422,8 @@ ControlAllocator::Run()
 		}
 	}
 
+	update_allocate_hydro_state();
+
 	// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
 	const hrt_abstime now = hrt_absolute_time();
 	const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
@@ -440,6 +493,7 @@ ControlAllocator::Run()
 			_actuator_effectiveness->allocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers
 			_actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp,
 								_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax());
+			_control_allocation[i]->zero_stopped_actuators(_stopped_actuator_bitmasks[(int)_allocate_hydro_state]);//关闭停止的执行器
 
 			if (_has_slew_rate) {
 				_control_allocation[i]->applySlewRateLimit(dt);

src/modules/control_allocator/ControlAllocator.hpp

@@ -78,6 +78,7 @@
 #include <uORB/topics/vehicle_thrust_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/failure_detector_status.h>
+#include <uORB/topics/manual_control_setpoint.h>
 
 class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParams, public px4::ScheduledWorkItem
 {
@@ -130,6 +131,8 @@ class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParam
 	void update_allocation_method(bool force);
 	bool update_effectiveness_source();
 
+	void update_allocate_hydro_state();
+
 	void update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason);
 
 	void check_for_motor_failures();
@@ -190,6 +193,18 @@ class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParam
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)};
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+
+	uint16_t _stopped_actuator_bitmasks[4];		//bitmask，用于指定不同状态下停止运行的执行器
+
+	enum class AllocaterHydroState : int32_t {
+		StopAll = 0,
+		WaterOnly = 1,
+		WaterAir = 2,
+		AirOnly = 3
+	};
+
+	AllocaterHydroState _allocate_hydro_state{AllocaterHydroState::WaterOnly};
 
 	matrix::Vector3f _torque_sp;
 	matrix::Vector3f _thrust_sp;