Trim params for Tilts

Conform to Tilt naming scheme Update description for trim tilts update tilt parameter name to conform
henrykotze · Nov 14, 2023 · fa76f04 · fa76f04
1 parent 1592aed
commit fa76f04
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 1 deletion.
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp
@@ -52,6 +52,8 @@ ActuatorEffectivenessTilts::ActuatorEffectivenessTilts(ModuleParams *parent)
 		_param_handles[i].max_angle = param_find(buffer);
 		snprintf(buffer, sizeof(buffer), "CA_SV_TL%u_TD", i);
 		_param_handles[i].tilt_direction = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_SV_TL%u_TRIM", i);
+		_param_handles[i].trim = param_find(buffer);
 	}
 
 	_count_handle = param_find("CA_SV_TL_COUNT");
@@ -76,6 +78,7 @@ void ActuatorEffectivenessTilts::updateParams()
 		param_get(_param_handles[i].tilt_direction, (int32_t *)&_params[i].tilt_direction);
 		param_get(_param_handles[i].min_angle, &_params[i].min_angle);
 		param_get(_param_handles[i].max_angle, &_params[i].max_angle);
+		param_get(_param_handles[i].trim, &_params[i].trim);
 
 		_params[i].min_angle = math::radians(_params[i].min_angle);
 		_params[i].max_angle = math::radians(_params[i].max_angle);
@@ -87,7 +90,11 @@ void ActuatorEffectivenessTilts::updateParams()
 bool ActuatorEffectivenessTilts::addActuators(Configuration &configuration)
 {
 	for (int i = 0; i < _count; i++) {
-		configuration.addActuator(ActuatorType::SERVOS, _torque[i], Vector3f{});
+		int actuator_idx = configuration.addActuator(ActuatorType::SERVOS, _torque[i], Vector3f{});
+
+		if (actuator_idx >= 0) {
+			configuration.trim[configuration.selected_matrix](actuator_idx) = _params[i].trim;
+		}
 	}
 
 	return true;

diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp
@@ -62,6 +62,7 @@ class ActuatorEffectivenessTilts : public ModuleParams, public ActuatorEffective
 		float min_angle;
 		float max_angle;
 		TiltDirection tilt_direction;
+		float trim;
 	};
 
 	ActuatorEffectivenessTilts(ModuleParams *parent);
@@ -89,6 +90,7 @@ class ActuatorEffectivenessTilts : public ModuleParams, public ActuatorEffective
 		param_t min_angle;
 		param_t max_angle;
 		param_t tilt_direction;
+		param_t trim;
 	};
 
 	ParamHandles _param_handles[MAX_COUNT];

diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml
@@ -411,6 +411,19 @@ parameters:
             instance_start: 0
             default: 0
 
+
+        CA_SV_TL${i}_TRIM:
+            description:
+                short: Tilt Servo ${i} trim
+                long: Can be used to add an offset to the servo control.
+            type: float
+            decimal: 2
+            min: -1.0
+            max: 1.0
+            num_instances: *max_num_tilts
+            instance_start: 0
+            default: 0.0
+
         # helicopter
         CA_SP0_COUNT:
             description:
@@ -1015,6 +1028,8 @@ mixer:
                             label: 'Tilt Direction'
                           - name: 'CA_SV_TL${i}_CT'
                             label: 'Use for Control'
+                          - name: 'CA_SV_TL${i}_TRIM'
+                            label: 'Trim'
 
             9: # Custom
                 actuators: