Diff: GPSUBX_UART.cpp

Revision:

17:2e6147aeba54

Parent:

16:8181909e88f8

--- a/GPSUBX_UART.cpp	Tue Nov 30 12:04:34 2021 +0000
+++ b/GPSUBX_UART.cpp	Fri Dec 10 10:41:56 2021 +0000
@@ -1,10 +1,9 @@
 #include "GPSUBX_UART.hpp"
 #include "mbed.h"
 #include <math.h>
-#define M_PI 3.14159265358979f
 
 GPSUBX_UART::GPSUBX_UART(PinName tx, PinName rx, int baud, int timezone)
-    :serial(tx, rx, baud), TimeZone(timezone), receive_index(0)
+    :serial(tx, rx, baud), receive_index(0), TimeZone(timezone)
 {
 }
 
@@ -163,9 +162,9 @@
             velned.byte_data[i] = buffer[i+6];
         }
         iTOW_VELNED = velned.data.iTOW;
-        VelocityNED.x = (float)velned.data.velN / 100.0f;
-        VelocityNED.y = (float)velned.data.velE / 100.0f;
-        VelocityNED.z = (float)velned.data.velD / 100.0f;
+        VelocityNED(0) = (float)velned.data.velN / 100.0f;
+        VelocityNED(1) = (float)velned.data.velE / 100.0f;
+        VelocityNED(2) = (float)velned.data.velD / 100.0f;
         Speed = (float)velned.data.speed / 100.0f;
         GroundSpeed = (float)velned.data.gSpeed / 100.0f;
         Heading = (float)velned.data.heading * 1e-5f;
@@ -173,51 +172,50 @@
     }
 }
 
-Vector3 GPSUBX_UART::ToUniversalUnit()
+Vector3f GPSUBX_UART::ToUniversalUnit()
 {
     // 東経180度、北緯0度で精度最大
-    float pi_2_theta = Latitude * M_PI / 180.0f;
-    float pi_phi = ((Longitude > 0.0f) ?  (Longitude - 180.0f) : (Longitude + 180.0f)) * 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);
-    
+    float pi_2_theta = Latitude * M_PI_F / 180.0f;
+    float pi_phi = ((Longitude > 0.0f) ?  (Longitude - 180.0f) : (Longitude + 180.0f)) * M_PI_F / 180.0f;
+    float x = - std::cos(pi_2_theta) * std::cos(pi_phi);
+    float y = - std::cos(pi_2_theta) * std::sin(pi_phi);
+    float z = std::sin(pi_2_theta);
+    Vector3f v(x, y, z);
     return v;
 }
 
-Vector3 GPSUBX_UART::ToUniversal()
+Vector3f GPSUBX_UART::ToUniversal()
 {
-    Vector3 v = ToUniversalUnit();
+    Vector3f v = ToUniversalUnit();
     return (Radius + Height) * v;
 }
 
 void GPSUBX_UART::CalculateUnit()
 {
-    Vector3 _d = -1.0f * ToUniversalUnit();
+    Vector3f _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 / _d.Norm();
+    Vector3f _z(0.0f, 0.0f, 1.0f);
+    Vector3f _e = _d.cross(_z);
+    Vector3f _n = _e.cross(_d);
+    UniversalZeroUnitN = _n.normalized();
+    UniversalZeroUnitE = _e.normalized();
+    UniversalZeroUnitD = _d.normalized();
     //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);
+    Vector3f relative = UniversalPosition - UniversalZeroPosition;
+    Vector3f _position(relative.dot(UniversalZeroUnitN), relative.dot(UniversalZeroUnitE), relative.dot(UniversalZeroUnitD));
     PositionNED = _position;
 }
 
-Vector3 GPSUBX_UART::Calculate(Vector3 position)
+Vector3f GPSUBX_UART::Calculate(Vector3f position)
 {
     UniversalPosition = ToUniversal();
-    Vector3 relative = position - UniversalZeroPosition;
-    Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
+    Vector3f relative = position - UniversalZeroPosition;
+    Vector3f _position(relative.dot(UniversalZeroUnitN), relative.dot(UniversalZeroUnitE), relative.dot(UniversalZeroUnitD));
     return _position;
 }