Diff: GYSFDMAXB.cpp

Revision:

1:0d9b4ba850d8

Parent:

0:8114a6b113f4

Child:

2:9b567c8f5429

--- a/GYSFDMAXB.cpp	Tue Apr 06 08:16:08 2021 +0000
+++ b/GYSFDMAXB.cpp	Thu Apr 08 08:08:53 2021 +0000
@@ -147,15 +147,26 @@
                 }
                 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];
-                    Latitude = atof(p[3]);
+                    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.0*_ms_deg_2)/60.0;
+                    }
                     N_S = p[4][0];
                     if (p[5][0] != '\0')
-                        Longitude = atof(p[5]);
+                    {
+                        _ms_deg_1 = atof(p[5]);
+                        _ms_deg_2 = (int)_ms_deg_1 / 100;
+                        Longitude = _ms_deg_2 + (_ms_deg_1-100.0*_ms_deg_2)/60.0;
+                    }
                     E_W = p[6][0];
                     if (p[7][0] != '\0')
                         Speed = atof(p[7]);
@@ -201,13 +212,15 @@
 {
     float theta = (90.0 + Latitude * ((N_S == 'N') ? -1 : 1)) * M_PI / 180.0;
     float phi = (Longitude * ((E_W == 'E') ? 1 : -1)) * M_PI / 180.0;
-    Vector3 _z(sin(theta)*cos(phi), sin(theta)*sin(phi), cos(theta));
-    Vector3 _y(-cos(theta)*cos(phi), -cos(theta)*sin(phi), sin(theta));
+    Vector3 _d(-sin(theta)*cos(phi), -sin(theta)*sin(phi), -cos(theta));
     
-    UniversalZeroPosition = (Radius+Elevation)*_z;
-    UniversalZeroUnitY = _y;
-    UniversalZeroUnitZ = _z;
-    UniversalZeroUnitX = _y * _z;
+    UniversalZeroPosition = -(Radius+Elevation)*_d;
+    Vector3 _z(0.0, 0.0, 1.0);
+    Vector3 _e = _d * _z;
+    Vector3 _n = _e * _d;
+    UniversalZeroUnitN = _n;
+    UniversalZeroUnitE = _e;
+    UniversalZeroUnitD = _d;
 }
 
 void GYSFDMAXB::Calcurate()