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

Committer:
cocorlow
Date:
Fri Apr 23 11:25:01 2021 +0000
Revision:
5:6fc37ae3f262
Parent:
3:f8045f83d7c1
Child:
6:db9f8d2afab7
mbed 2

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