I have found the source of the math error this issue! https://github.com/ArduPilot/MissionPlanner/issues/1950
When the dist is taken it is already scaled for the correct units. When calculating the angle the dist is scaled again (when not in SI units) making the angle smaller than it should be.
An example. Dist of 223ft. Change in Alt of 50ft. MP currently shows -3.9 deg. arctan(50/(223*scale_factor) = 3.9deg
Correct value is 12.6deg for that example. arctan(50/223)=12.6deg
I’m not sure how the call stack works for that particular calculation so I’m not sure how to fix it.
On an unrelated note. I found the bug that is causing fps speeds to not be converted to m/s.
From the master branch (commit f6015abe) on line 3820 of MainV2.cs. Change CurrentState.multiplierdist to CurrentState.multiplierspeed.
I haven’t had a chance to set up the dev environment yet so this is completely untested. The math at FlightPlanner.cs line 5522 seems to be correct so I am digging into the other pieces to figure this out.
The wiki said to make a forum post rather than a PR.