Takashi Koyama / GYSFDMAXB

GPS module (GYSFDMAXB) 57600 bps

Dependencies:   Vector3

Dependents:   HAPS_GPS_Test_0001

GYSFDMAXB GPSセンサーGYSFDMAXBのライブラリ 57600 bps

手順 [1] シリアルピンを設定 GYSFDMAXB gps(tx_pin, rx_pin); [2] 零点を設定する（NED座標系における零点） gps.Initialize(); [3] 自動的にデータを受信しては更新していくので、適宜メンバ変数を読み込んで使う

GYSFDMAXB.cpp@10:8f574ff249bd, 2021-04-26 (annotated)

Committer:
cocorlow
Date:
Mon Apr 26 08:18:48 2021 +0000
Revision:
10:8f574ff249bd
Parent:
9:8595608c56ca 
GPS completed [volatile]

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 5:6fc37ae3f262 1 #include "mbed.h"
cocorlow 5:6fc37ae3f262 2 #include "GYSFDMAXB.hpp"
cocorlow 5:6fc37ae3f262 3 #include <string.h>
cocorlow 5:6fc37ae3f262 4 #include <stdlib.h>
cocorlow 5:6fc37ae3f262 5 #include "Vector3.hpp"
cocorlow 5:6fc37ae3f262 6 #include <math.h>
cocorlow 5:6fc37ae3f262 7 #define M_PI 3.14159265358979f
cocorlow 5:6fc37ae3f262 8
cocorlow 5:6fc37ae3f262 9 GYSFDMAXB::GYSFDMAXB(PinName tx, PinName rx)
cocorlow 9:8595608c56ca 10 :serial(tx, rx, 57600), receive_flag(false), start_index(0), uart_index(0)
cocorlow 5:6fc37ae3f262 11 {
cocorlow 5:6fc37ae3f262 12 for (int i = 0; i < uart_size; i++)
cocorlow 5:6fc37ae3f262 13 {
cocorlow 5:6fc37ae3f262 14 uart_buffer[i] = '\0';
cocorlow 5:6fc37ae3f262 15 }
cocorlow 5:6fc37ae3f262 16 for (int i = 0; i < start_size; i++)
cocorlow 5:6fc37ae3f262 17 {
cocorlow 5:6fc37ae3f262 18 uart_start[i] = NULL;
cocorlow 5:6fc37ae3f262 19 }
cocorlow 5:6fc37ae3f262 20 serial.attach(this, &GYSFDMAXB::Receive, Serial::RxIrq);
cocorlow 6:db9f8d2afab7 21 timer.attach(this, &GYSFDMAXB::Punctuate, 0.1);
cocorlow 5:6fc37ae3f262 22 }
cocorlow 5:6fc37ae3f262 23
cocorlow 5:6fc37ae3f262 24 void GYSFDMAXB::Receive()
cocorlow 5:6fc37ae3f262 25 {
cocorlow 5:6fc37ae3f262 26 while (serial.readable())
cocorlow 5:6fc37ae3f262 27 {
cocorlow 5:6fc37ae3f262 28 char c;
cocorlow 5:6fc37ae3f262 29 c = serial.getc();
cocorlow 5:6fc37ae3f262 30 if (c == '$')
cocorlow 5:6fc37ae3f262 31 {
cocorlow 5:6fc37ae3f262 32 uart_buffer[uart_index] = c;
cocorlow 5:6fc37ae3f262 33 uart_start[start_index] = &(uart_buffer[uart_index]);
cocorlow 5:6fc37ae3f262 34 uart_index = (uart_index + 1) % uart_size;
cocorlow 5:6fc37ae3f262 35 }
cocorlow 5:6fc37ae3f262 36 else if (c == '\r')
cocorlow 5:6fc37ae3f262 37 {
cocorlow 5:6fc37ae3f262 38 }
cocorlow 5:6fc37ae3f262 39 else if (c == '\n')
cocorlow 5:6fc37ae3f262 40 {
cocorlow 5:6fc37ae3f262 41 start_index = (start_index + 1) % start_size;
cocorlow 5:6fc37ae3f262 42 uart_buffer[uart_index] = '\0';
cocorlow 5:6fc37ae3f262 43 receive_flag = true;
cocorlow 5:6fc37ae3f262 44 uart_index = (uart_index + 1) % uart_size;
cocorlow 5:6fc37ae3f262 45 }
cocorlow 5:6fc37ae3f262 46 else
cocorlow 5:6fc37ae3f262 47 {
cocorlow 5:6fc37ae3f262 48 uart_buffer[uart_index] = c;
cocorlow 5:6fc37ae3f262 49 uart_index = (uart_index + 1) % uart_size;
cocorlow 5:6fc37ae3f262 50 }
cocorlow 5:6fc37ae3f262 51 }
cocorlow 5:6fc37ae3f262 52 }
cocorlow 5:6fc37ae3f262 53
cocorlow 5:6fc37ae3f262 54 void GYSFDMAXB::Punctuate()
cocorlow 5:6fc37ae3f262 55 {
cocorlow 5:6fc37ae3f262 56 volatile static int counter = 0;
cocorlow 5:6fc37ae3f262 57 const int delay = 2;
cocorlow 5:6fc37ae3f262 58 if (receive_flag)
cocorlow 5:6fc37ae3f262 59 {
cocorlow 5:6fc37ae3f262 60 counter++;
cocorlow 5:6fc37ae3f262 61 }
cocorlow 5:6fc37ae3f262 62 if (counter >= delay)
cocorlow 5:6fc37ae3f262 63 {
cocorlow 5:6fc37ae3f262 64 counter = 0;
cocorlow 5:6fc37ae3f262 65 Update();
cocorlow 5:6fc37ae3f262 66 receive_flag = false;
cocorlow 5:6fc37ae3f262 67 }
cocorlow 5:6fc37ae3f262 68 }
cocorlow 5:6fc37ae3f262 69
cocorlow 5:6fc37ae3f262 70 void GYSFDMAXB::Update()
cocorlow 5:6fc37ae3f262 71 {
cocorlow 6:db9f8d2afab7 72 for (int i = 0; i < start_size; i++)
cocorlow 6:db9f8d2afab7 73 {
cocorlow 6:db9f8d2afab7 74 if (uart_start[i] != NULL)
cocorlow 5:6fc37ae3f262 75 {
cocorlow 6:db9f8d2afab7 76 char str[256];
cocorlow 6:db9f8d2afab7 77 char* p[64];
cocorlow 6:db9f8d2afab7 78 int p_index = 0;
cocorlow 6:db9f8d2afab7 79 for (int j = 0; j < 64; j++)
cocorlow 6:db9f8d2afab7 80 {
cocorlow 6:db9f8d2afab7 81 p[j] = NULL;
cocorlow 6:db9f8d2afab7 82 }
cocorlow 6:db9f8d2afab7 83 strcpy(str, uart_start[i]);
cocorlow 6:db9f8d2afab7 84
cocorlow 6:db9f8d2afab7 85
cocorlow 6:db9f8d2afab7 86 char checksum = 0;
cocorlow 6:db9f8d2afab7 87 int c_i = 1;
cocorlow 6:db9f8d2afab7 88 while (str[c_i] !='*')
cocorlow 5:6fc37ae3f262 89 {
cocorlow 6:db9f8d2afab7 90 checksum ^= str[c_i];
cocorlow 6:db9f8d2afab7 91 c_i++;
cocorlow 6:db9f8d2afab7 92 }
cocorlow 6:db9f8d2afab7 93 char data_checksum = 0;
cocorlow 6:db9f8d2afab7 94 char cc;
cocorlow 6:db9f8d2afab7 95 cc = str[c_i+1];
cocorlow 6:db9f8d2afab7 96 if ('0' <= cc && cc <= '9')
cocorlow 6:db9f8d2afab7 97 data_checksum += (cc-'0')*16;
cocorlow 6:db9f8d2afab7 98 else
cocorlow 6:db9f8d2afab7 99 data_checksum += ((cc-'A')+10)*16;
cocorlow 6:db9f8d2afab7 100 cc = str[c_i+2];
cocorlow 6:db9f8d2afab7 101 if ('0' <= cc && cc <= '9')
cocorlow 6:db9f8d2afab7 102 data_checksum += (cc-'0');
cocorlow 6:db9f8d2afab7 103 else
cocorlow 6:db9f8d2afab7 104 data_checksum += ((cc-'A')+10);
cocorlow 6:db9f8d2afab7 105 if (data_checksum != checksum)
cocorlow 6:db9f8d2afab7 106 {
cocorlow 6:db9f8d2afab7 107 continue;
cocorlow 6:db9f8d2afab7 108 }
cocorlow 6:db9f8d2afab7 109
cocorlow 6:db9f8d2afab7 110
cocorlow 6:db9f8d2afab7 111 int j = 0;
cocorlow 6:db9f8d2afab7 112 p[p_index] = str;
cocorlow 6:db9f8d2afab7 113 p_index++;
cocorlow 6:db9f8d2afab7 114 while (1)
cocorlow 6:db9f8d2afab7 115 {
cocorlow 6:db9f8d2afab7 116 if (str[j] == ',')
cocorlow 5:6fc37ae3f262 117 {
cocorlow 6:db9f8d2afab7 118 p[p_index] = &(str[j + 1]);
cocorlow 6:db9f8d2afab7 119 p_index++;
cocorlow 6:db9f8d2afab7 120 str[j] = '\0';
cocorlow 5:6fc37ae3f262 121 }
cocorlow 6:db9f8d2afab7 122 else if (str[j] == '\0')
cocorlow 5:6fc37ae3f262 123 {
cocorlow 6:db9f8d2afab7 124 break;
cocorlow 5:6fc37ae3f262 125 }
cocorlow 6:db9f8d2afab7 126 j++;
cocorlow 6:db9f8d2afab7 127 }
cocorlow 6:db9f8d2afab7 128
cocorlow 6:db9f8d2afab7 129
cocorlow 6:db9f8d2afab7 130 if (strcmp(p[0], "$GPGGA") == 0)
cocorlow 6:db9f8d2afab7 131 {
cocorlow 6:db9f8d2afab7 132 if (p[6][0] != '\0')
cocorlow 6:db9f8d2afab7 133 Quality = atoi(p[6]);
cocorlow 6:db9f8d2afab7 134 Satellites = 0;
cocorlow 6:db9f8d2afab7 135 if (p[7][0] != '\0')
cocorlow 6:db9f8d2afab7 136 Satellites = atoi(p[7]);
cocorlow 6:db9f8d2afab7 137 if (p[8][0] != '\0')
cocorlow 6:db9f8d2afab7 138 HDOP = atof(p[8]);
cocorlow 6:db9f8d2afab7 139 if (p[9][0] != '\0')
cocorlow 6:db9f8d2afab7 140 Elevation = atof(p[9]);
cocorlow 6:db9f8d2afab7 141 UnitElevation = p[10][0];
cocorlow 6:db9f8d2afab7 142 if (p[11][0] != '\0')
cocorlow 6:db9f8d2afab7 143 GeoidElevation = atof(p[11]);
cocorlow 6:db9f8d2afab7 144 UnitGeoidElevation = p[10][0];
cocorlow 6:db9f8d2afab7 145 }
cocorlow 6:db9f8d2afab7 146 else if (strcmp(p[0], "$GPGLL") == 0)
cocorlow 6:db9f8d2afab7 147 {
cocorlow 6:db9f8d2afab7 148 }
cocorlow 6:db9f8d2afab7 149 else if (strcmp(p[0], "$GPGSA") == 0)
cocorlow 6:db9f8d2afab7 150 {
cocorlow 6:db9f8d2afab7 151 }
cocorlow 6:db9f8d2afab7 152 else if (strcmp(p[0], "$GPGSV") == 0)
cocorlow 6:db9f8d2afab7 153 {
cocorlow 6:db9f8d2afab7 154 }
cocorlow 6:db9f8d2afab7 155 else if (strcmp(p[0], "$GPRMC") == 0)
cocorlow 6:db9f8d2afab7 156 {
cocorlow 6:db9f8d2afab7 157 float _ms_deg_1;
cocorlow 6:db9f8d2afab7 158 int _ms_deg_2;
cocorlow 6:db9f8d2afab7 159 Hours = (p[1][0]-'0')*10+(p[1][1]-'0');
cocorlow 6:db9f8d2afab7 160 Minutes = (p[1][2]-'0')*10+(p[1][3]-'0');
cocorlow 6:db9f8d2afab7 161 Seconds = (p[1][4]-'0')*10+(p[1][5]-'0');
cocorlow 6:db9f8d2afab7 162 Milliseconds = (p[1][7]-'0')*100+(p[1][8]-'0')*10+(p[1][9]-'0');
cocorlow 6:db9f8d2afab7 163 Status = p[2][0];
cocorlow 6:db9f8d2afab7 164 if (p[3][0] != '\0')
cocorlow 5:6fc37ae3f262 165 {
cocorlow 6:db9f8d2afab7 166 _ms_deg_1 = atof(p[3]);
cocorlow 6:db9f8d2afab7 167 _ms_deg_2 = (int)_ms_deg_1 / 100;
cocorlow 6:db9f8d2afab7 168 Latitude = _ms_deg_2 + (_ms_deg_1-100.0f*_ms_deg_2)/60.0f;
cocorlow 6:db9f8d2afab7 169 }
cocorlow 6:db9f8d2afab7 170 N_S = p[4][0];
cocorlow 6:db9f8d2afab7 171 if (p[5][0] != '\0')
cocorlow 6:db9f8d2afab7 172 {
cocorlow 6:db9f8d2afab7 173 _ms_deg_1 = atof(p[5]);
cocorlow 6:db9f8d2afab7 174 _ms_deg_2 = (int)_ms_deg_1 / 100;
cocorlow 6:db9f8d2afab7 175 Longitude = _ms_deg_2 + (_ms_deg_1-100.0f*_ms_deg_2)/60.0f;
cocorlow 5:6fc37ae3f262 176 }
cocorlow 6:db9f8d2afab7 177 E_W = p[6][0];
cocorlow 6:db9f8d2afab7 178 if (p[7][0] != '\0')
cocorlow 6:db9f8d2afab7 179 Speed = atof(p[7]);
cocorlow 6:db9f8d2afab7 180 if (p[8][0] != '\0')
cocorlow 6:db9f8d2afab7 181 Direction = atof(p[8]);
cocorlow 6:db9f8d2afab7 182 Day = (p[9][0]-'0')*10+(p[9][1]-'0');
cocorlow 6:db9f8d2afab7 183 Month = (p[9][2]-'0')*10+(p[9][3]-'0');
cocorlow 6:db9f8d2afab7 184 Year = (p[9][4]-'0')*10+(p[9][5]-'0');
cocorlow 6:db9f8d2afab7 185 if (p[10][0] != '\0')
cocorlow 6:db9f8d2afab7 186 GeomagneticDeclination = atof(p[10]);
cocorlow 6:db9f8d2afab7 187 GeomagneticE_W = p[11][0];
cocorlow 6:db9f8d2afab7 188 Mode = p[12][0];
cocorlow 5:6fc37ae3f262 189 }
cocorlow 6:db9f8d2afab7 190 else if (strcmp(p[0], "$GPVTG") == 0)
cocorlow 6:db9f8d2afab7 191 {
cocorlow 6:db9f8d2afab7 192 }
cocorlow 6:db9f8d2afab7 193 else if (strcmp(p[0], "$GPZDA") == 0)
cocorlow 6:db9f8d2afab7 194 {
cocorlow 6:db9f8d2afab7 195 uart_index = 0;
cocorlow 6:db9f8d2afab7 196 }
cocorlow 6:db9f8d2afab7 197
cocorlow 6:db9f8d2afab7 198 uart_start[i] = NULL;
cocorlow 5:6fc37ae3f262 199 }
cocorlow 6:db9f8d2afab7 200 }
cocorlow 5:6fc37ae3f262 201 }
cocorlow 5:6fc37ae3f262 202
cocorlow 5:6fc37ae3f262 203 void GYSFDMAXB::Initialize()
cocorlow 5:6fc37ae3f262 204 {
cocorlow 5:6fc37ae3f262 205 Satellites = 0;
cocorlow 5:6fc37ae3f262 206 while (Satellites <= 4 || Latitude == 0.0f || Longitude == 0.0f || Elevation == 0.0f)
cocorlow 5:6fc37ae3f262 207 {
cocorlow 5:6fc37ae3f262 208 }
cocorlow 5:6fc37ae3f262 209 CalcurateUnit();
cocorlow 5:6fc37ae3f262 210 }
cocorlow 5:6fc37ae3f262 211
cocorlow 5:6fc37ae3f262 212 Vector3 GYSFDMAXB::ToUniversalUnit()
cocorlow 5:6fc37ae3f262 213 {
cocorlow 5:6fc37ae3f262 214 // 東経180度、北緯0度で精度最大
cocorlow 5:6fc37ae3f262 215 float pi_2_theta = (Latitude * ((N_S == 'N') ? 1.0f : -1.0f)) * M_PI / 180.0f;
cocorlow 5:6fc37ae3f262 216 float pi_phi = ((E_W == 'E') ? Longitude - 180.0f : 180.0f - Longitude) * M_PI / 180.0f;
cocorlow 5:6fc37ae3f262 217 float x = - cosf(pi_2_theta) * cosf(pi_phi);
cocorlow 5:6fc37ae3f262 218 float y = - cosf(pi_2_theta) * sinf(pi_phi);
cocorlow 5:6fc37ae3f262 219 float z = sinf(pi_2_theta);
cocorlow 5:6fc37ae3f262 220 Vector3 v(x, y, z);
cocorlow 5:6fc37ae3f262 221 return v;
cocorlow 5:6fc37ae3f262 222 }
cocorlow 5:6fc37ae3f262 223
cocorlow 5:6fc37ae3f262 224 Vector3 GYSFDMAXB::ToUniversal()
cocorlow 5:6fc37ae3f262 225 {
cocorlow 5:6fc37ae3f262 226 Vector3 v = ToUniversalUnit();
cocorlow 5:6fc37ae3f262 227 return (Radius + Elevation) * v;
cocorlow 5:6fc37ae3f262 228 }
cocorlow 5:6fc37ae3f262 229
cocorlow 5:6fc37ae3f262 230 void GYSFDMAXB::CalcurateUnit()
cocorlow 5:6fc37ae3f262 231 {
cocorlow 5:6fc37ae3f262 232 Vector3 _d = -1.0f * ToUniversalUnit();
cocorlow 5:6fc37ae3f262 233
cocorlow 5:6fc37ae3f262 234 UniversalZeroPosition = -(Radius+Elevation)*_d;
cocorlow 5:6fc37ae3f262 235 Vector3 _z(0.0f, 0.0f, 1.0f);
cocorlow 5:6fc37ae3f262 236 Vector3 _e = _d * _z;
cocorlow 5:6fc37ae3f262 237 Vector3 _n = _e * _d;
cocorlow 5:6fc37ae3f262 238 UniversalZeroUnitN = _n;
cocorlow 5:6fc37ae3f262 239 UniversalZeroUnitE = _e;
cocorlow 5:6fc37ae3f262 240 UniversalZeroUnitD = _d;
cocorlow 5:6fc37ae3f262 241 }
cocorlow 5:6fc37ae3f262 242
cocorlow 5:6fc37ae3f262 243 void GYSFDMAXB::Calcurate()
cocorlow 5:6fc37ae3f262 244 {
cocorlow 5:6fc37ae3f262 245 UniversalPosition = ToUniversal();
cocorlow 5:6fc37ae3f262 246 Vector3 relative = UniversalPosition - UniversalZeroPosition;
cocorlow 5:6fc37ae3f262 247 Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
cocorlow 5:6fc37ae3f262 248 Position = _position;
cocorlow 5:6fc37ae3f262 249 }
cocorlow 5:6fc37ae3f262 250
cocorlow 5:6fc37ae3f262 251 Vector3 GYSFDMAXB::Calcurate(Vector3 position)
cocorlow 5:6fc37ae3f262 252 {
cocorlow 5:6fc37ae3f262 253 UniversalPosition = ToUniversal();
cocorlow 5:6fc37ae3f262 254 Vector3 relative = position - UniversalZeroPosition;
cocorlow 5:6fc37ae3f262 255 Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
cocorlow 5:6fc37ae3f262 256 return _position;
cocorlow 5:6fc37ae3f262 257 }