Can not set new Setpoint

I was working on collision avoidance using collision conus approach.
For that, I “upgraded” the check_traffic() function in Navigator.
Collision in XY plane is now detected with the collision conus approach that calculates a turn angle (dyaw).

My idea was to generate a vehicle command that stops the mission and overwrite the setpoint_tripple as long the avoidance maneuver is active. But the problem is, that the new setpoint is alway “nan”. What did I miss? How do I overwrite correctly the setpoint?

Here you can see the code:

Here the result at the moment:
I create in jMAVSim a fake_traffic. The drone stops where it is.