From 4c80adfaf157ab4e8ebccbf3ba581b77b571e55c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 3 Dec 2021 11:52:01 +0100 Subject: [PATCH] control_allocator: implement trim + slew rate limits configuration --- msg/CMakeLists.txt | 1 + msg/actuator_servos_trim.msg | 5 ++ src/lib/mixer_module/actuator_test.cpp | 11 ++++ src/lib/mixer_module/actuator_test.hpp | 2 + src/lib/mixer_module/motor_params.c | 2 +- .../ActuatorEffectiveness.hpp | 2 + .../ActuatorEffectivenessControlSurfaces.cpp | 10 ++- .../ActuatorEffectivenessControlSurfaces.hpp | 2 + .../ControlAllocation/ControlAllocation.cpp | 30 ++++++--- .../ControlAllocation/ControlAllocation.hpp | 28 ++++++--- .../ControlAllocationPseudoInverse.cpp | 6 +- .../ControlAllocationPseudoInverse.hpp | 2 +- .../ControlAllocationPseudoInverseTest.cpp | 3 +- ...ontrolAllocationSequentialDesaturation.cpp | 2 + .../control_allocator/ControlAllocator.cpp | 61 +++++++++++++++++- .../control_allocator/ControlAllocator.hpp | 19 ++++++ src/modules/control_allocator/module.yaml | 62 +++++++++++++++++++ 17 files changed, 224 insertions(+), 24 deletions(-) create mode 100644 msg/actuator_servos_trim.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 6ab5a030b4..c8343d0a56 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -44,6 +44,7 @@ set(msg_files actuator_motors.msg actuator_outputs.msg actuator_servos.msg + actuator_servos_trim.msg actuator_test.msg adc_report.msg airspeed.msg diff --git a/msg/actuator_servos_trim.msg b/msg/actuator_servos_trim.msg new file mode 100644 index 0000000000..30953e7ae5 --- /dev/null +++ b/msg/actuator_servos_trim.msg @@ -0,0 +1,5 @@ +# Servo trims, added as offset to servo outputs +uint64 timestamp # time since system start (microseconds) + +uint8 NUM_CONTROLS = 8 +float32[8] trim # range: [-1, 1] diff --git a/src/lib/mixer_module/actuator_test.cpp b/src/lib/mixer_module/actuator_test.cpp index 9c08d639bd..b022327429 100644 --- a/src/lib/mixer_module/actuator_test.cpp +++ b/src/lib/mixer_module/actuator_test.cpp @@ -81,6 +81,17 @@ void ActuatorTest::update(int num_outputs, float thrust_curve) FunctionMotors::updateValues(motors.reversible_flags >> motor_idx, thrust_curve, &value, 1); } + // handle servos: add trim + if (actuator_test.function >= (int)OutputFunction::Servo1 && actuator_test.function <= (int)OutputFunction::ServoMax) { + actuator_servos_trim_s trim{}; + _actuator_servos_trim_sub.copy(&trim); + int idx = actuator_test.function - (int)OutputFunction::Servo1; + + if (idx < actuator_servos_trim_s::NUM_CONTROLS) { + value += trim.trim[idx]; + } + } + _current_outputs[i] = value; } else { diff --git a/src/lib/mixer_module/actuator_test.hpp b/src/lib/mixer_module/actuator_test.hpp index 71b73e0ad8..26fcd80e21 100644 --- a/src/lib/mixer_module/actuator_test.hpp +++ b/src/lib/mixer_module/actuator_test.hpp @@ -38,6 +38,7 @@ #include #include #include +#include #include static_assert(actuator_test_s::FUNCTION_MOTOR1 == (int)OutputFunction::Motor1, "define mismatch"); @@ -66,6 +67,7 @@ private: uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)}; uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + uORB::Subscription _actuator_servos_trim_sub{ORB_ID(actuator_servos_trim)}; bool _in_test_mode{false}; hrt_abstime _next_timeout{0}; diff --git a/src/lib/mixer_module/motor_params.c b/src/lib/mixer_module/motor_params.c index d60cfaed2b..319f470842 100644 --- a/src/lib/mixer_module/motor_params.c +++ b/src/lib/mixer_module/motor_params.c @@ -44,7 +44,7 @@ * * Minimum time allowed for the motor input signal to pass through * a range of 1000 PWM units. A value x means that the motor signal - * can only go from 1000 to 2000 PWM in maximum x seconds. + * can only go from 1000 to 2000 PWM in minimum x seconds. * * Zero means that slew rate limiting is disabled. * diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 94fc7cac23..272eabf7e7 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -98,6 +98,8 @@ public: int num_actuators_matrix[MAX_NUM_MATRICES]; ///< current amount, and next actuator index to fill in to effectiveness_matrices ActuatorVector trim[MAX_NUM_MATRICES]; + ActuatorVector linearization_point[MAX_NUM_MATRICES]; + int selected_matrix; uint8_t matrix_selection_indexes[NUM_ACTUATORS * MAX_NUM_MATRICES]; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp index 66163891b3..7b849178b9 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp @@ -50,6 +50,8 @@ ActuatorEffectivenessControlSurfaces::ActuatorEffectivenessControlSurfaces(Modul _param_handles[i].torque[1] = param_find(buffer); snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRQ_Y", i); _param_handles[i].torque[2] = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRIM", i); + _param_handles[i].trim = param_find(buffer); } _count_handle = param_find("CA_SV_CS_COUNT"); @@ -78,6 +80,8 @@ void ActuatorEffectivenessControlSurfaces::updateParams() param_get(_param_handles[i].torque[n], &torque(n)); } + param_get(_param_handles[i].trim, &_params[i].trim); + // TODO: enforce limits? switch (_params[i].type) { case Type::Elevator: @@ -126,7 +130,11 @@ bool ActuatorEffectivenessControlSurfaces::getEffectivenessMatrix(Configuration } for (int i = 0; i < _count; i++) { - configuration.addActuator(ActuatorType::SERVOS, _params[i].torque, Vector3f{}); + int actuator_idx = configuration.addActuator(ActuatorType::SERVOS, _params[i].torque, Vector3f{}); + + if (actuator_idx >= 0) { + configuration.trim[configuration.selected_matrix](actuator_idx) = _params[i].trim; + } } return true; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp index 58875aad72..cad017ad87 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp @@ -63,6 +63,7 @@ public: Type type; matrix::Vector3f torque; + float trim; }; ActuatorEffectivenessControlSurfaces(ModuleParams *parent); @@ -86,6 +87,7 @@ private: param_t type; param_t torque[3]; + param_t trim; }; ParamHandles _param_handles[MAX_COUNT]; param_t _count_handle; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp index 134e8891bf..4c9cd8161f 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp @@ -51,18 +51,15 @@ ControlAllocation::ControlAllocation() void ControlAllocation::setEffectivenessMatrix( const matrix::Matrix &effectiveness, - const matrix::Vector &actuator_trim, int num_actuators) + const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators) { _effectiveness = effectiveness; - _actuator_trim = actuator_trim; + ActuatorVector linearization_point_clipped = linearization_point; + clipActuatorSetpoint(linearization_point_clipped); + _actuator_trim = actuator_trim + linearization_point_clipped; clipActuatorSetpoint(_actuator_trim); - _control_trim = _effectiveness * _actuator_trim; _num_actuators = num_actuators; - - // make sure unused actuators are initialized to trim - for (int i = num_actuators; i < NUM_ACTUATORS; ++i) { - _actuator_sp(i) = _actuator_trim(i); - } + _control_trim = _effectiveness * linearization_point_clipped; } void @@ -109,3 +106,20 @@ const return actuator_normalized; } + +void ControlAllocation::applySlewRateLimit(float dt) +{ + for (int i = 0; i < _num_actuators; i++) { + if (_actuator_slew_rate_limit(i) > FLT_EPSILON) { + float delta_sp_max = dt * (_actuator_max(i) - _actuator_min(i)) / _actuator_slew_rate_limit(i); + float delta_sp = _actuator_sp(i) - _prev_actuator_sp(i); + + if (delta_sp > delta_sp_max) { + _actuator_sp(i) = _prev_actuator_sp(i) + delta_sp_max; + + } else if (delta_sp < -delta_sp_max) { + _actuator_sp(i) = _prev_actuator_sp(i) - delta_sp_max; + } + } + } +} diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index 9b22194803..7fc7d7b8f2 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -104,7 +104,7 @@ public: * @param B Effectiveness matrix */ virtual void setEffectivenessMatrix(const matrix::Matrix &effectiveness, - const matrix::Vector &actuator_trim, int num_actuators); + const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators); /** * Get the allocated actuator vector @@ -181,6 +181,14 @@ public: */ void setActuatorSetpoint(const matrix::Vector &actuator_sp); + void setSlewRateLimit(const matrix::Vector &slew_rate_limit) + { _actuator_slew_rate_limit = slew_rate_limit; } + + /** + * Apply slew rate to current actuator setpoint + */ + void applySlewRateLimit(float dt); + /** * Clip the actuator setpoint between minimum and maximum values. * @@ -211,13 +219,15 @@ public: protected: friend class ControlAllocator; // for _actuator_sp - matrix::Matrix _effectiveness; //< Effectiveness matrix - matrix::Vector _control_allocation_scale; //< Scaling applied during allocation - matrix::Vector _actuator_trim; //< Neutral actuator values - matrix::Vector _actuator_min; //< Minimum actuator values - matrix::Vector _actuator_max; //< Maximum actuator values - matrix::Vector _actuator_sp; //< Actuator setpoint - matrix::Vector _control_sp; //< Control setpoint - matrix::Vector _control_trim; //< Control at trim actuator values + matrix::Matrix _effectiveness; ///< Effectiveness matrix + matrix::Vector _control_allocation_scale; ///< Scaling applied during allocation + matrix::Vector _actuator_trim; ///< Neutral actuator values + matrix::Vector _actuator_min; ///< Minimum actuator values + matrix::Vector _actuator_max; ///< Maximum actuator values + matrix::Vector _actuator_slew_rate_limit; ///< Slew rate limit + matrix::Vector _prev_actuator_sp; ///< Previous actuator setpoint + matrix::Vector _actuator_sp; ///< Actuator setpoint + matrix::Vector _control_sp; ///< Control setpoint + matrix::Vector _control_trim; ///< Control at trim actuator values int _num_actuators{0}; }; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index e9d07eee5e..9bf46eeee8 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -44,9 +44,9 @@ void ControlAllocationPseudoInverse::setEffectivenessMatrix( const matrix::Matrix &effectiveness, - const matrix::Vector &actuator_trim, int num_actuators) + const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators) { - ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, num_actuators); + ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, num_actuators); _mix_update_needed = true; } @@ -120,6 +120,8 @@ ControlAllocationPseudoInverse::allocate() //Compute new gains if needed updatePseudoInverse(); + _prev_actuator_sp = _actuator_sp; + // Allocate _actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim); } diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp index d934e17a80..6412b1cb11 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp @@ -55,7 +55,7 @@ public: void allocate() override; void setEffectivenessMatrix(const matrix::Matrix &effectiveness, - const matrix::Vector &actuator_trim, int num_actuators) override; + const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators) override; protected: matrix::Matrix _mix; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp index 41dbf9de42..830d1b19a1 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp @@ -54,9 +54,10 @@ TEST(ControlAllocationTest, AllZeroCase) matrix::Matrix effectiveness; matrix::Vector actuator_sp; matrix::Vector actuator_trim; + matrix::Vector linearization_point; matrix::Vector actuator_sp_expected; - method.setEffectivenessMatrix(effectiveness, actuator_trim, 16); + method.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16); method.setControlSetpoint(control_sp); method.allocate(); method.clipActuatorSetpoint(); diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp index 336f0303c2..6bfe493d5a 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -47,6 +47,8 @@ ControlAllocationSequentialDesaturation::allocate() //Compute new gains if needed updatePseudoInverse(); + _prev_actuator_sp = _actuator_sp; + switch (_param_mc_airmode.get()) { case 1: mixAirmodeRP(); diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 607338217d..f73a6ecf26 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -54,6 +54,18 @@ ControlAllocator::ControlAllocator() : WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { + for (int i = 0; i < MAX_NUM_MOTORS; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_R%u_SLEW", i); + _param_handles.slew_rate_motors[i] = param_find(buffer); + } + + for (int i = 0; i < MAX_NUM_SERVOS; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_SV%u_SLEW", i); + _param_handles.slew_rate_servos[i] = param_find(buffer); + } + parameters_updated(); } @@ -87,6 +99,18 @@ ControlAllocator::init() void ControlAllocator::parameters_updated() { + _has_slew_rate = false; + + for (int i = 0; i < MAX_NUM_MOTORS; ++i) { + param_get(_param_handles.slew_rate_motors[i], &_params.slew_rate_motors[i]); + _has_slew_rate |= _params.slew_rate_motors[i] > FLT_EPSILON; + } + + for (int i = 0; i < MAX_NUM_SERVOS; ++i) { + param_get(_param_handles.slew_rate_servos[i], &_params.slew_rate_servos[i]); + _has_slew_rate |= _params.slew_rate_servos[i] > FLT_EPSILON; + } + // Allocation method & effectiveness source // Do this first: in case a new method is loaded, it will be configured below bool updated = update_effectiveness_source(); @@ -343,6 +367,11 @@ ControlAllocator::Run() // Do allocation _control_allocation[i]->allocate(); _actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp); + + if (_has_slew_rate) { + _control_allocation[i]->applySlewRateLimit(dt); + } + _control_allocation[i]->clipActuatorSetpoint(); } @@ -378,14 +407,19 @@ ControlAllocator::update_effectiveness_matrix_if_needed(bool force) // Get the minimum and maximum depending on type and configuration ActuatorEffectiveness::ActuatorVector minimum[ActuatorEffectiveness::MAX_NUM_MATRICES]; ActuatorEffectiveness::ActuatorVector maximum[ActuatorEffectiveness::MAX_NUM_MATRICES]; + ActuatorEffectiveness::ActuatorVector slew_rate[ActuatorEffectiveness::MAX_NUM_MATRICES]; int actuator_idx = 0; int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; + actuator_servos_trim_s trims{}; + static_assert(actuator_servos_trim_s::NUM_CONTROLS == actuator_servos_s::NUM_CONTROLS, "size mismatch"); + for (int actuator_type = 0; actuator_type < (int)ActuatorType::COUNT; ++actuator_type) { _num_actuators[actuator_type] = config.num_actuators[actuator_type]; for (int actuator_type_idx = 0; actuator_type_idx < config.num_actuators[actuator_type]; ++actuator_type_idx) { if (actuator_idx >= NUM_ACTUATORS) { + _num_actuators[actuator_type] = 0; PX4_ERR("Too many actuators"); break; } @@ -393,6 +427,12 @@ ControlAllocator::update_effectiveness_matrix_if_needed(bool force) int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; if ((ActuatorType)actuator_type == ActuatorType::MOTORS) { + if (actuator_type_idx >= MAX_NUM_MOTORS) { + PX4_ERR("Too many motors"); + _num_actuators[actuator_type] = 0; + break; + } + if (_param_r_rev.get() & (1u << actuator_type_idx)) { minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = -1.f; @@ -400,11 +440,25 @@ ControlAllocator::update_effectiveness_matrix_if_needed(bool force) minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = 0.f; } + slew_rate[selected_matrix](actuator_idx_matrix[selected_matrix]) = _params.slew_rate_motors[actuator_type_idx]; + + } else if ((ActuatorType)actuator_type == ActuatorType::SERVOS) { + if (actuator_type_idx >= MAX_NUM_SERVOS) { + PX4_ERR("Too many servos"); + _num_actuators[actuator_type] = 0; + break; + } + + minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = -1.f; + slew_rate[selected_matrix](actuator_idx_matrix[selected_matrix]) = _params.slew_rate_servos[actuator_type_idx]; + trims.trim[actuator_type_idx] = config.trim[selected_matrix](actuator_idx_matrix[selected_matrix]); + } else { minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = -1.f; } maximum[selected_matrix](actuator_idx_matrix[selected_matrix]) = 1.f; + ++actuator_idx_matrix[selected_matrix]; ++actuator_idx; } @@ -413,11 +467,16 @@ ControlAllocator::update_effectiveness_matrix_if_needed(bool force) for (int i = 0; i < _num_control_allocation; ++i) { _control_allocation[i]->setActuatorMin(minimum[i]); _control_allocation[i]->setActuatorMax(maximum[i]); + _control_allocation[i]->setSlewRateLimit(slew_rate[i]); // Assign control effectiveness matrix int total_num_actuators = config.num_actuators_matrix[i]; - _control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i], total_num_actuators); + _control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i], + config.linearization_point[i], total_num_actuators); } + + trims.timestamp = hrt_absolute_time(); + _actuator_servos_trim_pub.publish(trims); } } diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 040b7b8cbd..bce976616c 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -64,6 +64,7 @@ #include #include #include +#include #include #include #include @@ -76,6 +77,9 @@ public: static constexpr int NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS; static constexpr int NUM_AXES = ControlAllocation::NUM_AXES; + static constexpr int MAX_NUM_MOTORS = actuator_motors_s::NUM_CONTROLS; + static constexpr int MAX_NUM_SERVOS = actuator_servos_s::NUM_CONTROLS; + using ActuatorVector = ActuatorEffectiveness::ActuatorVector; ControlAllocator(); @@ -100,6 +104,16 @@ public: private: + struct ParamHandles { + param_t slew_rate_motors[MAX_NUM_MOTORS]; + param_t slew_rate_servos[MAX_NUM_SERVOS]; + }; + + struct Params { + float slew_rate_motors[MAX_NUM_MOTORS]; + float slew_rate_servos[MAX_NUM_SERVOS]; + }; + /** * initialize some vectors/matrices from parameters */ @@ -147,6 +161,7 @@ private: uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; + uORB::Publication _actuator_servos_trim_pub{ORB_ID(actuator_servos_trim)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -161,6 +176,10 @@ private: hrt_abstime _timestamp_sample{0}; hrt_abstime _last_status_pub{0}; + ParamHandles _param_handles{}; + Params _params{}; + bool _has_slew_rate{false}; + DEFINE_PARAMETERS( (ParamInt) _param_ca_airframe, (ParamInt) _param_ca_method, diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 95057773e4..7a75c25316 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -53,6 +53,42 @@ parameters: 6: Motor 7 7: Motor 8 default: 0 + CA_R${i}_SLEW: + description: + short: Motor ${i} slew rate limit + long: | + Minimum time allowed for the motor input signal to pass through + the full output range. A value x means that the motor signal + can only go from 0 to 1 in minimum x seconds (in case of + reversible motors, the range is -1 to 1). + + Zero means that slew rate limiting is disabled. + type: float + decimal: 2 + increment: 0.01 + num_instances: *max_num_mc_motors + min: 0 + max: 10 + default: 0.0 + + # Servo params + CA_SV${i}_SLEW: + description: + short: Servo ${i} slew rate limit + long: | + Minimum time allowed for the servo input signal to pass through + the full output range. A value x means that the servo signal + can only go from -1 to 1 in minimum x seconds. + + Zero means that slew rate limiting is disabled. + type: float + decimal: 2 + increment: 0.05 + num_instances: *max_num_servos + min: 0 + max: 10 + default: 0.0 + # (MC) Rotors CA_ROTOR_COUNT: @@ -246,6 +282,17 @@ parameters: num_instances: *max_num_servos instance_start: 0 default: 0.0 + CA_SV_CS${i}_TRIM: + description: + short: Control Surface ${i} trim + long: Can be used to add an offset to the servo control. + type: float + decimal: 2 + min: -1.0 + max: 1.0 + num_instances: *max_num_servos + instance_start: 0 + default: 0.0 # Tilts CA_SV_TL_COUNT: @@ -341,12 +388,21 @@ mixer: show_as: 'bitset' index_offset: 0 advanced: true + - name: 'CA_R${i}_SLEW' + label: 'Slew Rate' + index_offset: 0 + advanced: true servo: functions: 'Servo' actuator_testing_values: min: -1 max: 1 default: 0 + per_item_parameters: + - name: 'CA_SV${i}_SLEW' + label: 'Slew Rate' + index_offset: 0 + advanced: true DEFAULT: actuator_testing_values: min: -1 @@ -427,6 +483,8 @@ mixer: - name: 'CA_SV_CS${i}_TRQ_Y' label: 'Yaw Scale' identifier: 'servo-torque-yaw' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' 2: # Tiltrotor VTOL actuators: @@ -458,6 +516,8 @@ mixer: - name: 'CA_SV_CS${i}_TRQ_Y' label: 'Yaw Torque' identifier: 'servo-torque-yaw' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' - actuator_type: 'servo' group_label: 'Tilt Servos' count: 'CA_SV_TL_COUNT' @@ -519,5 +579,7 @@ mixer: - name: 'CA_SV_CS${i}_TRQ_Y' label: 'Yaw Torque' identifier: 'servo-torque-yaw' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim'