WP Mode Set Head doesn't automatically reset when starting new mission

[breadoven]

Set up a mission with a Set Head WP that remained set until the end of the mission. Flew the mission twice without disarming but with WP mode cancelled and restarted between flights. Noticed that Set Head was still active when starting the second flight. Looking at the code there doesn't appear to be anything that cancels wpHeadingControl.mode when a new mission is started which would explain why it remained active.

Probably just needs wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE; setting in:

inav/src/main/navigation/navigation.c

Line 1463 in c63aa64

static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(navigationFSMState_t previousState)

[stronnag]

Indeed. Please PR.

[breadoven]

Tested and seems to do the job.