Diff: GPSUBX_UART.cpp

Revision:

19:ae1a294cb30b

Parent:

15:e77382079cd9

diff -r e77382079cd9 -r ae1a294cb30b GPSUBX_UART.cpp
--- a/GPSUBX_UART.cpp	Mon Nov 29 12:32:42 2021 +0000
+++ b/GPSUBX_UART.cpp	Wed Dec 01 05:29:32 2021 +0000
@@ -103,6 +103,21 @@
 
 void GPSUBX_UART::Decode(char buffer[], int mc, int mi)
 {
+    // POSECEF
+    if (mc == 0x01 && mi == 0x01)
+    {
+        POSECEF posecef;
+        for (int i = 0; i < POSECEF_LEN; i++)
+        {
+            posecef.byte_data[i] = buffer[i+6];
+        }
+        iTOW_POSECEF = posecef.data.iTOW;
+        PositionECEF.x = (float)posecef.data.ecefX/100.0f;
+        PositionECEF.y = (float)posecef.data.ecefY/100.0f;
+        PositionECEF.z = (float)posecef.data.ecefZ/100.0f;
+//        pc.printf("!%d, %f, %f, %f\r\n", iTOW_POSLLH, Longitude, Latitude, Height);
+    }
+    
     // POSLLH
     if (mc == 0x01 && mi == 0x02)
     {
@@ -172,7 +187,7 @@
 //        pc.printf("#%d, %f, %f, %f\r\n", iTOW_VELNED, VelocityNED.x, VelocityNED.y, VelocityNED.z);
     }
 }
-
+/*
 Vector3 GPSUBX_UART::ToUniversalUnit()
 {
     // 東経180度、北緯0度で精度最大
@@ -191,33 +206,33 @@
     Vector3 v = ToUniversalUnit();
     return (Radius + Height) * v;
 }
-
+*/
 void GPSUBX_UART::CalculateUnit()
 {
-    Vector3 _d = -1.0f * ToUniversalUnit();
     
-    UniversalZeroPosition = -(Radius+Height)*_d;
-    Vector3 _z(0.0f, 0.0f, 1.0f);
-    Vector3 _e = _d * _z;
-    Vector3 _n = _e * _d;
-    UniversalZeroUnitN = _n / _n.Norm();
-    UniversalZeroUnitE = _e / _e.Norm();
-    UniversalZeroUnitD = _d;
+    UniversalZeroPosition = PositionECEF;
+    UniversalLatitude = Latitude;
+    UniversalLongitude = Longitude;
+    float lat = Latitude * M_PI / 180.0f;
+    float lon = Longitude * M_PI / 180.0f;
+    UniversalZeroUnit1.x = -sinf(lat)*cosf(lon);
+    UniversalZeroUnit1.y = -sinf(lat)*sinf(lon);
+    UniversalZeroUnit1.z =  cosf(lat);
+    
+    UniversalZeroUnit2.x = -sinf(lon);
+    UniversalZeroUnit2.y = cosf(lon);
+    UniversalZeroUnit2.z = 0.0f;
+    
+    UniversalZeroUnit3.x = -cosf(lat)*cosf(lon);
+    UniversalZeroUnit3.y = -cosf(lat)*sinf(lon);
+    UniversalZeroUnit3.z = -sinf(lat);
+    
     //twelite.printf("%f %f %f \r\n",_n,_e,_d);
 }
 
 void GPSUBX_UART::Calculate()
 {
-    UniversalPosition = ToUniversal();
-    Vector3 relative = UniversalPosition - UniversalZeroPosition;
-    Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
+    Vector3 relative = PositionECEF - UniversalZeroPosition;
+    Vector3 _position(relative % UniversalZeroUnit1, relative % UniversalZeroUnit2, relative % UniversalZeroUnit3);
     PositionNED = _position;
 }
-
-Vector3 GPSUBX_UART::Calculate(Vector3 position)
-{
-    UniversalPosition = ToUniversal();
-    Vector3 relative = position - UniversalZeroPosition;
-    Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
-    return _position;
-}