Eigen
Dependencies: Vector3
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: GPSUBX_UART.cpp
- Revision:
- 19:ae1a294cb30b
- Parent:
- 15:e77382079cd9
--- 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; -}