fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 16:57:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix control_allocator: check if 2. thrust and torque sp topic is valid

Browse Source 
Otherwise the 2. setpoint is equal to the first one. This is only an issue
if the 2. instance isn't advertised.
This commit is contained in:
Beat Küng
2022-09-21 09:28:08 +02:00
committed by Silvan Fuhrer
parent a53e1277dd
commit d948fcd72b
1 changed files with 11 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocator.cpp
+11 -8
View File
@@ -400,14 +400,17 @@ ControlAllocator::Run()
c[0](5) = _thrust_sp(2);
if (_num_control_allocation > 1) {
_vehicle_torque_setpoint1_sub.copy(&vehicle_torque_setpoint);
_vehicle_thrust_setpoint1_sub.copy(&vehicle_thrust_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];
c[1](3) = vehicle_thrust_setpoint.xyz[0];
c[1](4) = vehicle_thrust_setpoint.xyz[1];
c[1](5) = vehicle_thrust_setpoint.xyz[2];
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];
}
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];
}
}
for (int i = 0; i < _num_control_allocation; ++i) {