gps

Dependencies:   mbed

gps.cpp

Committer:
naruu
Date:
2020-10-07
Revision:
1:6165e8067d7a
Parent:
0:77bff687ff05
Child:
3:c64909f39383

File content as of revision 1:6165e8067d7a:

#include "mbed.h"
#include "getGPS.h"
#include"math.h"

Serial pc(USBTX,USBRX);
GPS gps(D12,D13);
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);//2点間のなす角を求める
                      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;
     }