1. テスト

作ったプログラムが妥当な計算結果であるかを確認する為、イメージし易いように東京スカイツリーから東京タワーに向かう状況を図1の通り仮定してみました

1.1 前提

方位の基準はGPSから出力されるデータに合わせ、図1の上(北)を0°とし、そこから時計回りに360°としています。

1.2 計算に必要な情報

現在地を東京スカイツリーとしてGPSからその座標(緯度経度)が取得されたと想定し、また目的地は東京タワーとして既にその座標(緯度経度)は既知です。

図1：方位・距離計算用のサンプルデータ

1.3 サイトで方位と距離を計算

図1サンプルデータの条件を図2のちょと便利帳さんのサイトに入力すると東京スカイツリーから東京タワーまでの距離は直線で8,238m、また角度は226°と言う事が分かりました！

取り敢えずこの値に近い値が出ればOKと言う事にします。

※近い値と書きましたが距離と方位の計算方法はこれと言った決まりはなく、計算機の能力に合わせて簡易的なものから厳密なものまでピンキリです。サイトによっても出てくる値はバラバラです

図2：ちょと便利帳さんのサイトで方位と距離を計算！

1.4 地球を球として距離を計算

緯度・経度から距離に換算する計算方法は様々あり、一番簡単なのが地球の半径を6,378,137mとして度から地球上の移動距離を計算する方法です。欠点としては地球は自転による遠心力で赤道面が膨らんでおり、球体ではなく楕円です。その為赤道近くに行くほど経度の方が大きな値となってしまいます。

1.5 地球を楕円として距離を計算

楕円表面の二点間の緯度経度から距離を計算するにはArduinoは16MHｚの計算能力しかなく、厳しいです

1.6 地球を楕円と認めつつ計算を簡略化して距離を計算

私は日本以外の国でGPSラジコンで遊ぶ予定は今のところ有りません

と言う事で日本近辺の楕円の歪みが緯度経度に与える影響を係数化した値を使うのが賢明です！

日本の緯度(代表地点：35度39分29秒1572)の時、

○緯度1秒の長さ：25.153129m

○経度1秒の長さ：30.820188m

1.7 計算速度優先して距離を計算

正直私の作ろうとしているものは距離が1～2割ズレても問題ありません！と言う事で1.6の緯度経度の係数の平均は27.986659m/秒位です・・・採用！

秒の係数は使えないので、1秒の長さから1度の長さに変換するために3600を掛けます。

27.986659ｍ/秒×3600秒/度=100751.97ｍ/度

そこから色々微調整して結局99096.44ｍ/度を経度緯度共に使いました

2. プログラム

方位と距離の計算はatan2関数の使い方の記事に詳しく説明しておりますのでここでは割愛いたします。※↑をすこし改良して以下の通り書いてみました。

このプログラムをArduinoUNOで実行するとシリアルモニタ上に図3の通り「Direction = 237.37deg:Distance = 8238m」と表示されました！

図2では「角度=226°：距離=8238m」なので取り敢えず良さそうです

void setup() {
  Serial.begin(9600); //9600bpsでシリアルポートを開く
}

void loop() {                                                      //{}内を無限ループで実行する
  delay(1000);                                                  //1000ms(1秒)待ちます
  float LatA = 35.710039, LongA = 139.810726;      //緯度宣言
  float LatB = 35.658600, LongB = 139.745427; //経度宣言
  Serial.print("Direction = ");                               //目的地Aの方角(°）
  Serial.print(atan2((LongA - LongB) * 1.23, (LatA - LatB)) * 57.3 + 180);
  Serial.print("deg:Distance = ");                             //目的地A迄の距離(m)
  Serial.print(sqrt(pow(LongA - LongB, 2) + pow(LatA - LatB, 2)) * 99096.44, 0);
  Serial.println("m");
}