ControlAllocator: new unified battery scaling

src/modules/control_allocator/ControlAllocator.cpp

@@ -49,6 +49,7 @@
 using namespace matrix;
 using namespace time_literals;
 
+
 ControlAllocator::ControlAllocator() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
@@ -131,6 +132,7 @@ ControlAllocator::parameters_updated()
 	}
 
 	update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::CONFIGURATION_UPDATE);
+	update_battery_scaling_modes();
 }
 
 void
@@ -333,12 +335,22 @@ ControlAllocator::Run()
 		return;
 	}
 
+	if (_battery_status_sub.updated()) {
+		battery_status_s battery_status;
+
+		if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) {
+			_battery_scale = battery_status.scale;
+		}
+	}
+
+
 	{
 		vehicle_status_s vehicle_status;
 
 		if (_vehicle_status_sub.update(&vehicle_status)) {
 
 			_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
+			_is_vtol = vehicle_status.is_vtol;
 
 			ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT};
 
@@ -362,6 +374,8 @@ ControlAllocator::Run()
 
 			// Forward to effectiveness source
 			_actuator_effectiveness->setFlightPhase(flight_phase);
+
+			update_battery_scaling_modes();
 		}
 	}
 
@@ -383,7 +397,8 @@ ControlAllocator::Run()
 
 	// Run allocator on torque changes
 	if (_vehicle_torque_setpoint_sub.update(&vehicle_torque_setpoint)) {
-		_torque_sp = matrix::Vector3f(vehicle_torque_setpoint.xyz);
+
+		_torque_sp = battery_scale_torque_setpoint(vehicle_torque_setpoint, _battery_scaling_modes[0], _battery_scale);
 
 		do_update = true;
 		_timestamp_sample = vehicle_torque_setpoint.timestamp_sample;
@@ -391,7 +406,7 @@ ControlAllocator::Run()
 	}
 
 	if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)) {
-		_thrust_sp = matrix::Vector3f(vehicle_thrust_setpoint.xyz);
+		_thrust_sp = battery_scale_thrust_setpoint(vehicle_thrust_setpoint, _battery_scaling_modes[0], _battery_scale);
 	}
 
 	if (do_update) {
@@ -412,15 +427,17 @@ ControlAllocator::Run()
 
 		if (_num_control_allocation > 1) {
 			if (_vehicle_torque_setpoint1_sub.copy(&vehicle_torque_setpoint)) {
-				c[1](0) = vehicle_torque_setpoint.xyz[0];
-				c[1](1) = vehicle_torque_setpoint.xyz[1];
-				c[1](2) = vehicle_torque_setpoint.xyz[2];
+				const Vector3f torque_sp = battery_scale_torque_setpoint(vehicle_torque_setpoint, _battery_scaling_modes[1], _battery_scale);
+				c[1](0) = torque_sp(0);
+				c[1](1) = torque_sp(1);
+				c[1](2) = torque_sp(2);
 			}
 
 			if (_vehicle_thrust_setpoint1_sub.copy(&vehicle_thrust_setpoint)) {
-				c[1](3) = vehicle_thrust_setpoint.xyz[0];
-				c[1](4) = vehicle_thrust_setpoint.xyz[1];
-				c[1](5) = vehicle_thrust_setpoint.xyz[2];
+				const Vector3f thrust_sp = battery_scale_torque_setpoint(vehicle_torque_setpoint, _battery_scaling_modes[1], _battery_scale);
+				c[1](3) = thrust_sp(0);
+				c[1](4) = thrust_sp(1);
+				c[1](5) = thrust_sp(2);
 			}
 		}
 
@@ -460,6 +477,31 @@ ControlAllocator::Run()
 	perf_end(_loop_perf);
 }
 
