Just some simple tests of RTH and PH on Sparky using the new navigation code. No problems, although I'm not in love with the performance yet. Nothing special about the setup.
At the end I also test the code using the inertial navigation filter from ardupilot instead of the EKF. I couldn't get it to fly as well, but perhaps that was my tuning. It was getting dark.