fix control_allocator_status: torque_setpoint_achieved and thrust_setpoint_achieved are bool

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

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