SSLM1 / 2_GPS_GMS6-CR6

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   2-1_GMS6-CR6 5_flightmode 5-2_thrustermode 5-3_thruster_depressmode ... more

GMS6_CR6.cpp@3:b4133b354e5b, 2020-07-17 (annotated)

Committer:
rary
Date:
Fri Jul 17 12:13:26 2020 +0000
Revision:
3:b4133b354e5b
Parent:
2:98e44a8cca1d
Child:
4:dc48563aad90
add low mode

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rary 0:e504e33078a7 1 #include "mbed.h"
rary 0:e504e33078a7 2 #include "GMS6_CR6.h"
rary 0:e504e33078a7 3 #include "math.h"
rary 0:e504e33078a7 4
rary 0:e504e33078a7 5 GMS6_CR6::GMS6_CR6(PinName tx, PinName rx) : gps(tx, rx)
rary 0:e504e33078a7 6 {
rary 0:e504e33078a7 7 }
rary 0:e504e33078a7 8
rary 0:e504e33078a7 9 GMS6_CR6::~GMS6_CR6()
rary 0:e504e33078a7 10 {
rary 0:e504e33078a7 11 }
rary 0:e504e33078a7 12
rary 0:e504e33078a7 13 float GMS6_CR6::_DMS2DEG(float raw_data)
rary 0:e504e33078a7 14 {
rary 0:e504e33078a7 15 int d = (int)(raw_data/100);
rary 0:e504e33078a7 16 float m = (raw_data - (float)d * 100);
rary 0:e504e33078a7 17 return (float) d + m /60;
rary 0:e504e33078a7 18 }
rary 0:e504e33078a7 19
rary 0:e504e33078a7 20
rary 0:e504e33078a7 21 void GMS6_CR6::read(float *G)
rary 0:e504e33078a7 22 {
rary 0:e504e33078a7 23 char gps_data[256];
rary 0:e504e33078a7 24 memset(gps_data, '\0', 256);
rary 0:e504e33078a7 25
rary 1:cefba0edfdd3 26 int cnt_gps = 0;
rary 0:e504e33078a7 27
rary 0:e504e33078a7 28 float world_time;
rary 0:e504e33078a7 29 int rlock, sat_num;
rary 0:e504e33078a7 30 char lat, lon;
rary 0:e504e33078a7 31
rary 0:e504e33078a7 32 float lon_east = 0;
rary 0:e504e33078a7 33 float lat_north = 0;
rary 0:e504e33078a7 34 int i = 0;
rary 0:e504e33078a7 35
rary 2:98e44a8cca1d 36 while(lat_north < 20 && i < 1000000) //緯度が20度以上と測定されるまたは一定回数ループが回ると抜ける
rary 0:e504e33078a7 37 {
rary 0:e504e33078a7 38 if(gps.readable())
rary 0:e504e33078a7 39 {
rary 0:e504e33078a7 40 gps_data[cnt_gps] = gps.getc();
rary 0:e504e33078a7 41
rary 0:e504e33078a7 42 if(gps_data[cnt_gps] == '$' || cnt_gps == 256)
rary 0:e504e33078a7 43 {
rary 0:e504e33078a7 44 cnt_gps = 0;
rary 0:e504e33078a7 45 memset(gps_data, '\0', 256); //この関数なんや
rary 0:e504e33078a7 46 }
rary 0:e504e33078a7 47 else if(gps_data[cnt_gps] == '\r')
rary 0:e504e33078a7 48 {
rary 0:e504e33078a7 49 if(sscanf(gps_data, "GPGGA,%f,%f,%c,%f,%c,%d,%d", &world_time, &lat_north, &lat, &lon_east, &lon, &rlock, &sat_num) >= 1)
rary 0:e504e33078a7 50 {
rary 0:e504e33078a7 51 if (rlock == 1)
rary 0:e504e33078a7 52 {
rary 0:e504e33078a7 53 lat_north = _DMS2DEG(lat_north);
rary 0:e504e33078a7 54 lon_east = _DMS2DEG(lon_east);
rary 0:e504e33078a7 55 }
rary 0:e504e33078a7 56 }
rary 0:e504e33078a7 57 }
rary 0:e504e33078a7 58 else
rary 0:e504e33078a7 59 {
rary 0:e504e33078a7 60 cnt_gps++;
rary 0:e504e33078a7 61 }
rary 0:e504e33078a7 62 }
rary 0:e504e33078a7 63 i++;
rary 0:e504e33078a7 64 }
rary 0:e504e33078a7 65
rary 1:cefba0edfdd3 66 if(lat_north > 20 && lat_north < 90 && lon_east > 90 && lon_east < 180) //緯度が正常な値のときのみ値を返す
rary 0:e504e33078a7 67 {
rary 0:e504e33078a7 68 *G = lat_north;
rary 0:e504e33078a7 69 *(G+1) = lon_east;
rary 0:e504e33078a7 70 }
rary 0:e504e33078a7 71 else
rary 0:e504e33078a7 72 {
rary 1:cefba0edfdd3 73 *G = 4;
rary 1:cefba0edfdd3 74 *(G+1) = 4;
rary 0:e504e33078a7 75 }
rary 3:b4133b354e5b 76 }
rary 3:b4133b354e5b 77
rary 3:b4133b354e5b 78 void GMS6_CR6::read_low(float *G)
rary 3:b4133b354e5b 79 {
rary 3:b4133b354e5b 80 char gps_data[256];
rary 3:b4133b354e5b 81 memset(gps_data, '\0', 256);
rary 3:b4133b354e5b 82
rary 3:b4133b354e5b 83 int cnt_gps = 0;
rary 3:b4133b354e5b 84
rary 3:b4133b354e5b 85 float world_time;
rary 3:b4133b354e5b 86 int rlock, sat_num;
rary 3:b4133b354e5b 87 char lat, lon;
rary 3:b4133b354e5b 88
rary 3:b4133b354e5b 89 float lon_east = 0;
rary 3:b4133b354e5b 90 float lat_north = 0;
rary 3:b4133b354e5b 91 int i = 0;
rary 3:b4133b354e5b 92
rary 3:b4133b354e5b 93 while(lat_north < 20 && i < 1000000) //緯度が20度以上と測定されるまたは一定回数ループが回ると抜ける
rary 3:b4133b354e5b 94 {
rary 3:b4133b354e5b 95 if(gps.readable())
rary 3:b4133b354e5b 96 {
rary 3:b4133b354e5b 97 gps_data[cnt_gps] = gps.getc();
rary 3:b4133b354e5b 98
rary 3:b4133b354e5b 99 if(gps_data[cnt_gps] == '$' || cnt_gps == 256)
rary 3:b4133b354e5b 100 {
rary 3:b4133b354e5b 101 cnt_gps = 0;
rary 3:b4133b354e5b 102 memset(gps_data, '\0', 256); //この関数なんや
rary 3:b4133b354e5b 103 }
rary 3:b4133b354e5b 104 else if(gps_data[cnt_gps] == '\r')
rary 3:b4133b354e5b 105 {
rary 3:b4133b354e5b 106 if(sscanf(gps_data, "GPGGA,%f,%f,%c,%f,%c,%d,%d", &world_time, &lat_north, &lat, &lon_east, &lon, &rlock, &sat_num) >= 1)
rary 3:b4133b354e5b 107 {
rary 3:b4133b354e5b 108 if (rlock == 1)
rary 3:b4133b354e5b 109 {
rary 3:b4133b354e5b 110 lat_north = _DMS2DEG(lat_north);
rary 3:b4133b354e5b 111 lon_east = _DMS2DEG(lon_east);
rary 3:b4133b354e5b 112 }
rary 3:b4133b354e5b 113 }
rary 3:b4133b354e5b 114 }
rary 3:b4133b354e5b 115 else
rary 3:b4133b354e5b 116 {
rary 3:b4133b354e5b 117 cnt_gps++;
rary 3:b4133b354e5b 118 }
rary 3:b4133b354e5b 119 }
rary 3:b4133b354e5b 120 i++;
rary 3:b4133b354e5b 121 }
rary 3:b4133b354e5b 122
rary 3:b4133b354e5b 123 if(lat_north > 20 && lat_north < 90 && lon_east > 90 && lon_east < 180) //緯度が正常な値のときのみ値を返す
rary 3:b4133b354e5b 124 {
rary 3:b4133b354e5b 125 *G = lat_north;
rary 3:b4133b354e5b 126 *(G+1) = lon_east;
rary 3:b4133b354e5b 127 }
rary 3:b4133b354e5b 128 else
rary 3:b4133b354e5b 129 {
rary 3:b4133b354e5b 130 *G = 4;
rary 3:b4133b354e5b 131 *(G+1) = 4;
rary 3:b4133b354e5b 132 }
rary 0:e504e33078a7 133 }