From dee8d200d8bb42cd964065f9a6506e3c4a901f10 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 21 Nov 2019 22:03:13 +0100 Subject: [PATCH] mc_pos_control: fix position setpoints being logged as zero when NAN (#13557) This issue was found by @khabir and reported over slack. It resulted from the split up of the big pr #12072 into #13262. I took over the interface while the internal states still stayed the hard to understand internal ones. One of the follow up refactors will fix this completely and the entire legacy setpoint restore block can be removed. --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 2c6c5a8081..f8b23c8d82 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -676,6 +676,10 @@ MulticopterPositionControl::Run() vehicle_local_position_setpoint_s local_pos_sp{}; local_pos_sp.timestamp = time_stamp_now; _control.getLocalPositionSetpoint(local_pos_sp); + // Temporary setpoint message adjustments while PositionControl class is still keeping involved internal setpoints + local_pos_sp.x = setpoint.x; + local_pos_sp.y = setpoint.y; + local_pos_sp.z = setpoint.z; local_pos_sp.vx = PX4_ISFINITE(_control.getVelSp()(0)) ? _control.getVelSp()(0) : setpoint.vx; local_pos_sp.vy = PX4_ISFINITE(_control.getVelSp()(1)) ? _control.getVelSp()(1) : setpoint.vy; local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz;