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.
This commit is contained in:
Matthias Grob 2019-11-21 22:03:13 +01:00 committed by Daniel Agar
parent ebdc29bc5f
commit dee8d200d8
1 changed files with 4 additions and 0 deletions

View File

@ -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;