Strange jumps of my heli

Hi I tested the 3.1.1 version on my heli and when I moved from one guided mode to another guided mode
the swash of my heli is jumping back and forth.
I’ve looked at the code responsible for the problem and I think it’s the function:
AC_WPNav::set_destination(const Vector3f& destination)
this finction is not reseting the state machine in the function:
void AC_WPNav::update_wpnav()
e.g the “_wpnav_step” is not set to zero and the “dt” is not zero when I call set_destination time after time
I have fixed this problem and the swash is moving mach smoother.
I have tried to recreate the bug in the SITL mode but its not happening there. :frowning:
any way I have wanted to know if this is a known issue and is this issued solved in the master version
since there was major changes in this version I don’t know if this issue is still relevant.

@origan,
How about providing either tlogs or data flash logs to help troubleshoot your issue?
Regards,
TCIII GM

Origan, thanks for the report. It sounds like you’ve done some good work trying to track it down. I’ll try to point Randy to this thread as he’s more familiar with this part of the code.