fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 19:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

MC mixer: replace multirotor_motor_limits by control_allocator_status

Browse Source 
CA: fix saturation computation
Since the CA matrix is normalized, the same scale applied to be used when using the effectiveness matrix

MCRateControl: use control_allocator_status to get saturation info
This commit is contained in:
bresch
2021-11-03 16:56:33 +01:00
committed by Daniel Agar
parent dd83ef1813
commit d47f9f155a
17 changed files with 115 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mixer_module/mixer_module.cpp
+44 -4
View File
@@ -861,11 +861,51 @@ MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs)
saturation_status.value = _mixers->get_saturation_status();
if (saturation_status.flags.valid) {
multirotor_motor_limits_s motor_limits;
motor_limits.timestamp = actuator_outputs.timestamp;
motor_limits.saturation_status = saturation_status.value;
control_allocator_status_s sat{};
sat.timestamp = hrt_absolute_time();
sat.torque_setpoint_achieved = true;
sat.thrust_setpoint_achieved = true;
_to_mixer_status.publish(motor_limits);
// Note: the values '-1', '1' and '0' are just to indicate a negative,
// positive or no saturation to the rate controller. The actual magnitude
// is not used.
if (saturation_status.flags.roll_pos) {
sat.unallocated_torque[0] = 1.f;
sat.torque_setpoint_achieved = false;
} else if (saturation_status.flags.roll_neg) {
sat.unallocated_torque[0] = -1.f;
sat.torque_setpoint_achieved = false;
}
if (saturation_status.flags.pitch_pos) {
sat.unallocated_torque[1] = 1.f;
sat.torque_setpoint_achieved = false;
} else if (saturation_status.flags.pitch_neg) {
sat.unallocated_torque[1] = -1.f;
sat.torque_setpoint_achieved = false;
}
if (saturation_status.flags.yaw_pos) {
sat.unallocated_torque[2] = 1.f;
sat.torque_setpoint_achieved = false;
} else if (saturation_status.flags.yaw_neg) {
sat.unallocated_torque[2] = -1.f;
sat.torque_setpoint_achieved = false;
}
if (saturation_status.flags.thrust_pos) {
sat.unallocated_thrust[2] = 1.f;
sat.thrust_setpoint_achieved = false;
} else if (saturation_status.flags.thrust_neg) {
sat.unallocated_thrust[2] = -1.f;
sat.thrust_setpoint_achieved = false;
}
_control_allocator_status_pub.publish(sat);
}
}