From 933d2d263d1e32f08517dbde8e5a782b6990162d Mon Sep 17 00:00:00 2001 From: Balduin Date: Thu, 15 Jan 2026 14:34:08 +0100 Subject: [PATCH] ControlAllocator: new unified battery scaling Works by scaling the _mix matrix in the pinv conntrol allocator. This is architecturally a bit ugly, but I think is a reasonable solution considering the alternatives and their drawbacks: Current version (in rate control modules): + simple - Relies on FW/VTOL proxy to determine which directions of thrust/torque need battery scaling - introduce battery scaled thrust / torque commands that are not physical thrust and torque but Also modifying the torque / thrust setpoint like currently but inside of the allocator (this one could actually reasonably work as well, consider) + minimal functional change w.r.t. before, addresses the issue of confusing scaled and unscaled torque/thrust setpoints - FW/VTOL distinction gets hairier. - Currently we have separate scaling implementations for MC (-> scale all thrust and torque commands, reasonably assuming that all rotors are driven by the same battery) and FW (-> scale only forward thrust). If we move it into the allocator, - In the allocator we get either one torque sp (MC or FW, from the rate controllers) or two (VTOL, from VTOL attitude controller). In the VTOL case, we have 0 = MC and 1 = FW. To replicate the previous logic, we have to separate these setpoints again into clear MC and FW components. Instead modifying the effectiveness matrix through ActuatorEffectivenessRotors + VERY clean and physically consistent, the scale enters by modifying the thrust coefficient, 1:1 modelling the decreased effectiveness of the rotor in both thrust and torque, in one single place - Does not work because the normalizeControlAllocationMatrix reverts the scaling effect that entered through the effectivenss matrix. Getting it to work would require reworking this scaling from the ground up or removing it entirely Modifying only the motor outputs after the allocator + again very simple, does not care about FW/VTOL, we just do it for all the rotors - Introduces wrong actuator limits: Allocator assumes that rotors saturate based on a model unaware of battery scale, but because after the allocator we scale (increase) the commands, the actual saturation happens earlier. --- .../ActuatorEffectiveness.hpp | 10 +++++++++ .../control_allocation/ControlAllocation.hpp | 7 ++++++ .../ControlAllocationPseudoInverse.cpp | 3 +++ .../ControlAllocationPseudoInverse.hpp | 16 ++++++++++++++ .../control_allocator/ControlAllocator.cpp | 22 +++++++++++++++++++ .../control_allocator/ControlAllocator.hpp | 8 ++++++- .../ActuatorEffectivenessMCTilt.hpp | 5 +++++ .../ActuatorEffectivenessMultirotor.hpp | 5 +++++ .../ActuatorEffectivenessRotors.hpp | 5 +++++ .../ActuatorEffectivenessSpacecraft.hpp | 7 ++++++ .../ActuatorEffectivenessStandardVTOL.hpp | 6 +++++ .../ActuatorEffectivenessTailsitterVTOL.hpp | 6 +++++ .../ActuatorEffectivenessTiltrotorVTOL.hpp | 6 +++++ .../ActuatorEffectivenessUUV.hpp | 5 +++++ src/modules/control_allocator/module.yaml | 11 ++++++++++ 15 files changed, 121 insertions(+), 1 deletion(-) diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp index ffc2315b72..e73572d199 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp @@ -159,6 +159,16 @@ public: } } + /** + * Query if the mixing matrix requires battery compensation + */ + virtual void getNeedsBatteryScaling(bool needs_scaling[MAX_NUM_MATRICES]) const + { + for (int i = 0; i < MAX_NUM_MATRICES; ++i) { + needs_scaling[i] = false; + } + } + /** * Get the control effectiveness matrix if updated * diff --git a/src/lib/control_allocation/control_allocation/ControlAllocation.hpp b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp index a106120026..d7e0986ccd 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocation.hpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp @@ -70,6 +70,10 @@ #pragma once #include +#include +#include +#include +#include #include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" @@ -228,6 +232,8 @@ public: void setNormalizeRPY(bool normalize_rpy) { _normalize_rpy = normalize_rpy; } + virtual void setBatteryScaling(float new_battery_scaling) { _battery_scaling = new_battery_scaling; } + protected: friend class ControlAllocator; // for _actuator_sp @@ -244,4 +250,5 @@ protected: int _num_actuators{0}; bool _normalize_rpy{false}; ///< if true, normalize roll, pitch and yaw columns bool _had_actuator_failure{false}; + float _battery_scaling{1.0f}; }; diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp index 0dc512b4ed..1d5d0c51a3 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp @@ -73,6 +73,9 @@ ControlAllocationPseudoInverse::updatePseudoInverse() normalizeControlAllocationMatrix(); } + // empty battery = scaling > 1 = larger mix matrix = smaller effectiveness + _mix *= _battery_scaling; + _mix_update_needed = false; } } diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp index d4527cc1f4..153990a6a7 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp @@ -57,6 +57,22 @@ public: void setEffectivenessMatrix(const matrix::Matrix &effectiveness, const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators, bool update_normalization_scale) override; + + void setBatteryScaling(float new_battery_scaling) override + { + // Only update the battery scaling if changed significantly, to + // avoid doing costly pseudoinverse updates at high rate + // - could change the mix update itself to again only update the pinv itself when it used to, and to + // - might want to limit the rate at which we do this anyway in case battery scale behaves erratically to deterministically limit cpu + const bool update_significant = fabsf(new_battery_scaling - _battery_scaling) > 0.005f; + const bool updating_to_one = fabsf(new_battery_scaling - 1.0f) < FLT_EPSILON && fabsf(_battery_scaling - 1.0f) < FLT_EPSILON; + + if (update_significant || updating_to_one) { + _battery_scaling = new_battery_scaling; + _mix_update_needed = true; + } + } + void setMetricAllocation(bool metric_allocation) { _metric_allocation = metric_allocation; } protected: diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index f70eb1d8df..979c57a5d3 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -166,6 +166,8 @@ ControlAllocator::update_allocation_method(bool force) bool normalize_rpy[ActuatorEffectiveness::MAX_NUM_MATRICES]; _actuator_effectiveness->getNormalizeRPY(normalize_rpy); + _actuator_effectiveness->getNeedsBatteryScaling(_allocation_needs_battery_scaling); + for (int i = 0; i < _num_control_allocation; ++i) { AllocationMethod method = configured_method; @@ -424,8 +426,27 @@ ControlAllocator::Run() } } + float battery_scaling = 1.0f; + + if (_param_ca_bat_scale_en.get()) { + battery_status_s battery_status; + + if (_battery_status_sub.copy(&battery_status)) { + if (battery_status.connected && battery_status.scale > 0.f) { + battery_scaling = battery_status.scale; + } + } + } + for (int i = 0; i < _num_control_allocation; ++i) { + if (_allocation_needs_battery_scaling[i]) { + _control_allocation[i]->setBatteryScaling(battery_scaling); + + } else { + _control_allocation[i]->setBatteryScaling(1.0f); + } + _control_allocation[i]->setControlSetpoint(c[i]); // Do allocation @@ -471,6 +492,7 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso } if (_actuator_effectiveness->getEffectivenessMatrix(config, reason)) { + PX4_INFO("eff update"); _last_effectiveness_update = hrt_absolute_time(); memcpy(_control_allocation_selection_indexes, config.matrix_selection_indexes, diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 376171ea23..58e0aba0e9 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -193,6 +194,7 @@ private: uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)}; + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; matrix::Vector3f _torque_sp; matrix::Vector3f _thrust_sp; @@ -214,11 +216,15 @@ private: Params _params{}; bool _has_slew_rate{false}; + // For each allocation matrix, determine whether it needs battery scaling + bool _allocation_needs_battery_scaling[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; + DEFINE_PARAMETERS( (ParamInt) _param_ca_airframe, (ParamInt) _param_ca_method, (ParamInt) _param_ca_failure_mode, - (ParamInt) _param_r_rev + (ParamInt) _param_r_rev, + (ParamBool) _param_ca_bat_scale_en ) }; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp index 2fd5923824..26533c27af 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp @@ -55,6 +55,11 @@ public: normalize[0] = true; } + void getNeedsBatteryScaling(bool needs_scaling[MAX_NUM_MATRICES]) const override + { + needs_scaling[0] = true; + } + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp index e5b7f3dcf8..58ee12e845 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp @@ -54,6 +54,11 @@ public: normalize[0] = true; } + void getNeedsBatteryScaling(bool needs_scaling[MAX_NUM_MATRICES]) const override + { + needs_scaling[0] = true; + } + const char *name() const override { return "Multirotor"; } protected: diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp index c6f0425569..4b85d11eb7 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -95,6 +95,11 @@ public: normalize[0] = true; } + void getNeedsBatteryScaling(bool needs_scaling[MAX_NUM_MATRICES]) const override + { + needs_scaling[0] = true; + } + static int computeEffectivenessMatrix(const Geometry &geometry, EffectivenessMatrix &effectiveness, int actuator_start_index = 0); diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp index e2fb530f73..06bfe9d928 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp @@ -57,6 +57,13 @@ public: normalize[0] = true; } + void getNeedsBatteryScaling(bool needs_scaling[MAX_NUM_MATRICES]) const override + { + // Set to true if the spacecraft is using electric actuators + // that degrade as battery depletes + needs_scaling[0] = false; + } + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, const matrix::Vector &actuator_max) override; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 4879c6b022..694bb2c227 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -73,6 +73,12 @@ public: normalize[1] = false; } + void getNeedsBatteryScaling(bool needs_scaling[MAX_NUM_MATRICES]) const override + { + needs_scaling[0] = true; + needs_scaling[1] = false; + } + void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override; void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index c7a588954f..fd81ffff93 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -70,6 +70,12 @@ public: normalize[1] = false; } + void getNeedsBatteryScaling(bool needs_scaling[MAX_NUM_MATRICES]) const override + { + needs_scaling[0] = true; + needs_scaling[1] = true; // Even in fixed wing we want to scale all thrusts and torques by battery because everything is a rotor + } + void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override; void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index db7b37304b..98d08b710d 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -76,6 +76,12 @@ public: normalize[1] = false; } + void getNeedsBatteryScaling(bool needs_scaling[MAX_NUM_MATRICES]) const override + { + needs_scaling[0] = true; + needs_scaling[1] = false; + } + void setFlightPhase(const FlightPhase &flight_phase) override; void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp index 78e52c34aa..9bf7687fba 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp @@ -54,6 +54,11 @@ public: normalize[0] = true; } + void getNeedsBatteryScaling(bool needs_scaling[MAX_NUM_MATRICES]) const override + { + needs_scaling[0] = true; + } + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 23405466a6..c5f4b34af0 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -48,6 +48,17 @@ parameters: 2: Automatic default: 2 + CA_BAT_SCALE_EN: + description: + short: Enable battery scale compensation + long: | + Battery scale compensation results in consistent thrust and + torque response despite depleting battery. It applies to all + rotors -- if they are gas powered disable it. 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: