fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
msg/control_allocator_status.msg
View File

@ -1,11 +1,11 @@
uint64 timestamp # time since system start (microseconds)
uint8 torque_setpoint_achieved # Boolean indicating whether the 3D torque setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
bool torque_setpoint_achieved # Boolean indicating whether the 3D torque setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
float32[3] allocated_torque # Torque allocated to actuators. Equal to `vehicle_torque_setpoint_s::xyz` if the setpoint was achieved.
float32[3] unallocated_torque # Unallocated torque. Equal to 0 if the setpoint was achieved.
# Computed as: unallocated_torque = torque_setpoint - allocated_torque
uint8 thrust_setpoint_achieved # Boolean indicating whether the 3D thrust setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
bool thrust_setpoint_achieved # Boolean indicating whether the 3D thrust setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
float32[3] allocated_thrust # Thrust allocated to actuators. Equal to `vehicle_thrust_setpoint_s::xyz` if the setpoint was achieved.
float32[3] unallocated_thrust # Unallocated thrust. Equal to 0 if the setpoint was achieved.
# Computed as: unallocated_thrust = thrust_setpoint - allocated_thrust

26
src/modules/angular_velocity_controller/AngularVelocityController.cpp
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