CansatB_2021 / Mbed 2 deprecated gilatina_6legs_test

６足歩行試験用プログラム Movement のdefine の値を変えると方向変換

Dependencies:   mbed Servo

getGPS.cpp@0:6b57aea7ad9b, 2021-12-06 (annotated)

Committer:
minanao
Date:
Mon Dec 06 08:54:49 2021 +0000
Revision:
0:6b57aea7ad9b
6legs_test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
minanao 0:6b57aea7ad9b 1 #include "mbed.h"
minanao 0:6b57aea7ad9b 2 #include "getGPS.h"
minanao 0:6b57aea7ad9b 3
minanao 0:6b57aea7ad9b 4 GPS::GPS(PinName gpstx,PinName gpsrx): _gps(gpstx,gpsrx)
minanao 0:6b57aea7ad9b 5 {
minanao 0:6b57aea7ad9b 6 latitude = 0;
minanao 0:6b57aea7ad9b 7 longitude = 0;
minanao 0:6b57aea7ad9b 8 _gps.baud(GPSBAUD);
minanao 0:6b57aea7ad9b 9 _gps.printf("$PMTK314,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29");
minanao 0:6b57aea7ad9b 10 }
minanao 0:6b57aea7ad9b 11
minanao 0:6b57aea7ad9b 12 bool GPS::getgps()
minanao 0:6b57aea7ad9b 13 {
minanao 0:6b57aea7ad9b 14 char gps_data[256];
minanao 0:6b57aea7ad9b 15 int i;
minanao 0:6b57aea7ad9b 16
minanao 0:6b57aea7ad9b 17 do {
minanao 0:6b57aea7ad9b 18 while(_gps.getc() != '$'); //$マークまで読み飛ばし
minanao 0:6b57aea7ad9b 19 i = 0;
minanao 0:6b57aea7ad9b 20
minanao 0:6b57aea7ad9b 21 /* gpa_data初期化 */
minanao 0:6b57aea7ad9b 22 for(int j = 0; j < 256; j++)
minanao 0:6b57aea7ad9b 23 gps_data[j] = '\0';
minanao 0:6b57aea7ad9b 24
minanao 0:6b57aea7ad9b 25 /* NMEAから一行読み込み */
minanao 0:6b57aea7ad9b 26 while((gps_data[i] = _gps.getc()) != '\r') {
minanao 0:6b57aea7ad9b 27 i++;
minanao 0:6b57aea7ad9b 28 if(i == 256) {
minanao 0:6b57aea7ad9b 29 i = 255;
minanao 0:6b57aea7ad9b 30 break;
minanao 0:6b57aea7ad9b 31 }
minanao 0:6b57aea7ad9b 32 }
minanao 0:6b57aea7ad9b 33 } while(strstr(gps_data, "GPGGA") == NULL); //GGAセンテンスまで一行ずつ読み込み続ける
minanao 0:6b57aea7ad9b 34
minanao 0:6b57aea7ad9b 35 int rlock;
minanao 0:6b57aea7ad9b 36 char ns,ew;
minanao 0:6b57aea7ad9b 37 double w_time, raw_longitude, raw_latitude;
minanao 0:6b57aea7ad9b 38 int satnum;
minanao 0:6b57aea7ad9b 39 double hdop;
minanao 0:6b57aea7ad9b 40
minanao 0:6b57aea7ad9b 41 if(sscanf(gps_data, "GPGGA, %lf, %lf, %c, %lf, %c, %d, %d, %lf", &w_time, &raw_latitude, &ns, &raw_longitude, &ew, &rlock, &satnum, &hdop) > 1) {
minanao 0:6b57aea7ad9b 42 /* 座標1（度部分） */
minanao 0:6b57aea7ad9b 43 int latitude_dd = (int)(raw_latitude / 100);
minanao 0:6b57aea7ad9b 44 int longitude_dd = (int)(raw_longitude / 100);
minanao 0:6b57aea7ad9b 45
minanao 0:6b57aea7ad9b 46 /* 座標2（分部分 → 度） */
minanao 0:6b57aea7ad9b 47 double latitude_md = (raw_latitude - latitude_dd * 100) / 60;
minanao 0:6b57aea7ad9b 48 double longitude_md = (raw_longitude - longitude_dd * 100) / 60;
minanao 0:6b57aea7ad9b 49
minanao 0:6b57aea7ad9b 50 /* 座標1 + 2 */
minanao 0:6b57aea7ad9b 51 latitude = latitude_dd + latitude_md;
minanao 0:6b57aea7ad9b 52 longitude = longitude_dd + longitude_md;
minanao 0:6b57aea7ad9b 53
minanao 0:6b57aea7ad9b 54 return true;
minanao 0:6b57aea7ad9b 55 } else
minanao 0:6b57aea7ad9b 56 return false; //GGAセンテンスの情報が欠けている時
minanao 0:6b57aea7ad9b 57 }