+void
+ControlAllocator::update_battery_scaling_modes()
+{
+
+	const ActuatorEffectiveness::FlightPhase flight_phase = _actuator_effectiveness->getFlightPhase();
+
+	if (_param_ca_bat_scale_en.get()) {
+		if (_is_vtol) {
+			_battery_scaling_modes[0] = BatteryScalingMode::ALL;
+			_battery_scaling_modes[1] = BatteryScalingMode::FORWARD_THRUST_ONLY;
+
+		} else if (flight_phase == ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT) {
+			_battery_scaling_modes[0] = BatteryScalingMode::ALL;
+
+		} else {
+			// Remaining option is flight_phase == forward flight.
+			_battery_scaling_modes[0] = BatteryScalingMode::FORWARD_THRUST_ONLY;
+		}
+
+	} else {
+		_battery_scaling_modes[0] = BatteryScalingMode::NONE;
+		_battery_scaling_modes[1] = BatteryScalingMode::NONE;
+	}
+}
+
 void
 ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason)
 {

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>
@@ -139,6 +140,8 @@ private:
 
 	void publish_actuator_controls();
 
+	void update_battery_scaling_modes();
+
 	AllocationMethod _allocation_method_id{AllocationMethod::NONE};
 	ControlAllocation *_control_allocation[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; 	///< class for control allocation calculations
 	int _num_control_allocation{0};
@@ -168,6 +171,13 @@ private:
 		REMOVE_FIRST_FAILING_MOTOR = 1,
 	};
 
+	enum class BatteryScalingMode {
+		NONE = 0,
+		ALL,
+		FORWARD_THRUST_ONLY
+	};
+
+
 	EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE};
 	ActuatorEffectiveness *_actuator_effectiveness{nullptr}; 	///< class providing actuator effectiveness
 
@@ -181,6 +191,8 @@ private:
 	uORB::Subscription _vehicle_torque_setpoint1_sub{ORB_ID(vehicle_torque_setpoint), 1};  /**< vehicle torque setpoint subscription (2. instance) */
 	uORB::Subscription _vehicle_thrust_setpoint1_sub{ORB_ID(vehicle_thrust_setpoint), 1};	 /**< vehicle thrust setpoint subscription (2. instance) */
 
+	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
+
 	// Outputs
 	uORB::PublicationMulti<control_allocator_status_s> _control_allocator_status_pub[2] {ORB_ID(control_allocator_status), ORB_ID(control_allocator_status)};
 
@@ -206,6 +218,7 @@ private:
 	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
 
 	bool _armed{false};
+	bool _is_vtol{false};
 	hrt_abstime _last_run{0};
 	hrt_abstime _timestamp_sample{0};
 	hrt_abstime _last_status_pub{0};
@@ -214,11 +227,49 @@ private:
 	Params _params{};
 	bool _has_slew_rate{false};
 
+	float _battery_scale{1.0f};
+	BatteryScalingMode _battery_scaling_modes[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,
+		(ParamInt<px4::params::CA_BAT_SCALE_EN>) _param_ca_bat_scale_en
 	)
 
+	static matrix::Vector3f
+	battery_scale_torque_setpoint(const vehicle_torque_setpoint_s &torque_sp, const BatteryScalingMode mode,
+				      const float battery_scale)
+	{
+		switch (mode) {
+		case BatteryScalingMode::ALL:
+			return matrix::Vector3f(torque_sp.xyz) * battery_scale;
+
+		case BatteryScalingMode::FORWARD_THRUST_ONLY:
+		case BatteryScalingMode::NONE:
+		default:
+			return matrix::Vector3f(torque_sp.xyz);
+		}
+	}
+
+	static matrix::Vector3f
+	battery_scale_thrust_setpoint(const vehicle_thrust_setpoint_s &thrust_sp, const BatteryScalingMode mode,
+				      const float battery_scale)
+	{
+
+		switch (mode) {
+		case BatteryScalingMode::ALL:
+			return matrix::Vector3f(thrust_sp.xyz) * battery_scale;
+
+		case BatteryScalingMode::FORWARD_THRUST_ONLY:
+			return matrix::Vector3f(thrust_sp.xyz[0] * battery_scale, thrust_sp.xyz[1], thrust_sp.xyz[2]);
+
+		case BatteryScalingMode::NONE:
+		default:
+			return matrix::Vector3f(thrust_sp.xyz);
+		}
+	}
+
+
 };

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: