forked from Archive/PX4-Autopilot
Add heli servo trim
This commit is contained in:
parent
667e99be81
commit
aa8d594e9b
|
@ -46,6 +46,8 @@ ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *p
|
|||
_param_handles.swash_plate_servos[i].angle = param_find(buffer);
|
||||
snprintf(buffer, sizeof(buffer), "CA_SP0_ARM_L%u", i);
|
||||
_param_handles.swash_plate_servos[i].arm_length = param_find(buffer);
|
||||
snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRIM", i);
|
||||
_param_handles.swash_plate_servos[i].trim = param_find(buffer);
|
||||
}
|
||||
|
||||
_param_handles.num_swash_plate_servos = param_find("CA_SP0_COUNT");
|
||||
|
@ -84,6 +86,7 @@ void ActuatorEffectivenessHelicopter::updateParams()
|
|||
param_get(_param_handles.swash_plate_servos[i].angle, &angle_deg);
|
||||
_geometry.swash_plate_servos[i].angle = math::radians(angle_deg);
|
||||
param_get(_param_handles.swash_plate_servos[i].arm_length, &_geometry.swash_plate_servos[i].arm_length);
|
||||
param_get(_param_handles.swash_plate_servos[i].trim, &_geometry.swash_plate_servos[i].trim);
|
||||
}
|
||||
|
||||
for (int i = 0; i < NUM_CURVE_POINTS; ++i) {
|
||||
|
@ -118,6 +121,7 @@ ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configura
|
|||
|
||||
for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) {
|
||||
configuration.addActuator(ActuatorType::SERVOS, Vector3f{}, Vector3f{});
|
||||
configuration.trim[configuration.selected_matrix](i) = _geometry.swash_plate_servos[i].trim;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
@ -143,7 +147,8 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
|
|||
+ control_sp(ControlAxis::PITCH) * cosf(_geometry.swash_plate_servos[i].angle) *
|
||||
_geometry.swash_plate_servos[i].arm_length
|
||||
- control_sp(ControlAxis::ROLL) * sinf(_geometry.swash_plate_servos[i].angle) *
|
||||
_geometry.swash_plate_servos[i].arm_length;
|
||||
_geometry.swash_plate_servos[i].arm_length
|
||||
+ _geometry.swash_plate_servos[i].trim;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -51,6 +51,7 @@ public:
|
|||
struct SwashPlateGeometry {
|
||||
float angle;
|
||||
float arm_length;
|
||||
float trim;
|
||||
};
|
||||
|
||||
struct Geometry {
|
||||
|
@ -85,6 +86,7 @@ private:
|
|||
struct ParamHandlesSwashPlate {
|
||||
param_t angle;
|
||||
param_t arm_length;
|
||||
param_t trim;
|
||||
};
|
||||
struct ParamHandles {
|
||||
ParamHandlesSwashPlate swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX];
|
||||
|
|
|
@ -930,6 +930,8 @@ mixer:
|
|||
label: 'Angle'
|
||||
- name: 'CA_SP0_ARM_L${i}'
|
||||
label: 'Arm Length (relative)'
|
||||
- name: 'CA_SV_CS${i}_TRIM'
|
||||
label: 'Trim'
|
||||
parameters:
|
||||
- label: 'Yaw compensation scale based on collective pitch'
|
||||
name: CA_HELI_YAW_CP_S
|
||||
|
|
Loading…
Reference in New Issue