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)
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

6
src/modules/angular_velocity_controller/AngularVelocityController.cpp
View File

@@ -185,14 +185,13 @@ AngularVelocityController::Run()
}
// update saturation status from mixer feedback
if (_control_allocator_status_sub.updated()) {
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_negative;
if (not control_allocator_status.torque_setpoint_achieved) {
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;
@@ -205,7 +204,6 @@ AngularVelocityController::Run()
_control.setSaturationStatus(saturation_positive, saturation_negative);
}
}
// run rate controller
_control.update(angular_velocity, _angular_velocity_sp, _angular_acceleration, dt, _maybe_landed || _landed);