fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 23:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

helicopter: use tail motor & add CA_HELI_YAW_CP_S

Browse Source
This commit is contained in:
Beat Küng
2022-09-07 08:00:25 +02:00
parent 5ece24cdc4
commit e346190e63
3 changed files with 25 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
+8 -2
View File
@@ -57,6 +57,8 @@ ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *p
_param_handles.pitch_curve[i] = param_find(buffer);
}
_param_handles.yaw_collective_pitch_scale = param_find("CA_HELI_YAW_CP_S");
updateParams();
}
@@ -84,6 +86,8 @@ void ActuatorEffectivenessHelicopter::updateParams()
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);
}
bool
@@ -97,8 +101,8 @@ ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configura
// As the allocation is non-linear, we use updateSetpoint() instead of the matrix
configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{});
// Tail (yaw) servo
configuration.addActuator(ActuatorType::SERVOS, Vector3f{0.f, 0.f, 1.f}, Vector3f{});
// Tail (yaw) motor
configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{});
// N swash plate servos
_first_swash_plate_servo_index = configuration.num_actuators_matrix[0];
@@ -134,6 +138,8 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
actuator_sp(0) = throttle;
actuator_sp(1) = control_sp(ControlAxis::YAW) + collective_pitch * _geometry.yaw_collective_pitch_scale;
for (int i = 0; i < _geometry.num_swash_plate_servos; i++) {
actuator_sp(_first_swash_plate_servo_index + i) = collective_pitch
+ cosf(_geometry.swash_plate_servos[i].angle) * pitch_cmd * _geometry.swash_plate_servos[i].arm_length
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
+2
View File
@@ -54,6 +54,7 @@ public:
int num_swash_plate_servos{0};
float throttle_curve[NUM_CURVE_POINTS];
float pitch_curve[NUM_CURVE_POINTS];
float yaw_collective_pitch_scale;
};
ActuatorEffectivenessHelicopter(ModuleParams *parent);
@@ -80,6 +81,7 @@ private:
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;
};
ParamHandles _param_handles{};