Takashi Koyama / GYSFDMAXB_Mbed_OS

GPS module (GYSFDMAXB) 57600 bps

Dependents:   HAPS_GPS_Test_0002

GYSFDMAXB.cpp@3:f8045f83d7c1, 2021-04-22 (annotated)

Committer:
cocorlow
Date:
Thu Apr 22 11:51:16 2021 +0000
Revision:
3:f8045f83d7c1
Parent:
2:9b567c8f5429
Child:
4:8d315f7977b3
error

Who changed what in which revision?

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