fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-10 01:38:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

better invalidation output

Browse Source
This commit is contained in:
Balduin
2025-07-23 14:06:35 +02:00
parent 3e97782bc5
commit adae225f43
1 changed files with 7 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+7 -3
View File
@@ -228,9 +228,6 @@ bool PositionControl::_inputValid()
// Every axis x, y, z needs to have some setpoint
for (int i = 0; i <= 2; i++) {
valid = valid && (PX4_ISFINITE(_pos_sp(i)) || PX4_ISFINITE(_vel_sp(i)) || PX4_ISFINITE(_acc_sp(i)));
if (!valid) {
printf("invalidated at axis %d. pos finite %d, vel finite %d, acc finite %d\n", i, PX4_ISFINITE(_pos_sp(i)), PX4_ISFINITE(_vel_sp(i)), PX4_ISFINITE(_acc_sp(i)));
}
}
// x and y input setpoints always have to come in pairs
@@ -248,6 +245,13 @@ bool PositionControl::_inputValid()
valid = valid && PX4_ISFINITE(_vel(i)) && PX4_ISFINITE(_vel_dot(i));
}
}
if (!valid) {
printf("invalidated with:\n_pos_sp: (%.2f, %.2f, %.2f)\n", (double) _pos_sp(0), (double) _pos_sp(1), (double) _pos_sp(2));
printf("_vel_sp: (%.2f, %.2f, %.2f)\n", (double) _vel_sp(0), (double) _vel_sp(1), (double) _vel_sp(2));
printf("_acc_sp: (%.2f, %.2f, %.2f)\n", (double) _acc_sp(0), (double) _acc_sp(1), (double) _acc_sp(2));
}
return valid;
}