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.

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
 	 *

src/lib/control_allocation/control_allocation/ControlAllocation.hpp

@@ -70,6 +70,10 @@
 #pragma once
 
 #include <matrix/matrix/math.hpp>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 
 #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};
 };

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;
 	}
 }

src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp

@@ -57,6 +57,22 @@ public:
 	void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &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:

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,

src/modules/control_allocator/ControlAllocator.hpp

@@ -72,6 +72,7 @@
 #include <uORB/topics/actuator_motors.h>
 #include <uORB/topics/actuator_servos.h>
 #include <uORB/topics/actuator_servos_trim.h>
+#include <uORB/topics/battery_status.h>
 #include <uORB/topics/control_allocator_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -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<px4::params::CA_AIRFRAME>) _param_ca_airframe,
 		(ParamInt<px4::params::CA_METHOD>) _param_ca_method,
 		(ParamInt<px4::params::CA_FAILURE_MODE>) _param_ca_failure_mode,
-		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+		(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
+		(ParamBool<px4::params::CA_BAT_SCALE_EN>) _param_ca_bat_scale_en
 	)
 
 };

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<float, NUM_AXES> &control_sp, int matrix_index, ActuatorVector &actuator_sp,
 			    const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override;
 

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:

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);
 

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<float, NUM_AXES> &control_sp, int matrix_index,
 			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
 			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;

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<float, NUM_AXES> &control_sp, int matrix_index, ActuatorVector &actuator_sp,

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<float, NUM_AXES> &control_sp, int matrix_index, ActuatorVector &actuator_sp,

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;

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<float, NUM_AXES> &control_sp, int matrix_index, ActuatorVector &actuator_sp,
 			    const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override;
 

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: