From cf40d95ef0dd4e5fc288056229063febc7cc6c68 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 28 Mar 2023 14:33:48 +0200 Subject: [PATCH] HelicopterCoaxial: adjust for coaxial allocation --- ...ActuatorEffectivenessHelicopterCoaxial.cpp | 74 ++++--------------- ...ActuatorEffectivenessHelicopterCoaxial.hpp | 15 ---- 2 files changed, 16 insertions(+), 73 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp index e36709adf4..4ea7c956d3 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp @@ -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; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp index 57f36b2db9..d5316bf498 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp @@ -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}; };