flight/tests/math/coordinateconversiontest.cpp

   1 #include "gtest/gtest.h"
   2 #define __STDC_WANT_DEC_FP__ /* Tell implementation that we want Decimal FP */
   3
   4 #include <stdio.h> /* printf */
   5 #include <stdlib.h> /* abort */
   6 #include <string.h> /* memset */
   7
   8 extern "C" {
   9 #include <inc/CoordinateConversions.h>
  10 }
  11
  12 #define epsilon_deg        0.00001f
  13 #define epsilon_int_deg    ((int32_t)(epsilon_deg * 1e7))
  14 #define epsilon_metric     0.2f
  15 #define epsilon_int_metric ((int32_t)(epsilon_metric * 1e4))
  16
  17 // To use a test fixture, derive a class from testing::Test.
  18 class CoordinateConversionsTestRaw : public testing::Test {};
  19
  20 // ****** convert Lat,Lon,Alt to ECEF  ************
  21 // void LLA2ECEF(const int32_t LLAi[3], float ECEF[3]);
  22
  23 // ****** convert ECEF to Lat,Lon,Alt *********
  24 // void ECEF2LLA(const float ECEF[3], int32_t LLA[3]);
  25
  26 // void RneFromLLA(const int32_t LLAi[3], float Rne[3][3]);
  27
  28 // ****** Express LLA in a local NED Base Frame and back ********
  29 // void LLA2Base(const int32_t LLAi[3], const float BaseECEF[3], float Rne[3][3], float NED[3]);
  30 // void Base2LLA(const float NED[3], const float BaseECEF[3], float Rne[3][3], int32_t LLAi[3]);
  31
  32 // ****** Express ECEF in a local NED Base Frame and back ********
  33 // void ECEF2Base(const float ECEF[3], const float BaseECEF[3], float Rne[3][3], float NED[3]);
  34 // void Base2ECEF(const float NED[3], const float BaseECEF[3], float Rne[3][3], float ECEF[3]
  35
  36 TEST_F(CoordinateConversionsTestRaw, LLA2ECEF) {
  37     int32_t LLAi[3] = {
  38         419291818,
  39         125571688,
  40         50 * 1e4
  41     };
  42     int32_t LLAfromECEF[3];
  43
  44     float ecef[3];
  45
  46     LLA2ECEF(LLAi, ecef);
  47     ECEF2LLA(ecef, LLAfromECEF);
  48
  49     EXPECT_NEAR(LLAi[0], LLAfromECEF[0], epsilon_int_deg);
  50     EXPECT_NEAR(LLAi[1], LLAfromECEF[1], epsilon_int_deg);
  51     EXPECT_NEAR(LLAi[2], LLAfromECEF[2], epsilon_int_metric);
  52 }
  53
  54
  55 TEST_F(CoordinateConversionsTestRaw, LLA2NED) {
  56     int32_t LLAi[3] = {
  57         419291818,
  58         125571688,
  59         50 * 1e4
  60     };
  61     int32_t LLAfromNED[3];
  62
  63     int32_t HomeLLAi[3] = {
  64         419291600,
  65         125571300,
  66         24 * 1e4
  67     };
  68
  69     float Rne[3][3];
  70     float baseECEF[3];
  71     float NED[3];
  72
  73     RneFromLLA(HomeLLAi, Rne);
  74     LLA2ECEF(HomeLLAi, baseECEF);
  75
  76     LLA2Base(LLAi, baseECEF, Rne, NED);
  77     Base2LLA(NED, baseECEF, Rne, LLAfromNED);
  78
  79     EXPECT_NEAR(LLAi[0], LLAfromNED[0], epsilon_int_deg);
  80     EXPECT_NEAR(LLAi[1], LLAfromNED[1], epsilon_int_deg);
  81     EXPECT_NEAR(LLAi[2], LLAfromNED[2], epsilon_int_metric);
  82 }