I'd like to ask any of the math wizards out there if you can write and post an improved "get_bearing" function for the navigation tab of the ArduPilot source.
The more I test the ArduPilot code with my emulator, the more nervous I get. You'll notice in the screenshot of my 43MPH flight around Paris on my emulator, the ArduPilot decided to take a few loops around waypoints and the larger the distance between the waypoints, the greater the error in bearing.
Here's the existing get_bearing function
long get_bearing(struct Location *loc1, struct Location *loc2)
long off_x = loc2->lng - loc1->lng;
long off_y = (loc2->lat - loc1->lat) * scaleLongUp;
long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795;
if (bearing < 0) bearing += 36000;