HelicopterCoaxial: adjust for coaxial allocation

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp

@@ -51,19 +51,6 @@ ActuatorEffectivenessHelicopterCoaxial::ActuatorEffectivenessHelicopterCoaxial(M
 	}
 
 	_param_handles.num_swash_plate_servos = param_find("CA_SP0_COUNT");
-
-	for (int i = 0; i < NUM_CURVE_POINTS; ++i) {
-		char buffer[17];
-		snprintf(buffer, sizeof(buffer), "CA_HELI_THR_C%u", i);
-		_param_handles.throttle_curve[i] = param_find(buffer);
-		snprintf(buffer, sizeof(buffer), "CA_HELI_PITCH_C%u", i);
-		_param_handles.pitch_curve[i] = param_find(buffer);
-	}
-
-	_param_handles.yaw_collective_pitch_scale = param_find("CA_HELI_YAW_CP_S");
-	_param_handles.yaw_collective_pitch_offset = param_find("CA_HELI_YAW_CP_O");
-	_param_handles.yaw_throttle_scale = param_find("CA_HELI_YAW_TH_S");
-	_param_handles.yaw_ccw = param_find("CA_HELI_YAW_CCW");
 	_param_handles.spoolup_time = param_find("COM_SPOOLUP_TIME");
 
 	updateParams();
@@ -90,18 +77,7 @@ void ActuatorEffectivenessHelicopterCoaxial::updateParams()
 		param_get(_param_handles.swash_plate_servos[i].trim, &_geometry.swash_plate_servos[i].trim);
 	}
 
-	for (int i = 0; i < NUM_CURVE_POINTS; ++i) {
-		param_get(_param_handles.throttle_curve[i], &_geometry.throttle_curve[i]);
-		param_get(_param_handles.pitch_curve[i], &_geometry.pitch_curve[i]);
-	}
-
-	param_get(_param_handles.yaw_collective_pitch_scale, &_geometry.yaw_collective_pitch_scale);
-	param_get(_param_handles.yaw_collective_pitch_offset, &_geometry.yaw_collective_pitch_offset);
-	param_get(_param_handles.yaw_throttle_scale, &_geometry.yaw_throttle_scale);
 	param_get(_param_handles.spoolup_time, &_geometry.spoolup_time);
-	int32_t yaw_ccw = 0;
-	param_get(_param_handles.yaw_ccw, &yaw_ccw);
-	_geometry.yaw_sign = (yaw_ccw == 1) ? -1.f : 1.f;
 }
 
 bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuration &configuration,
@@ -112,10 +88,8 @@ bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuratio
 	}
 
 	// As the allocation is non-linear, we use updateSetpoint() instead of the matrix
-	configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{});
+	configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); // Clockwise rotor
-
+	configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); // Counter-clockwise rotor
-	// Tail (yaw) motor
-	configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{});
 
 	// N swash plate servos
 	_first_swash_plate_servo_index = configuration.num_actuators_matrix[0];
@@ -135,32 +109,28 @@ void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector
 	_saturation_flags = {};
 
 	// throttle/collective pitch curve
-	const float throttle = math::interpolateN(-control_sp(ControlAxis::THRUST_Z),
+	const float throttle = -control_sp(ControlAxis::THRUST_Z) * throttleSpoolupProgress();
-			       _geometry.throttle_curve) * throttleSpoolupProgress();
+	const float yaw = control_sp(ControlAxis::YAW);
-	const float collective_pitch = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), _geometry.pitch_curve);
 
 	// actuator mapping
-	actuator_sp(0) = mainMotorEnaged() ? throttle : NAN;
+	actuator_sp(0) = throttle - yaw; // Clockwise
+	actuator_sp(1) = throttle + yaw; // Counter-clockwise
 
-	actuator_sp(1) = control_sp(ControlAxis::YAW) * _geometry.yaw_sign
+	// Saturation check for yaw TODO check saturation
-			 + fabsf(collective_pitch - _geometry.yaw_collective_pitch_offset) * _geometry.yaw_collective_pitch_scale
+	// if (actuator_sp(1) < actuator_min(1)) {
-			 + throttle * _geometry.yaw_throttle_scale;
+	// 	setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos);
 
