Takashi Koyama / GYSFDMAXB_Mbed_OS

GPS module (GYSFDMAXB) 57600 bps

Dependents:   HAPS_GPS_Test_0002

GYSFDMAXB.cpp@5:0983cd1d7183, 2021-04-23 (annotated)

Committer:
cocorlow
Date:
Fri Apr 23 11:43:26 2021 +0000
Revision:
5:0983cd1d7183
Parent:
4:8d315f7977b3 
16 -> 64

Who changed what in which revision?

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