fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 18:40:36 +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/modules/mc_rate_control/MulticopterRateControl.cpp
+17 -7
View File
@@ -215,16 +215,26 @@ MulticopterRateControl::Run()
_rate_control.resetIntegral();
}
// update saturation status from mixer feedback
if (_motor_limits_sub.updated()) {
multirotor_motor_limits_s motor_limits;
// update saturation status from control allocation feedback
control_allocator_status_s control_allocator_status;
if (_motor_limits_sub.copy(&motor_limits)) {
MultirotorMixer::saturation_status saturation_status;
saturation_status.value = motor_limits.saturation_status;
if (_control_allocator_status_sub.update(&control_allocator_status)) {
Vector<bool, 3> saturation_positive;
Vector<bool, 3> saturation_negative;
_rate_control.setSaturationStatus(saturation_status);
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;
}
}
}
// TODO: send the unallocated value directly for better anti-windup
_rate_control.setSaturationStatus(saturation_positive, saturation_negative);
}
// run rate controller