File content as of revision 2:639bfe0721a7:

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

Serial pc(USBTX,USBRX);
GPS gps (p28,p27);


int main() {
    double distance(double a, double b)
    
    pc.printf("GPS Start\n");
    
     while(1) {
         if(gps.getgps()){
           a = gps.latitude;
           b = gps.longitude;
           
          pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示   
           break;
           
         }else{
          pc.printf("NO DATA\r\n");//データ取得失敗
          wait(1);
            }
       }
      while(1){
         if(gps.getgps()) {
           
           
            
           pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示   
          
                   
           // 球面三角法により、大円距離(メートル)を求める
           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;
     }
     
     
     
     
     
    

// 球面三角法により、大円距離(メートル)を求める
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;
}