GPS rescue fixes

Hey @Limon thanks for your testing.
Here is gps_rescue_pitch_smoothing = 60, windy day, just to show that it should be pretty smooth on that setting.
This has the four recent commits that we have been discussing.