GPS GNSS
Dependencies: Vector3
GPS_HAPS.cpp
- Committer:
- cocorlow
- Date:
- 2021-09-08
- Revision:
- 5:f23265e8d4aa
- Parent:
- 4:708e05b10257
File content as of revision 5:f23265e8d4aa:
#include "mbed.h" #include "GPS_HAPS.hpp" #include <string.h> #include <stdlib.h> #include "Vector3.hpp" #include <math.h> #define M_PI 3.14159265358979f GPS_HAPS::GPS_HAPS(PinName tx, PinName rx, int baud) :serial(tx, rx, baud), receive_start(0), receive_index(0), receive_request(0) { } void GPS_HAPS::Receive() { while (serial.readable()) { char c; c = serial.getc(); // pc.putc(c); receive_buffer[receive_index] = c; if (c == '\r') { receive_request++; } receive_index = (receive_index + 1) % RECEIVE_SIZE; } } void GPS_HAPS::Loop() { if (receive_request > 0) { if (receive_buffer[receive_start] == '$') { for (int i = 0; i < SENTENCE_SIZE; i++) { char c; c = receive_buffer[(receive_start + i) % RECEIVE_SIZE]; sentence_buffer[i] = c; if (c == '\r') { receive_start = (receive_start + i + 1) % RECEIVE_SIZE; sentence_buffer[i+1] = 0; break; } } Parse(sentence_buffer); receive_request--; } else { while (receive_start != receive_index) { receive_start = (receive_start + 1) % RECEIVE_SIZE; if (receive_buffer[receive_start] == '$') { break; } } } } } void GPS_HAPS::Parse(char *cmd) { char ns, ew, tf, status; int fq, nst, fix, date; // fix quality, Number of satellites being tracked, 3D fix float latitude, longitude, timefix, speed, altitude; char s[16]; // Global Positioning System Fix Data // $GNGGA,030503.00,3542.92568,N,13945.76087,E,1,05,10.02,225.2,M,39.4,M,,*7C // if(strncmp(cmd,"$GPGGA", 6) == 0) if(strstr(cmd,"GGA") != NULL) { sscanf(cmd, "%[^,],%f,%f,%c,%f,%c,%d,%d,%f", s, &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude); if (abs(latitude) < ERROR_F) { Latitude = -1.0f; } else { float lat = (int)latitude/100; Latitude = lat + (latitude-lat*100)/60.0f; } if (abs(longitude) < ERROR_F) { Longitude = -1.0f; } else { float lon = (int)longitude/100; Longitude = lon + (longitude-lon*100)/60.0f; } N_S = ns; E_W = ew; Quality = fq; Satellites = nst; if (abs(altitude) < ERROR_F) { Altitude = -1.0f; } { Altitude = altitude; } // pc.printf("%s\n", cmd); // pc.printf("##GPGGA Fix taken at: %f, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\r\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude); } // // Satellite status // else if(strncmp(cmd,"$GPGSA", 6) == 0) // { // sscanf(cmd, "$GPGSA,%c,%d,%d", &tf, &fix, &nst); // pc.printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst); // } // Geographic position, Latitude and Longitude // else if(strncmp(cmd,"$GPGLL", 6) == 0) // { // sscanf(cmd, "$GPGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix); // pc.printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix); // } // Geographic position, Latitude and Longitude // $GNRMC,035344.00,A,3542.91153,N,13945.74153,E,0.239,,070921,,,A,V*15 else if(strstr(cmd,"RMC") != NULL) { sscanf(cmd, "%[^,],%f,%c,%f,%c,%f,%c,%f,,%d", s, &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date); // pc.printf("%s,%f,%c,%f,%c,%f,%c,%f,,%d", s, timefix, status, latitude, ns, longitude, ew, speed, date); if (abs(timefix) > ERROR_F) { float time = timefix + 90000.00f; Hours = int(time) / 10000; Minutes = (int(time) % 10000) / 100; Seconds = int(time) % 100; Milliseconds = (int)(((time - (Hours*10000+Minutes*100+Seconds)))/0.001f); Time = Hours*3600000+Minutes*60000+Seconds*1000+Milliseconds; if (abs(speed) < ERROR_F) { Speed = -1.0f; } else { Speed = speed * 0.514444f; } Year = date % 100; date /= 100; Month = date % 100; date /= 100; Day = date; } // pc.printf("%s\n", cmd); // pc.printf("##GPRMC Fix taken at: %f, Status: %c, Latitude: %f %c, Longitude: %f %c, Speed: %f, Date: %d\r\n", timefix, status, latitude, ns, longitude, ew, speed, date); } // // $GNVTG,,T,,M,0.444,N,0.822,K,A*31 else if(strstr(cmd,"VTG") != NULL) { // pc.printf("%s\n", cmd); Direction = -1.0f; if(strstr(cmd, "VTG,,") == NULL){ sscanf(cmd, "%[^,],%f", s, &Direction); // pc.printf("<<%f\r\n", Direction); // pc.printf("its a Vector Track message.\n"); } } // else if(strncmp(cmd,"$GNGGA", 6) == 0) // { // sscanf(cmd, "$GNGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude); // pc.printf("GNGGA Fix taken at: %f, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude); // parseTime(timefix); // pc.printf("Time: %d:%d:%d\n", h_time, m_time, s_time); // } // else if(strncmp(cmd,"$GNGSA", 6) == 0) // { // sscanf(cmd, "$GNGSA,%c,%d,%d", &tf, &fix, &nst); // pc.printf("GNGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst); // } // else if(strncmp(cmd,"$GPGSV", 6) == 0) // { // // pc.printf("its a Satellite details message.\n"); // } // else if(strncmp(cmd,"$GNGLL", 6) == 0) // { // sscanf(cmd, "$GNGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix); // pc.printf("GNGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix); // } // else // { // pc.printf("Unknown message type\n"); // } } void GPS_HAPS::Attach() { serial.attach(this, &GPS_HAPS::Receive, Serial::RxIrq); } void GPS_HAPS::Initialize() { Satellites = 0; while (Satellites <= 4 || Latitude == 0.0f || Longitude == 0.0f || Altitude == 0.0f) { } CalculateUnit(); } Vector3 GPS_HAPS::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 GPS_HAPS::ToUniversal() { Vector3 v = ToUniversalUnit(); return (Radius + Altitude) * v; } void GPS_HAPS::CalculateUnit() { Vector3 _d = -1.0f * ToUniversalUnit(); UniversalZeroPosition = -(Radius+Altitude)*_d; Vector3 _z(0.0f, 0.0f, 1.0f); Vector3 _e = _d * _z; Vector3 _n = _e * _d; UniversalZeroUnitN = _n; UniversalZeroUnitE = _e; UniversalZeroUnitD = _d; } void GPS_HAPS::Calculate() { UniversalPosition = ToUniversal(); Vector3 relative = UniversalPosition - UniversalZeroPosition; Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD); Position = _position; } Vector3 GPS_HAPS::Calculate(Vector3 position) { UniversalPosition = ToUniversal(); Vector3 relative = position - UniversalZeroPosition; Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD); return _position; }