Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 04:06:33 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

refactor(control_allocation): Clarify and enforce that motors are in matrix 0

Browse Source
This commit is contained in:
Balduin
2026-03-09 15:08:52 +01:00
committed by Balduin
parent b6164107d1
commit 029edb50b3
2 changed files with 8 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp
+5
View File
@@ -50,6 +50,11 @@ int ActuatorEffectiveness::Configuration::addActuator(ActuatorType type, const m
return -1; return -1;
} }
if (type == ActuatorType::MOTORS && selected_matrix != 0) {
PX4_ERR("Trying to add motors to matrix %d (add to matrix 0)", selected_matrix);
return -1;
}
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::ROLL, actuator_idx) = torque(0); effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::ROLL, actuator_idx) = torque(0);
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::PITCH, actuator_idx) = torque(1); effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::PITCH, actuator_idx) = torque(1);
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::YAW, actuator_idx) = torque(2); effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::YAW, actuator_idx) = torque(2);
src/modules/control_allocator/ControlAllocator.cpp
+3 -2
View File
@@ -438,6 +438,7 @@ ControlAllocator::Run()
_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax()); _control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax());
if (i == 0) { if (i == 0) {
// The motors are always in allocation 0
handle_stopped_motors(now); handle_stopped_motors(now);
} }
@@ -611,7 +612,7 @@ ControlAllocator::handle_stopped_motors(const hrt_abstime now)
| _motor_stop_mask; | _motor_stop_mask;
// Handle stopped motors by setting NaN // Handle stopped motors by setting NaN
const unsigned int allocation_index = 0; const unsigned int allocation_index = 0; // Motors always in allocation 0
_control_allocation[allocation_index]->applyNanToActuators(stopped_motors); _control_allocation[allocation_index]->applyNanToActuators(stopped_motors);
// Apply ice shedding, which applies _only_ to stopped motors // Apply ice shedding, which applies _only_ to stopped motors
@@ -619,7 +620,7 @@ ControlAllocator::handle_stopped_motors(const hrt_abstime now)
const float ice_shedding_output = get_ice_shedding_output(now); const float ice_shedding_output = get_ice_shedding_output(now);
if (ice_shedding_output > FLT_EPSILON && !any_stopped_motor_failed) { if (ice_shedding_output > FLT_EPSILON && !any_stopped_motor_failed) {
for (int motors_idx = 0; motors_idx < _num_actuators[0] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) { for (int motors_idx = 0; motors_idx < _num_actuators[allocation_index] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) {
if (stopped_motors & 1u << motors_idx) { if (stopped_motors & 1u << motors_idx) {
_control_allocation[allocation_index]->_actuator_sp(motors_idx) = ice_shedding_output; _control_allocation[allocation_index]->_actuator_sp(motors_idx) = ice_shedding_output;
} }