From d2382184dffa1830790a9b7f4bcce656c06324b1 Mon Sep 17 00:00:00 2001 From: Balduin Date: Wed, 7 Jan 2026 15:56:49 +0100 Subject: [PATCH] ControlAllocator: New unified battery scaling In contrast to the previous versions that applied in the rate control modules, this has some advantages: - Greater encapsulation from rest of autopilot - this implementation applies through the effectiveness matrix, and any torque / rate / thrust commands are now in "physical", non-battery-scaled units, decreasing confusion - Less repetition - only one implementation handles thrust and torque, FW/MC/VTOL all the same It is now controlled by a single parameter, CA_BAT_SCALE_EN, which replaces the previous three MC_BAT_SCALE_EN, FW_BAT_SCALE_EN, and SC_BAT_SCALE_EN. We remove the option of differently setting the MC and FW parameters for VTOL vehicles. --- .../ActuatorEffectiveness.hpp | 1 + .../control_allocator/ControlAllocator.cpp | 32 ++++++++++++++++--- .../ActuatorEffectivenessRotors.cpp | 32 +++++++++++++++++-- .../ActuatorEffectivenessRotors.hpp | 9 ++++-- src/modules/control_allocator/module.yaml | 12 +++++++ 5 files changed, 78 insertions(+), 8 deletions(-) diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp index ffc2315b72..9bcf700207 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp @@ -64,6 +64,7 @@ enum class EffectivenessUpdateReason { NO_EXTERNAL_UPDATE = 0, CONFIGURATION_UPDATE = 1, ///< config changes (parameter) MOTOR_ACTIVATION_UPDATE = 2, ///< motor failure detected or certain redundant motors are switched off to save energy + BATTERY_SCALE_UPDATE = 3, ///< Update with low rate to recalculate battery scale compensation }; class ActuatorEffectiveness diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index f70eb1d8df..e2ca6cd1bc 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -398,7 +398,6 @@ ControlAllocator::Run() _last_run = now; check_for_motor_failures(); - update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE); // Set control setpoint vector(s) @@ -465,9 +464,34 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso { ActuatorEffectiveness::Configuration config{}; - if (reason == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE - && hrt_elapsed_time(&_last_effectiveness_update) < 100_ms) { // rate-limit updates - return; + const hrt_abstime time_since_last_update = hrt_elapsed_time(&_last_effectiveness_update); + + if (reason == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + if (time_since_last_update < 100_ms) { + return; + + } else if (time_since_last_update > 1_s) { + + // Kind of ugly way to force updating it due to battery scale at ~1Hz + + // The only way anything happens with NO_EXTERNAL_UPDATE + // is in ActuatorEffectivenessTiltrotorVTOL, where we + // additionally watch the collective tilt update: + // if (!_collective_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + // return false; + // } + + // Therefore, changing it once per second to + // BATTERY_SCALE_UPDATE should have no effect, as the + // Tiltrotor class will still just update a bit more + // often but never less. + + // It is still needed though as all the other classes + // (non-tilting motors) do nothing when + // NO_EXTERNAL_UPDATE is given. + + reason = EffectivenessUpdateReason::BATTERY_SCALE_UPDATE; + } } if (_actuator_effectiveness->getEffectivenessMatrix(config, reason)) { diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp index b4edd0f2b0..17ba1bd9d4 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp @@ -134,16 +134,36 @@ ActuatorEffectivenessRotors::addActuators(Configuration &configuration) return false; } + if (_param_ca_bat_scale_en.get()) { + battery_status_s battery_status; + + if (_battery_status_sub.update(&battery_status)) { + + if (battery_status.scale > 0.f) { + _battery_scale = battery_status.scale; + + } else { + _battery_scale = 1.f; + } + } + + } else { + _battery_scale = 1.0f; + } + + + int num_actuators = computeEffectivenessMatrix(_geometry, configuration.effectiveness_matrices[configuration.selected_matrix], - configuration.num_actuators_matrix[configuration.selected_matrix]); + configuration.num_actuators_matrix[configuration.selected_matrix], _battery_scale); + configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators); return true; } int ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry, - EffectivenessMatrix &effectiveness, int actuator_start_index) + EffectivenessMatrix &effectiveness, int actuator_start_index, float battery_scale) { int num_actuators = 0; @@ -176,6 +196,14 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry float ct = geometry.rotors[i].thrust_coef; float km = geometry.rotors[i].moment_ratio; + // When battery depletes, scale rises above 1 (initially + // designed as a compensation factor to multiply thrust and + // torque commands with). Rather than that we model it here by a decreasing + // thrust coefficient (thrust = ct * u^2) + if (battery_scale > 1.f) { + ct /= battery_scale; + } + if (geometry.propeller_torque_disabled) { km = 0.f; } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp index c6f0425569..552eeb694d 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -46,6 +46,7 @@ #include #include #include +#include class ActuatorEffectivenessTilts; @@ -96,7 +97,7 @@ public: } static int computeEffectivenessMatrix(const Geometry &geometry, - EffectivenessMatrix &effectiveness, int actuator_start_index = 0); + EffectivenessMatrix &effectiveness, int actuator_start_index = 0, float battery_scale = 1.f); bool addActuators(Configuration &configuration); @@ -135,6 +136,8 @@ private: const AxisConfiguration _axis_config; const bool _tilt_support; ///< if true, tilt servo assignment params are loaded + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + struct ParamHandles { param_t position_x; param_t position_y; @@ -149,8 +152,10 @@ private: ParamHandles _param_handles[NUM_ROTORS_MAX]; Geometry _geometry{}; + float _battery_scale{1.f}; DEFINE_PARAMETERS( - (ParamInt) _param_ca_rotor_count + (ParamInt) _param_ca_rotor_count, + (ParamBool) _param_ca_bat_scale_en ) }; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index dd36a2d070..ef7c7dabfb 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -48,6 +48,18 @@ parameters: 2: Automatic default: 2 + CA_BAT_SCALE_EN: + description: + short: Enable battery scale compensation + long: | + Battery scale compensation modifies the rotor effectiveness, + modelling the weaker response with depleting battery. Results + in consistent thrust and torque response despite depleting + battery. This replaces MC_BAT_SCALE_EN, FW_BAT_SCALE_EN, and + SC_BAT_SCALE_EN. + type: boolean + default: false + # Motor parameters CA_R_REV: description: