diff --git a/msg/control_allocator_status.msg b/msg/control_allocator_status.msg index 0870ff9d48..2e90dbd454 100644 --- a/msg/control_allocator_status.msg +++ b/msg/control_allocator_status.msg @@ -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 diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.cpp b/src/modules/angular_velocity_controller/AngularVelocityController.cpp index c59f57dc5b..1d8dcba7c9 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityController.cpp +++ b/src/modules/angular_velocity_controller/AngularVelocityController.cpp @@ -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 saturation_positive; - Vector saturation_negative; + if (_control_allocator_status_sub.update(&control_allocator_status)) { + Vector saturation_positive; + Vector 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