fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 22:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

control_allocator: avoid vector copies and sqrt()

Browse Source
This commit is contained in:
Beat Küng
2020-08-13 08:20:51 +02:00
committed by Daniel Agar
parent 0e66b0876b
commit 164511a7b5
1 changed files with 6 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocator.cpp
+6 -6
View File
@@ -393,7 +393,7 @@ ControlAllocator::publish_control_allocator_status()
control_allocator_status.timestamp = hrt_absolute_time();
// Allocated control
matrix::Vector<float, NUM_AXES> allocated_control = _control_allocation->getAllocatedControl();
const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation->getAllocatedControl();
control_allocator_status.allocated_torque[0] = allocated_control(0);
control_allocator_status.allocated_torque[1] = allocated_control(1);
control_allocator_status.allocated_torque[2] = allocated_control(2);
@@ -412,14 +412,14 @@ ControlAllocator::publish_control_allocator_status()
// Allocation success flags
control_allocator_status.torque_setpoint_achieved = (Vector3f(unallocated_control(0), unallocated_control(1),
unallocated_control(2)).norm() < FLT_EPSILON);
unallocated_control(2)).norm_squared() < 1e-6f);
control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4),
unallocated_control(5)).norm() < FLT_EPSILON);
unallocated_control(5)).norm_squared() < 1e-6f);
// Actuator saturation
matrix::Vector<float, NUM_ACTUATORS> actuator_sp = _control_allocation->getActuatorSetpoint();
matrix::Vector<float, NUM_ACTUATORS> actuator_min = _control_allocation->getActuatorMin();
matrix::Vector<float, NUM_ACTUATORS> actuator_max = _control_allocation->getActuatorMax();
const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation->getActuatorSetpoint();
const matrix::Vector<float, NUM_ACTUATORS> &actuator_min = _control_allocation->getActuatorMin();
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max = _control_allocation->getActuatorMax();
for (size_t i = 0; i < NUM_ACTUATORS; i++) {
if (actuator_sp(i) > (actuator_max(i) - FLT_EPSILON)) {