GPS module (GYSFDMAXB) 57600 bps
Dependents: HAPS_GPS_Test_0002
Diff: GYSFDMAXB.cpp
- Revision:
- 2:9b567c8f5429
- Parent:
- 1:0d9b4ba850d8
- Child:
- 3:f8045f83d7c1
diff -r 0d9b4ba850d8 -r 9b567c8f5429 GYSFDMAXB.cpp --- 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; }