Takashi Koyama
GPS module (GYSFDMAXB) 57600 bps
Dependents: HAPS_GPS_Test_0002
Diff: GYSFDMAXB.cpp
- Revision:
- 4:8d315f7977b3
- Parent:
- 3:f8045f83d7c1
- Child:
- 5:0983cd1d7183
--- a/GYSFDMAXB.cpp Thu Apr 22 11:51:16 2021 +0000
+++ b/GYSFDMAXB.cpp Thu Apr 22 12:20:43 2021 +0000
@@ -1,260 +1,262 @@
-//#include "mbed.h"
-//#include "GYSFDMAXB.hpp"
-//#include <string.h>
-//#include <stdlib.h>
-//#include "Vector3.hpp"
-//#include <math.h>
-//#define M_PI 3.14159265358979f
-//
-//GYSFDMAXB::GYSFDMAXB(PinName tx, PinName rx)
-// :serial(tx, rx), receive_flag(false), uart_index(0), start_index(0)
-//{
-// for (int i = 0; i < uart_size; i++)
-// {
-// uart_buffer[i] = '\0';
-// for (int j = 0; j < phrase_size; j++)
-// {
-// phrase_buffer[i][j] = '\0';
-// }
-// }
-// for (int i = 0; i < start_size; i++)
-// {
-// uart_start[i] = NULL;
-// }
-// serial.baud(57600);
-// serial.attach(this, &GYSFDMAXB::Receive, Serial::RxIrq);
-//}
-//
-//void GYSFDMAXB::Receive()
-//{
-// while (serial.readable())
-// {
-// char c;
-// c = serial.getc();
-// if (c == '$')
-// {
-// uart_buffer[uart_index] = c;
-// uart_start[start_index] = &(uart_buffer[uart_index]);
-// uart_index = (uart_index + 1) % uart_size;
-// }
-// else if (c == '\r')
-// {
-// }
-// else if (c == '\n')
-// {
-// start_index = (start_index + 1) % start_size;
-// uart_buffer[uart_index] = '\0';
-// receive_flag = true;
-// uart_index = (uart_index + 1) % uart_size;
-// }
-// else
-// {
-// uart_buffer[uart_index] = c;
-// uart_index = (uart_index + 1) % uart_size;
-// }
-// }
-//}
-//
-//void GYSFDMAXB::Update()
-//{
-// for (int i = 0; i < start_size; i++)
-// {
-// if (uart_start[i] != NULL)
-// {
-// strcpy(phrase_buffer[i], uart_start[i]);
-// uart_start[i] = NULL;
-// }
+#include "mbed.h"
+#include "GYSFDMAXB.hpp"
+#include <string.h>
+#include <stdlib.h>
+#include "Vector3.hpp"
+#include <math.h>
+#define M_PI 3.14159265358979f
+
+GYSFDMAXB::GYSFDMAXB(PinName tx, PinName rx)
+ :serial(tx, rx), receive_flag(false), start_index(0), uart_index(0)
+{
+ for (int i = 0; i < uart_size; i++)
+ {
+ uart_buffer[i] = '\0';
+ }
+ for (int i = 0; i < start_size; i++)
+ {
+ uart_start[i] = NULL;
+ }
+ serial.baud(57600);
+ serial.attach(this, &GYSFDMAXB::Receive, Serial::RxIrq);
+ timer.attach(this, &GYSFDMAXB::Punctuate, 0.01);
+}
+
+void GYSFDMAXB::Receive()
+{
+ while (serial.readable())
+ {
+ char c;
+ c = serial.getc();
+ if (c == '$')
+ {
+ uart_buffer[uart_index] = c;
+ uart_start[start_index] = &(uart_buffer[uart_index]);
+ uart_index = (uart_index + 1) % uart_size;
+ }
+ else if (c == '\r')
+ {
+ }
+ else if (c == '\n')
+ {
+ start_index = (start_index + 1) % start_size;
+ uart_buffer[uart_index] = '\0';
+ receive_flag = true;
+ uart_index = (uart_index + 1) % uart_size;
+ }
+ else
+ {
+ uart_buffer[uart_index] = c;
+ uart_index = (uart_index + 1) % uart_size;
+ }
+ }
+}
+
+void GYSFDMAXB::Punctuate()
+{
+ volatile static int counter = 0;
+ const int delay = 2;
+ if (receive_flag)
+ {
+ counter++;
+ }
+ if (counter >= delay)
+ {
+ counter = 0;
+ Update();
+ receive_flag = false;
+ }
+}
+
+void GYSFDMAXB::Update()
+{
+// if (receive_flag){
+ for (int i = 0; i < start_size; i++)
+ {
+ if (uart_start[i] != NULL)
+ {
+ char str[256];
+ char* p[16];
+ int p_index = 0;
+ for (int j = 0; j < 16; j++)
+ {
+ p[j] = NULL;
+ }
+ strcpy(str, uart_start[i]);
+
+
+ char checksum = 0;
+ int c_i = 1;
+ while (str[c_i] !='*')
+ {
+ checksum ^= str[c_i];
+ c_i++;
+ }
+ char data_checksum = 0;
+ char cc;
+ cc = str[c_i+1];
+ if ('0' <= cc && cc <= '9')
+ data_checksum += (cc-'0')*16;
+ else
+ data_checksum += ((cc-'A')+10)*16;
+ cc = str[c_i+2];
+ if ('0' <= cc && cc <= '9')
+ data_checksum += (cc-'0');
+ else
+ data_checksum += ((cc-'A')+10);
+ if (data_checksum != checksum)
+ {
+ continue;
+ }
+
+
+ int j = 0;
+ p[p_index] = str;
+ p_index++;
+ while (1)
+ {
+ if (str[j] == ',')
+ {
+ p[p_index] = &(str[j + 1]);
+ p_index++;
+ str[j] = '\0';
+ }
+ else if (str[j] == '\0')
+ {
+ break;
+ }
+ j++;
+ }
+
+
+ if (strcmp(p[0], "$GPGGA") == 0)
+ {
+ if (p[6][0] != '\0')
+ Quality = atoi(p[6]);
+ Satellites = 0;
+ if (p[7][0] != '\0')
+ Satellites = atoi(p[7]);
+ if (p[8][0] != '\0')
+ HDOP = atof(p[8]);
+ if (p[9][0] != '\0')
+ Elevation = atof(p[9]);
+ UnitElevation = p[10][0];
+ if (p[11][0] != '\0')
+ GeoidElevation = atof(p[11]);
+ UnitGeoidElevation = p[10][0];
+ }
+ else if (strcmp(p[0], "$GPGLL") == 0)
+ {
+ }
+ else if (strcmp(p[0], "$GPGSA") == 0)
+ {
+ }
+ else if (strcmp(p[0], "$GPGSV") == 0)
+ {
+ }
+ else if (strcmp(p[0], "$GPRMC") == 0)
+ {
+ float _ms_deg_1;
+ int _ms_deg_2;
+ Hours = (p[1][0]-'0')*10+(p[1][1]-'0');
+ Minutes = (p[1][2]-'0')*10+(p[1][3]-'0');
+ Seconds = (p[1][4]-'0')*10+(p[1][5]-'0');
+ Milliseconds = (p[1][7]-'0')*100+(p[1][8]-'0')*10+(p[1][9]-'0');
+ Status = p[2][0];
+ if (p[3][0] != '\0')
+ {
+ _ms_deg_1 = atof(p[3]);
+ _ms_deg_2 = (int)_ms_deg_1 / 100;
+ Latitude = _ms_deg_2 + (_ms_deg_1-100.0f*_ms_deg_2)/60.0f;
+ }
+ N_S = p[4][0];
+ if (p[5][0] != '\0')
+ {
+ _ms_deg_1 = atof(p[5]);
+ _ms_deg_2 = (int)_ms_deg_1 / 100;
+ Longitude = _ms_deg_2 + (_ms_deg_1-100.0f*_ms_deg_2)/60.0f;
+ }
+ E_W = p[6][0];
+ if (p[7][0] != '\0')
+ Speed = atof(p[7]);
+ if (p[8][0] != '\0')
+ Direction = atof(p[8]);
+ Day = (p[9][0]-'0')*10+(p[9][1]-'0');
+ Month = (p[9][2]-'0')*10+(p[9][3]-'0');
+ Year = (p[9][4]-'0')*10+(p[9][5]-'0');
+ if (p[10][0] != '\0')
+ GeomagneticDeclination = atof(p[10]);
+ GeomagneticE_W = p[11][0];
+ Mode = p[12][0];
+ }
+ else if (strcmp(p[0], "$GPVTG") == 0)
+ {
+ }
+ else if (strcmp(p[0], "$GPZDA") == 0)
+ {
+ uart_index = 0;
+ }
+
+ uart_start[i] = NULL;
+ }
+ }
+// receive_flag = false;
// }
-// for (int i = 0; i < start_size; i++)
-// {
-// if (phrase_buffer[i][0] != '\0')
-// {
-// char str[128];
-// char* p[16];
-// int p_index = 0;
-// for (int j = 0; j < 16; j++)
-// {
-// p[j] = NULL;
-// }
-// strcpy(str, phrase_buffer[i]);
-//
-//
-// char checksum = 0;
-// int c_i = 1;
-// while (str[c_i] !='*')
-// {
-// checksum ^= str[c_i];
-// c_i++;
-// }
-// char data_checksum = 0;
-// char cc;
-// cc = str[c_i+1];
-// if ('0' <= cc && cc <= '9')
-// data_checksum += (cc-'0')*16;
-// else
-// data_checksum += ((cc-'A')+10)*16;
-// cc = str[c_i+2];
-// if ('0' <= cc && cc <= '9')
-// data_checksum += (cc-'0');
-// else
-// data_checksum += ((cc-'A')+10);
-// if (data_checksum != checksum)
-// {
-// continue;
-// }
-//
-//
-// int j = 0;
-// p[p_index] = str;
-// p_index++;
-// while (1)
-// {
-// if (str[j] == ',')
-// {
-// p[p_index] = &(str[j + 1]);
-// p_index++;
-// str[j] = '\0';
-// }
-// else if (str[j] == '\0')
-// {
-// break;
-// }
-// j++;
-// }
-//
-//
-// if (strcmp(p[0], "$GPGGA") == 0)
-// {
-// strcpy(gpgga, phrase_buffer[i]);
-// for (int i = 0; i < 128; i++)
-// {
-// gpgga[i] =
-// }
-// else if (strcmp(p[0], "$GPGLL") == 0)
-// {
-// }
-// else if (strcmp(p[0], "$GPGSA") == 0)
-// {
-// }
-// else if (strcmp(p[0], "$GPGSV") == 0)
-// {
-// }
-// else if (strcmp(p[0], "$GPRMC") == 0)
-// {
-// float _ms_deg_1;
-// int _ms_deg_2;
-// Hours = (p[1][0]-'0')*10+(p[1][1]-'0');
-// Minutes = (p[1][2]-'0')*10+(p[1][3]-'0');
-// Seconds = (p[1][4]-'0')*10+(p[1][5]-'0');
-// Milliseconds = (p[1][7]-'0')*100+(p[1][8]-'0')*10+(p[1][9]-'0');
-// Status = p[2][0];
-// if (p[3][0] != '\0')
-// {
-// _ms_deg_1 = atof(p[3]);
-// _ms_deg_2 = (int)_ms_deg_1 / 100;
-// Latitude = _ms_deg_2 + (_ms_deg_1-100.0f*_ms_deg_2)/60.0f;
-// }
-// N_S = p[4][0];
-// if (p[5][0] != '\0')
-// {
-// _ms_deg_1 = atof(p[5]);
-// _ms_deg_2 = (int)_ms_deg_1 / 100;
-// Longitude = _ms_deg_2 + (_ms_deg_1-100.0f*_ms_deg_2)/60.0f;
-// }
-// E_W = p[6][0];
-// if (p[7][0] != '\0')
-// Speed = atof(p[7]);
-// if (p[8][0] != '\0')
-// Direction = atof(p[8]);
-// Day = (p[9][0]-'0')*10+(p[9][1]-'0');
-// Month = (p[9][2]-'0')*10+(p[9][3]-'0');
-// Year = (p[9][4]-'0')*10+(p[9][5]-'0');
-// if (p[10][0] != '\0')
-// GeomagneticDeclination = atof(p[10]);
-// GeomagneticE_W = p[11][0];
-// Mode = p[12][0];
-// }
-// else if (strcmp(p[0], "$GPVTG") == 0)
-// {
-// }
-// else if (strcmp(p[0], "$GPZDA") == 0)
-// {
-// uart_index = 0;
-//
-// if (gpgga[6][0] != '\0')
-// Quality = atoi(p[6]);
-// Satellites = 0;
-// if (gpgga[7][0] != '\0')
-// Satellites = atoi(p[7]);
-// if (gpgga[8][0] != '\0')
-// HDOP = atof(p[8]);
-// if (gpgga[9][0] != '\0')
-// Elevation = atof(p[9]);
-// UnitElevation = p[10][0];
-// if (gpgga[11][0] != '\0')
-// GeoidElevation = atof(gpgga[11]);
-// UnitGeoidElevation = gpgga[10][0];
-//
-// }
-//
-// strcpy(phrase_buffer[i], "");
-// }
-// }
-//}
-//
-//void GYSFDMAXB::Initialize()
-//{
-// Satellites = 0;
-// while (Satellites <= 4 || Latitude == 0.0f || Longitude == 0.0f || Elevation == 0.0f)
-// {
-// Update();
-// }
-// CalcurateUnit();
-//}
-//
-//Vector3 GYSFDMAXB::ToUniversalUnit()
-//{
-// // 東経180度、北緯0度で精度最大
-// float pi_2_theta = (Latitude * ((N_S == 'N') ? 1.0f : -1.0f)) * M_PI / 180.0f;
-// float pi_phi = ((E_W == 'E') ? Longitude - 180.0f : 180.0f - Longitude) * M_PI / 180.0f;
-// float x = - cosf(pi_2_theta) * cosf(pi_phi);
-// float y = - cosf(pi_2_theta) * sinf(pi_phi);
-// float z = sinf(pi_2_theta);
-// Vector3 v(x, y, z);
-// return v;
-//}
-//
-//Vector3 GYSFDMAXB::ToUniversal()
-//{
-// Vector3 v = ToUniversalUnit();
-// return (Radius + Elevation) * v;
-//}
-//
-//void GYSFDMAXB::CalcurateUnit()
-//{
-// Vector3 _d = -1.0f * ToUniversalUnit();
-//
-// UniversalZeroPosition = -(Radius+Elevation)*_d;
-// Vector3 _z(0.0f, 0.0f, 1.0f);
-// Vector3 _e = _d * _z;
-// Vector3 _n = _e * _d;
-// UniversalZeroUnitN = _n;
-// UniversalZeroUnitE = _e;
-// UniversalZeroUnitD = _d;
-//}
-//
-//void GYSFDMAXB::Calcurate()
-//{
-// UniversalPosition = ToUniversal();
-// Vector3 relative = UniversalPosition - UniversalZeroPosition;
-// Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
-// Position = _position;
-//}
-//
-//Vector3 GYSFDMAXB::Calcurate(Vector3 position)
-//{
-// UniversalPosition = ToUniversal();
-// Vector3 relative = position - UniversalZeroPosition;
-// Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
-// return _position;
-//}
+}
+
+void GYSFDMAXB::Initialize()
+{
+ Satellites = 0;
+ while (Satellites <= 4 || Latitude == 0.0f || Longitude == 0.0f || Elevation == 0.0f)
+ {
+ Update();
+ }
+ CalcurateUnit();
+}
+
+Vector3 GYSFDMAXB::ToUniversalUnit()
+{
+ // 東経180度、北緯0度で精度最大
+ float pi_2_theta = (Latitude * ((N_S == 'N') ? 1.0f : -1.0f)) * M_PI / 180.0f;
+ float pi_phi = ((E_W == 'E') ? Longitude - 180.0f : 180.0f - Longitude) * M_PI / 180.0f;
+ float x = - cosf(pi_2_theta) * cosf(pi_phi);
+ float y = - cosf(pi_2_theta) * sinf(pi_phi);
+ float z = sinf(pi_2_theta);
+ Vector3 v(x, y, z);
+ return v;
+}
+
+Vector3 GYSFDMAXB::ToUniversal()
+{
+ Vector3 v = ToUniversalUnit();
+ return (Radius + Elevation) * v;
+}
+
+void GYSFDMAXB::CalcurateUnit()
+{
+ Vector3 _d = -1.0f * ToUniversalUnit();
+
+ UniversalZeroPosition = -(Radius+Elevation)*_d;
+ Vector3 _z(0.0f, 0.0f, 1.0f);
+ Vector3 _e = _d * _z;
+ Vector3 _n = _e * _d;
+ UniversalZeroUnitN = _n;
+ UniversalZeroUnitE = _e;
+ UniversalZeroUnitD = _d;
+}
+
+void GYSFDMAXB::Calcurate()
+{
+ UniversalPosition = ToUniversal();
+ Vector3 relative = UniversalPosition - UniversalZeroPosition;
+ Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
+ Position = _position;
+}
+
+Vector3 GYSFDMAXB::Calcurate(Vector3 position)
+{
+ UniversalPosition = ToUniversal();
+ Vector3 relative = position - UniversalZeroPosition;
+ Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
+ return _position;
+}