Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

Helicopter: refactor complicated throttle curve logic

Browse Source
This commit is contained in:
Matthias Grob
2022-08-11 14:38:30 +02:00
committed by Beat Küng
parent 2edb35b1b5
commit 7bf62373ae
1 changed files with 8 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
+8 -21
View File
@@ -119,24 +119,10 @@ ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configura
void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
int matrix_index, ActuatorVector &actuator_sp) int matrix_index, ActuatorVector &actuator_sp)
{ {
// Find index to use for curves // throttle/collective pitch curve
float thrust_cmd = -control_sp(ControlAxis::THRUST_Z); const float throttle = math::interpolateN(-control_sp(ControlAxis::THRUST_Z),
float num_intervals = NUM_CURVE_POINTS - 1; _geometry.throttle_curve) * throttleSpoolupProgress();
// We access idx + 1 below, so max legal index is (size - 2) const float collective_pitch = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), _geometry.pitch_curve);
int idx = math::constrain((int)(thrust_cmd * num_intervals), 0, NUM_CURVE_POINTS - 2);
// Local throttle curve gradient and offset
float tg = (_geometry.throttle_curve[idx + 1] - _geometry.throttle_curve[idx]) * num_intervals;
float to = (_geometry.throttle_curve[idx]) - (tg * idx / num_intervals);
float throttle = math::constrain(tg * thrust_cmd + to, 0.0f, 1.0f);
// Local pitch curve gradient and offset
float pg = (_geometry.pitch_curve[idx + 1] - _geometry.pitch_curve[idx]) * num_intervals;
float po = (_geometry.pitch_curve[idx]) - (pg * idx / num_intervals);
float collective_pitch = math::constrain((pg * thrust_cmd + po), -0.5f, 0.5f);
float roll_cmd = control_sp(ControlAxis::ROLL);
float pitch_cmd = control_sp(ControlAxis::PITCH);
actuator_sp(0) = throttle; actuator_sp(0) = throttle;
actuator_sp(1) = control_sp(ControlAxis::YAW) actuator_sp(1) = control_sp(ControlAxis::YAW)
@@ -145,8 +131,9 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
for (int i = 0; i < _geometry.num_swash_plate_servos; i++) { for (int i = 0; i < _geometry.num_swash_plate_servos; i++) {
actuator_sp(_first_swash_plate_servo_index + i) = collective_pitch 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 + control_sp(ControlAxis::PITCH) * cosf(_geometry.swash_plate_servos[i].angle) *
- sinf(_geometry.swash_plate_servos[i].angle) * roll_cmd * _geometry.swash_plate_servos[i].arm_length; _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;
} }
} }