yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00: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

4
msg/control_allocator_status.msg
View File

@@ -1,11 +1,11 @@
uint64 timestamp # time since system start (microseconds) 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] 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. float32[3] unallocated_torque # Unallocated torque. Equal to 0 if the setpoint was achieved.
# Computed as: unallocated_torque = torque_setpoint - allocated_torque # 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] 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. float32[3] unallocated_thrust # Unallocated thrust. Equal to 0 if the setpoint was achieved.
# Computed as: unallocated_thrust = thrust_setpoint - allocated_thrust # 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 // 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)) { if (_control_allocator_status_sub.update(&control_allocator_status)) {
Vector<bool, 3> saturation_positive; Vector<bool, 3> saturation_positive;
Vector<bool, 3> saturation_negative; Vector<bool, 3> saturation_negative;
if (not control_allocator_status.torque_setpoint_achieved) { if (!control_allocator_status.torque_setpoint_achieved) {
for (size_t i = 0; i < 3; i++) { for (size_t i = 0; i < 3; i++) {
if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
saturation_positive(i) = true; saturation_positive(i) = true;
} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
saturation_negative(i) = true; saturation_negative(i) = true;
}
} }
} }
_control.setSaturationStatus(saturation_positive, saturation_negative);
} }
_control.setSaturationStatus(saturation_positive, saturation_negative);
} }
// run rate controller // run rate controller