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.

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

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

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

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp

@@ -46,6 +46,7 @@
 #include <px4_platform_common/module_params.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/battery_status.h>
 
 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<px4::params::CA_ROTOR_COUNT>) _param_ca_rotor_count
+		(ParamInt<px4::params::CA_ROTOR_COUNT>) _param_ca_rotor_count,
+		(ParamBool<px4::params::CA_BAT_SCALE_EN>) _param_ca_bat_scale_en
 	)
 };

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: