Takashi Koyama / GYSFDMAXB_Mbed_OS

GPS module (GYSFDMAXB) 57600 bps

Dependents:   HAPS_GPS_Test_0002

GYSFDMAXB.cpp@0:8114a6b113f4, 2021-04-06 (annotated)

Committer:
cocorlow
Date:
Tue Apr 06 08:16:08 2021 +0000
Revision:
0:8114a6b113f4
Child:
1:0d9b4ba850d8
gps library

Who changed what in which revision?

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