control_allocator: implement trim + slew rate limits configuration

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -98,6 +98,8 @@ public:
 		int num_actuators_matrix[MAX_NUM_MATRICES]; ///< current amount, and next actuator index to fill in to effectiveness_matrices
 		ActuatorVector trim[MAX_NUM_MATRICES];
 
+		ActuatorVector linearization_point[MAX_NUM_MATRICES];
+
 		int selected_matrix;
 
 		uint8_t matrix_selection_indexes[NUM_ACTUATORS * MAX_NUM_MATRICES];

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp

@@ -50,6 +50,8 @@ ActuatorEffectivenessControlSurfaces::ActuatorEffectivenessControlSurfaces(Modul
 		_param_handles[i].torque[1] = param_find(buffer);
 		snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRQ_Y", i);
 		_param_handles[i].torque[2] = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRIM", i);
+		_param_handles[i].trim = param_find(buffer);
 	}
 
 	_count_handle = param_find("CA_SV_CS_COUNT");
@@ -78,6 +80,8 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
 			param_get(_param_handles[i].torque[n], &torque(n));
 		}
 
+		param_get(_param_handles[i].trim, &_params[i].trim);
+
 		// TODO: enforce limits?
 		switch (_params[i].type) {
 		case Type::Elevator:
@@ -126,7 +130,11 @@ bool ActuatorEffectivenessControlSurfaces::getEffectivenessMatrix(Configuration
 	}
 
 	for (int i = 0; i < _count; i++) {
-		configuration.addActuator(ActuatorType::SERVOS, _params[i].torque, Vector3f{});
+		int actuator_idx = configuration.addActuator(ActuatorType::SERVOS, _params[i].torque, Vector3f{});
+
+		if (actuator_idx >= 0) {
+			configuration.trim[configuration.selected_matrix](actuator_idx) = _params[i].trim;
+		}
 	}
 
 	return true;

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp

@@ -63,6 +63,7 @@ public:
 		Type type;
 
 		matrix::Vector3f torque;
+		float trim;
 	};
 
 	ActuatorEffectivenessControlSurfaces(ModuleParams *parent);
@@ -86,6 +87,7 @@ private:
 		param_t type;
 
 		param_t torque[3];
+		param_t trim;
 	};
 	ParamHandles _param_handles[MAX_COUNT];
 	param_t _count_handle;