diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 28baf752ee..14957197bb 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/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) { diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 376171ea23..9fb021aeb0 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 @@ -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_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) _param_ca_airframe, (ParamInt) _param_ca_method, (ParamInt) _param_ca_failure_mode, - (ParamInt) _param_r_rev + (ParamInt) _param_r_rev, + (ParamInt) _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); + } + } + + }; 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: