Diff: gps.cpp

Revision:

0:77bff687ff05

Child:

1:6165e8067d7a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gps.cpp	Fri Oct 02 13:48:57 2020 +0000
@@ -0,0 +1,67 @@
+#include "mbed.h"
+#include "getGPS.h"
+#include"math.h"
+
+Serial pc(USBTX,USBRX);
+GPS gps(D13,D14);
+Serial xbee(D0,D1);
+
+int main(){
+    double a;
+    double b;
+    double distance;
+    
+    pc.printf("GPS begin\n");
+    
+    while(1){
+        if(gps.getgps()){
+            /*a,bを緯度経度の初期値で初期化*/
+            a=gps.latitude;
+            b=gps.longitude;
+            pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
+                      break;
+        }else{
+            pc.printf("Fault_No_Data\r\n);
+         wait(1);
+        }
+         } while(1){
+         if(gps.getgps()){
+             pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示
+             
+    /*ここから距離の計算*/
+                 /*c、dを得た緯度経度の値で初期化*/   
+            　　　　　　double c;
+                      double d;
+                      c=gps.latitude;
+                      d=gps.longitude;
+                      
+                      const double pi=3.14159265359;//円周率
+                      
+                      /*ラジアンに変換*/
+                      double theta_a=a*pi/180;
+                      double theta_b=b*pi/180;
+                      double theta_c=c*pi/180;
+                      double theta_d=d*pi/180;
+                      
+                      double e=sin(theta_a)*sin(theta_c)+cos(theta_a)*cos(theta_c)*cos(theta_b-theta_d);//２点間のなす角を求める
+                      double theta_r=acos(e);
+                      
+                      const double earth_radius=6378140;//赤道半径
+                      
+                      distance=earth_radius*theta_r;//距離の計算
+                      
+             /*距離が20m以上なら表示、通信*/         
+                 if(distance>=20){
+                   pc.printf("run 20m");
+                  xbee.printf("run 20m");
+                  break;
+                  }
+
+            }else {
+                xbee.print("False_No_Data\r\n");
+                pc.printf("False_No_Date\r\n");
+                wait(1);
+            }//データ取得失敗を表示、通信、1秒待機
+        }
+        return 0;
+     }   
\ No newline at end of file