Added taget waypoint for GO_HOME/ GO_SPECIFIED_POS
Added function to calculate distance between two coordinates
Finished navigation rule for WAYPOINT mode, not yet tested