When an aircraft passes their last waypoint in the route, vnav, lnav and vnavspeed are set to False, but old waypoint remains the active waypoint.

Therefore, when adding a waypoint after the last waypoint in the route has been passed, lnav switches back on, but will try to navigate to the last waypoint in the route before the addition of the new waypoint. I'm not sure if this is intended, tried incrementing self.route[i].iactwp by 1 but did not function as expected.