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@6:db9f8d2afab7, 2021-04-23 (annotated)

Committer:
cocorlow
Date:
Fri Apr 23 11:52:33 2021 +0000
Revision:
6:db9f8d2afab7
Parent:
5:6fc37ae3f262
Child:
9:8595608c56ca
punctuate; latest

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