-	// Saturation check for yaw
+	// } else if (actuator_sp(1) > actuator_max(1)) {
-	if (actuator_sp(1) < actuator_min(1)) {
+	// 	setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg);
-		setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos);
+	// }
-
-	} else if (actuator_sp(1) > actuator_max(1)) {
-		setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg);
-	}
 
 	for (int i = 0; i < _geometry.num_swash_plate_servos; i++) {
 		float roll_coeff = sinf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length;
 		float pitch_coeff = cosf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length;
-		actuator_sp(_first_swash_plate_servo_index + i) = collective_pitch
+		actuator_sp(_first_swash_plate_servo_index + i) =
-				+ control_sp(ControlAxis::PITCH) * pitch_coeff
+			+ control_sp(ControlAxis::PITCH) * pitch_coeff
-				- control_sp(ControlAxis::ROLL) * roll_coeff
+			- control_sp(ControlAxis::ROLL) * roll_coeff
-				+ _geometry.swash_plate_servos[i].trim;
+			+ _geometry.swash_plate_servos[i].trim;
 
 		// Saturation check for roll & pitch
 		if (actuator_sp(_first_swash_plate_servo_index + i) < actuator_min(_first_swash_plate_servo_index + i)) {
@@ -174,18 +144,6 @@ void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector
 	}
 }
 
-bool ActuatorEffectivenessHelicopterCoaxial::mainMotorEnaged()
-{
-	manual_control_switches_s manual_control_switches;
-
-	if (_manual_control_switches_sub.update(&manual_control_switches)) {
-		_main_motor_engaged = manual_control_switches.engage_main_motor_switch == manual_control_switches_s::SWITCH_POS_NONE
-				      || manual_control_switches.engage_main_motor_switch == manual_control_switches_s::SWITCH_POS_ON;
-	}
-
-	return _main_motor_engaged;
-}
-
 float ActuatorEffectivenessHelicopterCoaxial::throttleSpoolupProgress()
 {
 	vehicle_status_s vehicle_status;

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp

@@ -46,7 +46,6 @@ class ActuatorEffectivenessHelicopterCoaxial : public ModuleParams, public Actua
 public:
 
 	static constexpr int NUM_SWASH_PLATE_SERVOS_MAX = 4;
-	static constexpr int NUM_CURVE_POINTS = 5;
 
 	struct SwashPlateGeometry {
 		float angle;
@@ -57,12 +56,6 @@ public:
 	struct Geometry {
 		SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX];
 		int num_swash_plate_servos{0};
-		float throttle_curve[NUM_CURVE_POINTS];
-		float pitch_curve[NUM_CURVE_POINTS];
-		float yaw_collective_pitch_scale;
-		float yaw_collective_pitch_offset;
-		float yaw_throttle_scale;
-		float yaw_sign;
 		float spoolup_time;
 	};
 
@@ -83,7 +76,6 @@ public:
 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 private:
 	float throttleSpoolupProgress();
-	bool mainMotorEnaged();
 
 	void updateParams() override;
 
@@ -107,12 +99,6 @@ private:
 	struct ParamHandles {
 		ParamHandlesSwashPlate swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX];
 		param_t num_swash_plate_servos;
-		param_t throttle_curve[NUM_CURVE_POINTS];
-		param_t pitch_curve[NUM_CURVE_POINTS];
-		param_t yaw_collective_pitch_scale;
-		param_t yaw_collective_pitch_offset;
-		param_t yaw_throttle_scale;
-		param_t yaw_ccw;
 		param_t spoolup_time;
 	};
 	ParamHandles _param_handles{};
@@ -128,5 +114,4 @@ private:
 	uint64_t _armed_time{0};
 
 	uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)};
-	bool _main_motor_engaged{true};
 };