sae ichinose / Mbed 2 deprecated GPSXbee-completed

GPSとXbeeの統合

Dependencies:   mbed

gps.cpp@2:639bfe0721a7, 2019-10-30 (annotated)

Committer:
KINU
Date:
Wed Oct 30 05:14:03 2019 +0000
Revision:
2:639bfe0721a7
Parent:
1:c142b1682312
Child:
3:3cd62c194bd0
GPS3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
KINU 1:c142b1682312 1 #include "mbed.h"
KINU 1:c142b1682312 2 #include "getGPS.h"
KINU 1:c142b1682312 3 #include "math.h"
KINU 1:c142b1682312 4
KINU 1:c142b1682312 5 Serial pc(USBTX,USBRX);
KINU 2:639bfe0721a7 6 GPS gps (p28,p27);
KINU 1:c142b1682312 7
KINU 1:c142b1682312 8
KINU 1:c142b1682312 9 int main() {
KINU 2:639bfe0721a7 10 double distance(double a, double b)
KINU 1:c142b1682312 11
KINU 1:c142b1682312 12 pc.printf("GPS Start\n");
KINU 1:c142b1682312 13
KINU 2:639bfe0721a7 14 while(1) {
KINU 2:639bfe0721a7 15 if(gps.getgps()){
KINU 1:c142b1682312 16 a = gps.latitude;
KINU 1:c142b1682312 17 b = gps.longitude;
KINU 1:c142b1682312 18
KINU 2:639bfe0721a7 19 pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
KINU 1:c142b1682312 20 break;
KINU 2:639bfe0721a7 21
KINU 2:639bfe0721a7 22 }else{
KINU 1:c142b1682312 23 pc.printf("NO DATA\r\n");//データ取得失敗
KINU 1:c142b1682312 24 wait(1);
KINU 1:c142b1682312 25 }
KINU 1:c142b1682312 26 }
KINU 2:639bfe0721a7 27 while(1){
KINU 2:639bfe0721a7 28 if(gps.getgps()) {
KINU 2:639bfe0721a7 29
KINU 2:639bfe0721a7 30
KINU 2:639bfe0721a7 31
KINU 1:c142b1682312 32 pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示
KINU 2:639bfe0721a7 33
KINU 2:639bfe0721a7 34
KINU 2:639bfe0721a7 35 // 球面三角法により、大円距離(メートル)を求める
KINU 2:639bfe0721a7 36 double distance(double c, double d) {
KINU 2:639bfe0721a7 37 c = gps.latitude;
KINU 2:639bfe0721a7 38 d = gps.longitude;
KINU 2:639bfe0721a7 39
KINU 2:639bfe0721a7 40 const double pi = 3.14159265359; // 円周率
KINU 2:639bfe0721a7 41
KINU 2:639bfe0721a7 42 double ra = a * pi / 180;
KINU 2:639bfe0721a7 43 double rb = b * pi / 180; // 緯度経度をラジアンに変換
KINU 2:639bfe0721a7 44 double rc = c * pi / 180;
KINU 2:639bfe0721a7 45 double rd = d * pi / 180;
KINU 2:639bfe0721a7 46
KINU 2:639bfe0721a7 47
KINU 2:639bfe0721a7 48 double e = sin(ra) * sin(rc) + cos(ra) * cos(rc) * cos(rb - rd); // 2点の中心角(ラジアン)を求める
KINU 2:639bfe0721a7 49 double rr = acos(e);
KINU 2:639bfe0721a7 50
KINU 2:639bfe0721a7 51 const double earth_radius = 6378140; // 地球赤道半径(m)
KINU 2:639bfe0721a7 52
KINU 2:639bfe0721a7 53 double distance = earth_radius * rr; // 2点間の距離(m)
KINU 2:639bfe0721a7 54
KINU 2:639bfe0721a7 55 return distance;
KINU 2:639bfe0721a7 56 }
KINU 2:639bfe0721a7 57
KINU 2:639bfe0721a7 58 if (distance<5){
KINU 2:639bfe0721a7 59 }else{
KINU 1:c142b1682312 60 pc.printf("5m clear!");
KINU 1:c142b1682312 61 break;
KINU 2:639bfe0721a7 62 }
KINU 2:639bfe0721a7 63
KINU 1:c142b1682312 64 }else{
KINU 1:c142b1682312 65 pc.printf("NO DATA\r\n");//データ取得失敗
KINU 1:c142b1682312 66 wait(1);
KINU 1:c142b1682312 67 }
KINU 1:c142b1682312 68 }
KINU 1:c142b1682312 69 return 0;
KINU 2:639bfe0721a7 70 }
KINU 2:639bfe0721a7 71
KINU 2:639bfe0721a7 72
KINU 2:639bfe0721a7 73
KINU 2:639bfe0721a7 74
KINU 2:639bfe0721a7 75
KINU 2:639bfe0721a7 76
KINU 2:639bfe0721a7 77
KINU 2:639bfe0721a7 78 // 球面三角法により、大円距離(メートル)を求める
KINU 2:639bfe0721a7 79 double distance(double lat1, double lng1, double lat2, double lng2) {
KINU 2:639bfe0721a7 80
KINU 2:639bfe0721a7 81 // 円周率
KINU 2:639bfe0721a7 82 const double pi = 3.14159265359;
KINU 2:639bfe0721a7 83
KINU 2:639bfe0721a7 84 // 緯度経度をラジアンに変換
KINU 2:639bfe0721a7 85 double rlat1 = lat1 * pi / 180;
KINU 2:639bfe0721a7 86 double rlng1 = lng1 * pi / 180;
KINU 2:639bfe0721a7 87 double rlat2 = lat2 * pi / 180;
KINU 2:639bfe0721a7 88 double rlng2 = lng2 * pi / 180;
KINU 2:639bfe0721a7 89
KINU 2:639bfe0721a7 90 // 2点の中心角(ラジアン)を求める
KINU 2:639bfe0721a7 91 double a =
KINU 2:639bfe0721a7 92 sin(rlat1) * sin(rlat2) +
KINU 2:639bfe0721a7 93 cos(rlat1) * cos(rlat2) *
KINU 2:639bfe0721a7 94 cos(rlng1 - rlng2);
KINU 2:639bfe0721a7 95 double rr = acos(a);
KINU 2:639bfe0721a7 96
KINU 2:639bfe0721a7 97 // 地球赤道半径(メートル)
KINU 2:639bfe0721a7 98 const double earth_radius = 6378140;
KINU 2:639bfe0721a7 99
KINU 2:639bfe0721a7 100 // 2点間の距離(メートル)
KINU 2:639bfe0721a7 101 double distance = earth_radius * rr;
KINU 2:639bfe0721a7 102
KINU 2:639bfe0721a7 103 return distance;
KINU 2:639bfe0721a7 104 }
KINU 2:639bfe0721a7 105