diff options
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/estimator_utilities.cpp')
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_utilities.cpp | 21 |
1 files changed, 21 insertions, 0 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 83a738463..284a09902 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -114,6 +114,27 @@ Mat3f Mat3f::transpose() const return ret; } +void calcvelNED(float (&velNEDr)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) +{ + velNEDr[0] = gpsGndSpd*cosf(gpsCourse); + velNEDr[1] = gpsGndSpd*sinf(gpsCourse); + velNEDr[2] = gpsVelD; +} + +void calcposNED(float (&posNEDr)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference) +{ + posNEDr[0] = earthRadius * (lat - latReference); + posNEDr[1] = earthRadius * cos(latReference) * (lon - lonReference); + posNEDr[2] = -(hgt - hgtReference); +} + +void calcLLH(float posNEDi[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef) +{ + lat = latRef + (double)posNEDi[0] * earthRadiusInv; + lon = lonRef + (double)posNEDi[1] * earthRadiusInv / cos(latRef); + hgt = hgtRef - posNEDi[2]; +} + // overload + operator to provide a vector addition Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2) { |