LibrePilot/flight/tests/math/coordinateconversiontest.cpp at ee292989b5b892f037e7b128bb89ce3c235c0bf0 · librepilot/LibrePilot · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
#include "gtest/gtest.h"
#define __STDC_WANT_DEC_FP__ /* Tell implementation that we want Decimal FP */

#include <stdio.h> /* printf */
#include <stdlib.h> /* abort */
#include <string.h> /* memset */
#include <stdint.h>

extern "C" {
#include <inc/CoordinateConversions.h>
}

#define epsilon_deg        0.00001f
#define epsilon_int_deg    ((int32_t)(epsilon_deg * 1e7))
#define epsilon_metric     0.2f
#define epsilon_int_metric ((int32_t)(epsilon_metric * 1e4))

// To use a test fixture, derive a class from testing::Test.
class CoordinateConversionsTestRaw : public testing::Test {};

// ****** convert Lat,Lon,Alt to ECEF  ************
// void LLA2ECEF(const int32_t LLAi[3], float ECEF[3]);

// ****** convert ECEF to Lat,Lon,Alt *********
// void ECEF2LLA(const float ECEF[3], int32_t LLA[3]);

// void RneFromLLA(const int32_t LLAi[3], float Rne[3][3]);

// ****** Express LLA in a local NED Base Frame and back ********
// void LLA2Base(const int32_t LLAi[3], const float BaseECEF[3], float Rne[3][3], float NED[3]);
// void Base2LLA(const float NED[3], const float BaseECEF[3], float Rne[3][3], int32_t LLAi[3]);

// ****** Express ECEF in a local NED Base Frame and back ********
// void ECEF2Base(const float ECEF[3], const float BaseECEF[3], float Rne[3][3], float NED[3]);
// void Base2ECEF(const float NED[3], const float BaseECEF[3], float Rne[3][3], float ECEF[3]

TEST_F(CoordinateConversionsTestRaw, LLA2ECEF) {
    int32_t LLAi[3] = {
        419291818,
        125571688,
        500000
    };
    int32_t LLAfromECEF[3];

    float ecef[3];

    LLA2ECEF(LLAi, ecef);
    ECEF2LLA(ecef, LLAfromECEF);

    EXPECT_NEAR(LLAi[0], LLAfromECEF[0], epsilon_int_deg);
    EXPECT_NEAR(LLAi[1], LLAfromECEF[1], epsilon_int_deg);
    EXPECT_NEAR(LLAi[2], LLAfromECEF[2], epsilon_int_metric);
}


TEST_F(CoordinateConversionsTestRaw, LLA2NED) {
    int32_t LLAi[3] = {
        419291818,
        125571688,
        50000
    };
    int32_t LLAfromNED[3];

    int32_t HomeLLAi[3] = {
        419291600,
        125571300,
        24000
    };

    float Rne[3][3];
    float baseECEF[3];
    float NED[3];

    RneFromLLA(HomeLLAi, Rne);
    LLA2ECEF(HomeLLAi, baseECEF);

    LLA2Base(LLAi, baseECEF, Rne, NED);
    Base2LLA(NED, baseECEF, Rne, LLAfromNED);

    EXPECT_NEAR(LLAi[0], LLAfromNED[0], epsilon_int_deg);
    EXPECT_NEAR(LLAi[1], LLAfromNED[1], epsilon_int_deg);
    EXPECT_NEAR(LLAi[2], LLAfromNED[2], epsilon_int_metric);
}