Takashi Koyama / GYSFDMAXB

GPS module (GYSFDMAXB) 57600 bps

Dependencies:   Vector3

Dependents:   HAPS_GPS_Test_0001

GYSFDMAXB GPSセンサーGYSFDMAXBのライブラリ 57600 bps

手順 [1] シリアルピンを設定 GYSFDMAXB gps(tx_pin, rx_pin); [2] 零点を設定する（NED座標系における零点） gps.Initialize(); [3] 自動的にデータを受信しては更新していくので、適宜メンバ変数を読み込んで使う

Diff: GYSFDMAXB.cpp

Revision:
2:9b567c8f5429
Parent:
1:0d9b4ba850d8
Child:
3:f8045f83d7c1
--- a/GYSFDMAXB.cpp	Thu Apr 08 08:08:53 2021 +0000
+++ b/GYSFDMAXB.cpp	Thu Apr 08 10:38:27 2021 +0000
@@ -196,23 +196,29 @@
 }
 
 
-Vector3 GYSFDMAXB::ToUniversal()
+Vector3 GYSFDMAXB::ToUniversalUnit()
 {
-    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;
+    // 東経180度、北緯0度で精度最大
+    float pi_2_theta = (Latitude * ((N_S == 'N') ? 1.0 : -1.0)) * M_PI / 180.0;
+    float pi_phi = ((E_W == 'E') ? Longitude - 180 : 180 - Longitude) * M_PI / 180.0;
+    const float pi_2 = M_PI / 2;
     float r = Radius + Elevation;
-    float x = r * sin(theta) * cos(phi);
-    float y = r * sin(theta) * sin(phi);
-    float z = r * cos(theta);
+    float x = - cos(pi_2_theta) * cos(pi_phi);
+    float y = - cos(pi_2_theta) * sin(pi_phi);
+    float z = sin(pi_2_theta);
     Vector3 v(x, y, z);
     return v;
 }
 
+Vector3 GYSFDMAXB::ToUniversal()
+{
+    Vector3 v = ToUniversalUnit();
+    return (Radius + Elevation) * v;
+}
+
 void GYSFDMAXB::CalcurateUnit()
 {
-    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 _d(-sin(theta)*cos(phi), -sin(theta)*sin(phi), -cos(theta));
+    Vector3 _d = -1.0 * ToUniversalUnit();
     
     UniversalZeroPosition = -(Radius+Elevation)*_d;
     Vector3 _z(0.0, 0.0, 1.0);
@@ -227,7 +233,7 @@
 {
     UniversalPosition = ToUniversal();
     Vector3 relative = UniversalPosition - UniversalZeroPosition;
-    Vector3 _position(relative % UniversalZeroUnitX, relative % UniversalZeroUnitY, relative % UniversalZeroUnitZ);
+    Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
     Position = _position;
 }
 
@@ -235,6 +241,6 @@
 {
     UniversalPosition = ToUniversal();
     Vector3 relative = position - UniversalZeroPosition;
-    Vector3 _position(relative % UniversalZeroUnitX, relative % UniversalZeroUnitY, relative % UniversalZeroUnitZ);
+    Vector3 _position(relative % UniversalZeroUnitN, relative % UniversalZeroUnitE, relative % UniversalZeroUnitD);
     return _position;
 }