GPS GNSS
Dependencies: Vector3
Diff: GPS_HAPS.cpp
- Revision:
- 0:6c7f7ed85f2c
- Child:
- 1:543687550091
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GPS_HAPS.cpp Tue Sep 07 05:30:59 2021 +0000 @@ -0,0 +1,239 @@ +#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); + float lat = (int)latitude/100; + Latitude = lat + (latitude-lat*100)/60.0f; + float lon = (int)longitude/100; + Longitude = lon + (longitude-lon*100)/60.0f; + N_S = ns; + E_W = ew; + Quality = fq; + Satellites = nst; + 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); + 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*86400000+Minutes*3600000+Seconds*1000+Milliseconds; + 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::Punctuate() +{ +} + +void GPS_HAPS::Attach() +{ + serial.attach(this, &GPS_HAPS::Receive, Serial::RxIrq); +// timer.attach(this, &GPS_HAPS::Punctuate, 0.1); +} + +void GPS_HAPS::Update() +{ +} + +void GPS_HAPS::Initialize() +{ + Satellites = 0; + while (Satellites <= 4 || Latitude == 0.0f || Longitude == 0.0f || Elevation == 0.0f) + { + } + CalcurateUnit(); +} + +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 + Elevation) * v; +} + +void GPS_HAPS::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 GPS_HAPS::Calcurate() +{ + UniversalPosition = ToUniversal(); + Vector3 relative = UniversalPosition - UniversalZeroPosition; + Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD); + Position = _position; +} + +Vector3 GPS_HAPS::Calcurate(Vector3 position) +{ + UniversalPosition = ToUniversal(); + Vector3 relative = position - UniversalZeroPosition; + Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD); + return _position; +}