Takashi Koyama / GYSFDMAXB_Mbed_OS

GPS module (GYSFDMAXB) 57600 bps

Dependents:   HAPS_GPS_Test_0002

GYSFDMAXB.cpp@2:9b567c8f5429, 2021-04-08 (annotated)

Committer:
cocorlow
Date:
Thu Apr 08 10:38:27 2021 +0000
Revision:
2:9b567c8f5429
Parent:
1:0d9b4ba850d8
Child:
3:f8045f83d7c1
sin cos accuracy

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 1:0d9b4ba850d8 150 float _ms_deg_1;
cocorlow 1:0d9b4ba850d8 151 int _ms_deg_2;
cocorlow 0:8114a6b113f4 152 Hours = (p[1][0]-'0')*10+(p[1][1]-'0');
cocorlow 0:8114a6b113f4 153 Minutes = (p[1][2]-'0')*10+(p[1][3]-'0');
cocorlow 0:8114a6b113f4 154 Seconds = (p[1][4]-'0')*10+(p[1][5]-'0');
cocorlow 0:8114a6b113f4 155 Milliseconds = (p[1][7]-'0')*100+(p[1][8]-'0')*10+(p[1][9]-'0');
cocorlow 0:8114a6b113f4 156 Status = p[2][0];
cocorlow 1:0d9b4ba850d8 157 if (p[3][0] != '\0')
cocorlow 1:0d9b4ba850d8 158 {
cocorlow 1:0d9b4ba850d8 159 _ms_deg_1 = atof(p[3]);
cocorlow 1:0d9b4ba850d8 160 _ms_deg_2 = (int)_ms_deg_1 / 100;
cocorlow 1:0d9b4ba850d8 161 Latitude = _ms_deg_2 + (_ms_deg_1-100.0*_ms_deg_2)/60.0;
cocorlow 1:0d9b4ba850d8 162 }
cocorlow 0:8114a6b113f4 163 N_S = p[4][0];
cocorlow 0:8114a6b113f4 164 if (p[5][0] != '\0')
cocorlow 1:0d9b4ba850d8 165 {
cocorlow 1:0d9b4ba850d8 166 _ms_deg_1 = atof(p[5]);
cocorlow 1:0d9b4ba850d8 167 _ms_deg_2 = (int)_ms_deg_1 / 100;
cocorlow 1:0d9b4ba850d8 168 Longitude = _ms_deg_2 + (_ms_deg_1-100.0*_ms_deg_2)/60.0;
cocorlow 1:0d9b4ba850d8 169 }
cocorlow 0:8114a6b113f4 170 E_W = p[6][0];
cocorlow 0:8114a6b113f4 171 if (p[7][0] != '\0')
cocorlow 0:8114a6b113f4 172 Speed = atof(p[7]);
cocorlow 0:8114a6b113f4 173 if (p[8][0] != '\0')
cocorlow 0:8114a6b113f4 174 Direction = atof(p[8]);
cocorlow 0:8114a6b113f4 175 Day = (p[9][0]-'0')*10+(p[9][1]-'0');
cocorlow 0:8114a6b113f4 176 Month = (p[9][2]-'0')*10+(p[9][3]-'0');
cocorlow 0:8114a6b113f4 177 Year = (p[9][4]-'0')*10+(p[9][5]-'0');
cocorlow 0:8114a6b113f4 178 if (p[10][0] != '\0')
cocorlow 0:8114a6b113f4 179 GeomagneticDeclination = atof(p[10]);
cocorlow 0:8114a6b113f4 180 GeomagneticE_W = p[11][0];
cocorlow 0:8114a6b113f4 181 Mode = p[12][0];
cocorlow 0:8114a6b113f4 182 }
cocorlow 0:8114a6b113f4 183 else if (strcmp(p[0], "$GPVTG") == 0)
cocorlow 0:8114a6b113f4 184 {
cocorlow 0:8114a6b113f4 185 }
cocorlow 0:8114a6b113f4 186 else if (strcmp(p[0], "$GPZDA") == 0)
cocorlow 0:8114a6b113f4 187 {
cocorlow 0:8114a6b113f4 188 uart_index = 0;
cocorlow 0:8114a6b113f4 189 }
cocorlow 0:8114a6b113f4 190
cocorlow 0:8114a6b113f4 191 uart_start[i] = NULL;
cocorlow 0:8114a6b113f4 192 }
cocorlow 0:8114a6b113f4 193 }
cocorlow 0:8114a6b113f4 194 receive_flag = false;
cocorlow 0:8114a6b113f4 195 }
cocorlow 0:8114a6b113f4 196 }
cocorlow 0:8114a6b113f4 197
cocorlow 0:8114a6b113f4 198
cocorlow 2:9b567c8f5429 199 Vector3 GYSFDMAXB::ToUniversalUnit()
cocorlow 0:8114a6b113f4 200 {
cocorlow 2:9b567c8f5429 201 // 東経180度、北緯0度で精度最大
cocorlow 2:9b567c8f5429 202 float pi_2_theta = (Latitude * ((N_S == 'N') ? 1.0 : -1.0)) * M_PI / 180.0;
cocorlow 2:9b567c8f5429 203 float pi_phi = ((E_W == 'E') ? Longitude - 180 : 180 - Longitude) * M_PI / 180.0;
cocorlow 2:9b567c8f5429 204 const float pi_2 = M_PI / 2;
cocorlow 0:8114a6b113f4 205 float r = Radius + Elevation;
cocorlow 2:9b567c8f5429 206 float x = - cos(pi_2_theta) * cos(pi_phi);
cocorlow 2:9b567c8f5429 207 float y = - cos(pi_2_theta) * sin(pi_phi);
cocorlow 2:9b567c8f5429 208 float z = sin(pi_2_theta);
cocorlow 0:8114a6b113f4 209 Vector3 v(x, y, z);
cocorlow 0:8114a6b113f4 210 return v;
cocorlow 0:8114a6b113f4 211 }
cocorlow 0:8114a6b113f4 212
cocorlow 2:9b567c8f5429 213 Vector3 GYSFDMAXB::ToUniversal()
cocorlow 2:9b567c8f5429 214 {
cocorlow 2:9b567c8f5429 215 Vector3 v = ToUniversalUnit();
cocorlow 2:9b567c8f5429 216 return (Radius + Elevation) * v;
cocorlow 2:9b567c8f5429 217 }
cocorlow 2:9b567c8f5429 218
cocorlow 0:8114a6b113f4 219 void GYSFDMAXB::CalcurateUnit()
cocorlow 0:8114a6b113f4 220 {
cocorlow 2:9b567c8f5429 221 Vector3 _d = -1.0 * ToUniversalUnit();
cocorlow 0:8114a6b113f4 222
cocorlow 1:0d9b4ba850d8 223 UniversalZeroPosition = -(Radius+Elevation)*_d;
cocorlow 1:0d9b4ba850d8 224 Vector3 _z(0.0, 0.0, 1.0);
cocorlow 1:0d9b4ba850d8 225 Vector3 _e = _d * _z;
cocorlow 1:0d9b4ba850d8 226 Vector3 _n = _e * _d;
cocorlow 1:0d9b4ba850d8 227 UniversalZeroUnitN = _n;
cocorlow 1:0d9b4ba850d8 228 UniversalZeroUnitE = _e;
cocorlow 1:0d9b4ba850d8 229 UniversalZeroUnitD = _d;
cocorlow 0:8114a6b113f4 230 }
cocorlow 0:8114a6b113f4 231
cocorlow 0:8114a6b113f4 232 void GYSFDMAXB::Calcurate()
cocorlow 0:8114a6b113f4 233 {
cocorlow 0:8114a6b113f4 234 UniversalPosition = ToUniversal();
cocorlow 0:8114a6b113f4 235 Vector3 relative = UniversalPosition - UniversalZeroPosition;
cocorlow 2:9b567c8f5429 236 Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
cocorlow 0:8114a6b113f4 237 Position = _position;
cocorlow 0:8114a6b113f4 238 }
cocorlow 0:8114a6b113f4 239
cocorlow 0:8114a6b113f4 240 Vector3 GYSFDMAXB::Calcurate(Vector3 position)
cocorlow 0:8114a6b113f4 241 {
cocorlow 0:8114a6b113f4 242 UniversalPosition = ToUniversal();
cocorlow 0:8114a6b113f4 243 Vector3 relative = position - UniversalZeroPosition;
cocorlow 2:9b567c8f5429 244 Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
cocorlow 0:8114a6b113f4 245 return _position;
cocorlow 0:8114a6b113f4 246 }