From 7105c6df3ee488b6cef9e46287b1d570302d1af4 Mon Sep 17 00:00:00 2001 From: Balduin Date: Wed, 23 Jul 2025 10:30:20 +0200 Subject: [PATCH] print reason for setpoint invalidity --- src/modules/mc_pos_control/PositionControl/PositionControl.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 72ae5a4a31..9491ef0b7a 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -228,6 +228,9 @@ 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