dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

HelicopterCoaxial: adjust for coaxial allocation

This commit is contained in:
Matthias Grob 2023-03-28 14:33:48 +02:00 committed by Beat Küng
parent b3b373e074
commit cf40d95ef0
2 changed files with 16 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp
View File

@ -51,19 +51,6 @@ ActuatorEffectivenessHelicopterCoaxial::ActuatorEffectivenessHelicopterCoaxial(M
}
_param_handles.num_swash_plate_servos = param_find("CA_SP0_COUNT");
for (int i = 0; i < NUM_CURVE_POINTS; ++i) {
char buffer[17];
snprintf(buffer, sizeof(buffer), "CA_HELI_THR_C%u", i);
_param_handles.throttle_curve[i] = param_find(buffer);
snprintf(buffer, sizeof(buffer), "CA_HELI_PITCH_C%u", i);
_param_handles.pitch_curve[i] = param_find(buffer);
}
_param_handles.yaw_collective_pitch_scale = param_find("CA_HELI_YAW_CP_S");
_param_handles.yaw_collective_pitch_offset = param_find("CA_HELI_YAW_CP_O");
_param_handles.yaw_throttle_scale = param_find("CA_HELI_YAW_TH_S");
_param_handles.yaw_ccw = param_find("CA_HELI_YAW_CCW");
_param_handles.spoolup_time = param_find("COM_SPOOLUP_TIME");
updateParams();
@ -90,18 +77,7 @@ void ActuatorEffectivenessHelicopterCoaxial::updateParams()
param_get(_param_handles.swash_plate_servos[i].trim, &_geometry.swash_plate_servos[i].trim);
}
for (int i = 0; i < NUM_CURVE_POINTS; ++i) {
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);
param_get(_param_handles.yaw_collective_pitch_offset, &_geometry.yaw_collective_pitch_offset);
param_get(_param_handles.yaw_throttle_scale, &_geometry.yaw_throttle_scale);
param_get(_param_handles.spoolup_time, &_geometry.spoolup_time);
int32_t yaw_ccw = 0;
param_get(_param_handles.yaw_ccw, &yaw_ccw);
_geometry.yaw_sign = (yaw_ccw == 1) ? -1.f : 1.f;
}
bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuration &configuration,
@ -112,10 +88,8 @@ bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuratio
}
// As the allocation is non-linear, we use updateSetpoint() instead of the matrix
configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{});
// Tail (yaw) motor
configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{});
configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); // Clockwise rotor
configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); // Counter-clockwise rotor
// N swash plate servos
_first_swash_plate_servo_index = configuration.num_actuators_matrix[0];
@ -135,32 +109,28 @@ void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector
_saturation_flags = {};
// throttle/collective pitch curve
const float throttle = math::interpolateN(-control_sp(ControlAxis::THRUST_Z),
_geometry.throttle_curve) * throttleSpoolupProgress();
const float collective_pitch = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), _geometry.pitch_curve);
const float throttle = -control_sp(ControlAxis::THRUST_Z) * throttleSpoolupProgress();
const float yaw = control_sp(ControlAxis::YAW);
// actuator mapping
actuator_sp(0) = mainMotorEnaged() ? throttle : NAN;
actuator_sp(0) = throttle - yaw; // Clockwise
actuator_sp(1) = throttle + yaw; // Counter-clockwise
actuator_sp(1) = control_sp(ControlAxis::YAW) * _geometry.yaw_sign
+ fabsf(collective_pitch - _geometry.yaw_collective_pitch_offset) * _geometry.yaw_collective_pitch_scale
+ throttle * _geometry.yaw_throttle_scale;
// Saturation check for yaw TODO check saturation
// if (actuator_sp(1) < actuator_min(1)) {
// setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos);
// Saturation check for yaw
if (actuator_sp(1) < actuator_min(1)) {
setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos);
} else if (actuator_sp(1) > actuator_max(1)) {
setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg);
}
// } else if (actuator_sp(1) > actuator_max(1)) {
// setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg);
// }
for (int i = 0; i < _geometry.num_swash_plate_servos; i++) {
float roll_coeff = sinf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length;
float pitch_coeff = cosf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length;
actuator_sp(_first_swash_plate_servo_index + i) = collective_pitch
+ control_sp(ControlAxis::PITCH) * pitch_coeff
- control_sp(ControlAxis::ROLL) * roll_coeff
+ _geometry.swash_plate_servos[i].trim;
actuator_sp(_first_swash_plate_servo_index + i) =
+ control_sp(ControlAxis::PITCH) * pitch_coeff
- control_sp(ControlAxis::ROLL) * roll_coeff
+ _geometry.swash_plate_servos[i].trim;
// Saturation check for roll & pitch
if (actuator_sp(_first_swash_plate_servo_index + i) < actuator_min(_first_swash_plate_servo_index + i)) {
@ -174,18 +144,6 @@ void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector
}
}
bool ActuatorEffectivenessHelicopterCoaxial::mainMotorEnaged()
{
manual_control_switches_s manual_control_switches;
if (_manual_control_switches_sub.update(&manual_control_switches)) {
_main_motor_engaged = manual_control_switches.engage_main_motor_switch == manual_control_switches_s::SWITCH_POS_NONE
|| manual_control_switches.engage_main_motor_switch == manual_control_switches_s::SWITCH_POS_ON;
}
return _main_motor_engaged;
}
float ActuatorEffectivenessHelicopterCoaxial::throttleSpoolupProgress()
{
vehicle_status_s vehicle_status;

15
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp
View File

@ -46,7 +46,6 @@ class ActuatorEffectivenessHelicopterCoaxial : public ModuleParams, public Actua
public:
static constexpr int NUM_SWASH_PLATE_SERVOS_MAX = 4;
static constexpr int NUM_CURVE_POINTS = 5;
struct SwashPlateGeometry {
float angle;
@ -57,12 +56,6 @@ public:
struct Geometry {
SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX];
int num_swash_plate_servos{0};
float throttle_curve[NUM_CURVE_POINTS];
float pitch_curve[NUM_CURVE_POINTS];
float yaw_collective_pitch_scale;
float yaw_collective_pitch_offset;
float yaw_throttle_scale;
float yaw_sign;
float spoolup_time;
};
@ -83,7 +76,6 @@ public:
void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
private:
float throttleSpoolupProgress();
bool mainMotorEnaged();
void updateParams() override;
@ -107,12 +99,6 @@ private:
struct ParamHandles {
ParamHandlesSwashPlate swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX];
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;
param_t yaw_collective_pitch_offset;
param_t yaw_throttle_scale;
param_t yaw_ccw;
param_t spoolup_time;
};
ParamHandles _param_handles{};
@ -128,5 +114,4 @@ private:
uint64_t _armed_time{0};
uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)};
bool _main_motor_engaged{true};
};