Diff: gps.cpp

Revision:

2:639bfe0721a7

Parent:

1:c142b1682312

Child:

3:3cd62c194bd0

--- a/gps.cpp	Wed Oct 30 01:42:40 2019 +0000
+++ b/gps.cpp	Wed Oct 30 05:14:03 2019 +0000
@@ -3,54 +3,103 @@
 #include "math.h"
 
 Serial pc(USBTX,USBRX);
-GPS gps (p27,p28);
+GPS gps (p28,p27);
 
 
 int main() {
-    double a;
-    double b;
-    double c;
-    double d;
-    double distance;
+    double distance(double a, double b)
     
     pc.printf("GPS Start\n");
     
-     while(1)
-     {
-         if(gps.getgps())
-         {
+     while(1) {
+         if(gps.getgps()){
            a = gps.latitude;
            b = gps.longitude;
            
-          pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示   
+          pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示   
            break;
-         }
-         else
-         {
+           
+         }else{
           pc.printf("NO DATA\r\n");//データ取得失敗
           wait(1);
             }
        }
-      while(1)
-      {
-         if(gps.getgps())
-         {
-           c = gps.latitude;
-           d = gps.longitude;
+      while(1){
+         if(gps.getgps()) {
+           
+           
+            
            pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示   
-           distance =6370*cos(sin(a)*sin(c)+cos(a)*cos(c)*cos(b-d));
-           
-            if (distance<5)
-            {
-             }else
-             {
+          
+                   
+           // 球面三角法により、大円距離(メートル)を求める
+           double distance(double c, double d) {
+               c = gps.latitude;
+               d = gps.longitude;
+
+            const double pi = 3.14159265359; // 円周率
+                           
+            double ra = a * pi / 180;
+            double rb = b * pi / 180;     // 緯度経度をラジアンに変換
+            double rc = c * pi / 180;
+            double rd = d * pi / 180;
+
+            
+            double e = sin(ra) * sin(rc) +  cos(ra) * cos(rc) * cos(rb - rd);  // 2点の中心角(ラジアン)を求める
+            double rr = acos(e);
+
+            const double earth_radius = 6378140;   // 地球赤道半径(m)
+
+            double distance = earth_radius * rr; // 2点間の距離(m)
+
+           return distance;
+             }
+
+         if (distance<5){
+             }else{
              pc.printf("5m clear!");
              break;
-             }
+              }
+          
           }else{
            pc.printf("NO DATA\r\n");//データ取得失敗
            wait(1);
            }
         }
         return 0;
-     }
\ No newline at end of file
+     }
+     
+     
+     
+     
+     
+    
+
+// 球面三角法により、大円距離(メートル)を求める
+double distance(double lat1, double lng1, double lat2, double lng2) {
+
+    // 円周率
+    const double pi = 3.14159265359;
+
+    // 緯度経度をラジアンに変換
+    double rlat1 = lat1 * pi / 180;
+    double rlng1 = lng1 * pi / 180;
+    double rlat2 = lat2 * pi / 180;
+    double rlng2 = lng2 * pi / 180;
+
+    // 2点の中心角(ラジアン)を求める
+    double a =
+      sin(rlat1) * sin(rlat2) +
+      cos(rlat1) * cos(rlat2) *
+      cos(rlng1 - rlng2);
+    double rr = acos(a);
+
+    // 地球赤道半径(メートル)
+    const double earth_radius = 6378140;
+
+    // 2点間の距離(メートル)
+    double distance = earth_radius * rr;
+
+    return distance;
+}
+