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

fix control_allocator_status: torque_setpoint_achieved and thrust_setpoint_achieved are bool

Browse Source
This commit is contained in:
Beat Küng
2020-08-13 08:20:23 +02:00
committed by Daniel Agar
parent 343cf5603e
commit 25f3fe8456
2 changed files with 14 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/angular_velocity_controller/AngularVelocityController.cpp
+12 -14
View File
@@ -185,26 +185,24 @@ AngularVelocityController::Run()
}
// update saturation status from mixer feedback
if (_control_allocator_status_sub.updated()) {
control_allocator_status_s control_allocator_status;
control_allocator_status_s control_allocator_status;
if (_control_allocator_status_sub.copy(&control_allocator_status)) {
Vector<bool, 3> saturation_positive;
Vector<bool, 3> saturation_negative;
if (_control_allocator_status_sub.update(&control_allocator_status)) {
Vector<bool, 3> saturation_positive;
Vector<bool, 3> saturation_negative;
if (not control_allocator_status.torque_setpoint_achieved) {
for (size_t i = 0; i < 3; i++) {
if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
saturation_positive(i) = true;
if (!control_allocator_status.torque_setpoint_achieved) {
for (size_t i = 0; i < 3; i++) {
if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
saturation_positive(i) = true;
} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
saturation_negative(i) = true;
}
} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
saturation_negative(i) = true;
}
}
_control.setSaturationStatus(saturation_positive, saturation_negative);
}
_control.setSaturationStatus(saturation_positive, saturation_negative);
}
// run rate controller