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: