Browse Source

fix control_allocator_status: torque_setpoint_achieved and thrust_setpoint_achieved are bool

release/1.12
Beat Küng 5 years ago committed by Daniel Agar
parent
commit
25f3fe8456
  1. 4
      msg/control_allocator_status.msg
  2. 6
      src/modules/angular_velocity_controller/AngularVelocityController.cpp

4
msg/control_allocator_status.msg

@ -1,11 +1,11 @@ @@ -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

@ -185,14 +185,13 @@ AngularVelocityController::Run() @@ -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() @@ -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);

Loading…
Cancel
Save