fw_pos_control_l1: when doing a position reset just reset position controller

- do not try to shift the target waypoints when a reset occurs in position
control mode. Just make it lock into the current heading.

Signed-off-by: Roman <bapstroman@gmail.com>