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
Merge branch 'add_OS_for_test' of https://github.com/ming6842/firmware into os_dev

Conflicts:
	program/controller/navigation.h
slower down gps info, also increase the rate fo heartbeat