使用GPS

Question

我的工作目标在下面的算法中给出。

1. 初始化H / W;等到GPS模块有一个有效的修复，然后启动电机。
2. 从GPS获取实际位置。
3. 计算到下一个航路点的距离和航向信息。
4. 将实际航向与当前航向角进行比较，并将差值输入电机。
5. 当实际位置接近第一个航点时，切换到下一个航点。
6. 如果/当达到最后一个航点时，请停止电动机。
7. 如果没有，请转到第2步。

我正在使用Haversine公式计算预定义坐标与当前位置之间的距离。我需要同一对坐标的方向角。但是当我使用 atan2 公式并使用Arduino IDE进行编译时，我的角度为0.05,0.03。我做错了吗？可以计算出10m附近点的航向角吗？

公式

double y = sin(dlon) * cos(lat2);
double x = ((cos(lat1) * sin(lat2)) - (sin(lat1) * cos(lat2) * cos(dlon)));
double brng = atan2(y, x);