Eigen
Dependencies: Vector3
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
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; }