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;
+}