CoCalc -- test_location.cpp

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/libraries/AP_Common/tests/test_location.cpp
Views: ¹⁷⁹⁹
1
#include <AP_gtest.h>
2
#include <AP_Common/Location.h>
3
#include <AP_Math/AP_Math.h>
4
#include <AP_AHRS/AP_AHRS.h>
5
#include <AP_Terrain/AP_Terrain.h>
6
#include <GCS_MAVLink/GCS_Dummy.h>
7

8
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
9

10
class DummyVehicle {
11
public:
12
    bool start_cmd(const AP_Mission::Mission_Command& cmd) { return true; };
13
    bool verify_cmd(const AP_Mission::Mission_Command& cmd) { return true; };
14
    void mission_complete() { };
15
    AP_AHRS ahrs{AP_AHRS::FLAG_ALWAYS_USE_EKF};
16

17
    AP_Mission mission{
18
        FUNCTOR_BIND_MEMBER(&DummyVehicle::start_cmd, bool, const AP_Mission::Mission_Command &),
19
        FUNCTOR_BIND_MEMBER(&DummyVehicle::verify_cmd, bool, const AP_Mission::Mission_Command &),
20
        FUNCTOR_BIND_MEMBER(&DummyVehicle::mission_complete, void)};
21
    AP_Terrain terrain;
22
};
23

24
const struct AP_Param::GroupInfo        GCS_MAVLINK_Parameters::var_info[] = {
25
    AP_GROUPEND
26
};
27
GCS_Dummy _gcs;
28

29
static DummyVehicle vehicle;
30

31
#define EXPECT_VECTOR2F_EQ(v1, v2)              \
32
do {                                        \
33
EXPECT_FLOAT_EQ(v1[0], v2[0]);          \
34
EXPECT_FLOAT_EQ(v1[1], v2[1]);          \
35
} while (false);
36

37
#define EXPECT_VECTOR3F_EQ(v1, v2)              \
38
do {                                        \
39
EXPECT_FLOAT_EQ(v1[0], v2[0]);          \
40
EXPECT_FLOAT_EQ(v1[1], v2[1]);          \
41
EXPECT_FLOAT_EQ(v1[2], v2[2]);          \
42
} while (false);
43

44
#define EXPECT_VECTOR2F_NEAR(v1, v2, acc)              \
45
do {                                        \
46
EXPECT_NEAR(v1[0], v2[0], acc);          \
47
EXPECT_NEAR(v1[1], v2[1], acc);          \
48
} while (false);
49

50
#define EXPECT_VECTOR3F_NEAR(v1, v2, acc)              \
51
do {                                        \
52
EXPECT_NEAR(v1[0], v2[0], acc);          \
53
EXPECT_NEAR(v1[1], v2[1], acc);          \
54
EXPECT_NEAR(v1[2], v2[2], acc);          \
55
} while (false);
56

57
TEST(Location, LatLngWrapping)
58
{
59
    struct {
60
        int32_t start_lat;
61
        int32_t start_lng;
62
        Vector2f delta_metres_ne;
63
        int32_t expected_lat;
64
        int32_t expected_lng;
65
    } tests[] {
66
        {519634000, 1797560000, Vector2f{0, 100000}, 519634000, -1787860777}
67
    };
68

69
    for (auto &test : tests) {
70
        // forward
71
        {
72
            Location loc{test.start_lat, test.start_lng, 0, Location::AltFrame::ABOVE_HOME};
73
            loc.offset(test.delta_metres_ne[0], test.delta_metres_ne[1]);
74
            EXPECT_EQ(test.expected_lat, loc.lat);
75
            EXPECT_EQ(test.expected_lng, loc.lng);
76
            EXPECT_EQ(0, loc.alt);
77
        }
78
        // and now reverse
79
        {
80
            Location rev{test.expected_lat, test.expected_lng, 0, Location::AltFrame::ABOVE_HOME};
81
            rev.offset(-test.delta_metres_ne[0], -test.delta_metres_ne[1]);
82
            EXPECT_EQ(rev.lat, test.start_lat);
83
            EXPECT_EQ(rev.lng, test.start_lng);
84
            EXPECT_EQ(0, rev.alt);
85
        }
86
    }
87
}
88

89
TEST(Location, LocOffsetDouble)
90
{
91
    struct {
92
        int32_t home_lat;
93
        int32_t home_lng;
94
        Vector2d delta_metres_ne1;
95
        Vector2d delta_metres_ne2;
96
        Vector2d expected_pos_change;
97
    } tests[] {
98
               -353632620, 1491652373,
99
               Vector2d{4682795.4576701336, 5953662.7673837934},
100
               Vector2d{4682797.1904749088, 5953664.1586009059},
101
               Vector2d{1.7365739,1.4261966},
102
    };
103

104
    for (auto &test : tests) {
105
        Location home{test.home_lat, test.home_lng, 0, Location::AltFrame::ABOVE_HOME};
106
        Location loc1 = home;
107
        Location loc2 = home;
108
        loc1.offset(test.delta_metres_ne1.x, test.delta_metres_ne1.y);
109
        loc2.offset(test.delta_metres_ne2.x, test.delta_metres_ne2.y);
110
        Vector2d diff = loc1.get_distance_NE_double(loc2);
111
        EXPECT_FLOAT_EQ(diff.x, test.expected_pos_change.x);
112
        EXPECT_FLOAT_EQ(diff.y, test.expected_pos_change.y);
113
    }
114
}
115

116
TEST(Location, LocOffset3DDouble)
117
{
118
    Location loc {
119
        -353632620, 1491652373, 60000, Location::AltFrame::ABSOLUTE
120
    };
121
    // this is ned, so our latitude should change, and our new
122
    // location should be above the original:
123
    loc.offset(Vector3d{1000, 0, -10});
124
    EXPECT_EQ(loc.lat, -353542788);
125
    EXPECT_EQ(loc.lng, 1491652373);
126
    EXPECT_EQ(loc.alt, 61000);
127
}
128

129
TEST(Location, Tests)
130
{
131
    Location test_location;
132
    EXPECT_TRUE(test_location.is_zero());
133
    EXPECT_FALSE(test_location.initialised());
134
    const Location test_home{-35362938, 149165085, 100, Location::AltFrame::ABSOLUTE};
135
    EXPECT_EQ(-35362938, test_home.lat);
136
    EXPECT_EQ(149165085, test_home.lng);
137
    EXPECT_EQ(100, test_home.alt);
138
    EXPECT_EQ(0, test_home.relative_alt);
139
    EXPECT_EQ(0, test_home.terrain_alt);
140
    EXPECT_EQ(0, test_home.origin_alt);
141
    EXPECT_EQ(0, test_home.loiter_ccw);
142
    EXPECT_EQ(0, test_home.loiter_xtrack);
143
    EXPECT_TRUE(test_home.initialised());
144

145
    const Vector3f test_vect{-42, 42, 0};
146
    Location test_location3{test_vect, Location::AltFrame::ABOVE_HOME};
147
    EXPECT_EQ(0, test_location3.lat);
148
    EXPECT_EQ(0, test_location3.lng);
149
    EXPECT_EQ(0, test_location3.alt);
150
    EXPECT_EQ(1, test_location3.relative_alt);
151
    EXPECT_EQ(0, test_location3.terrain_alt);
152
    EXPECT_EQ(0, test_location3.origin_alt);
153
    EXPECT_EQ(0, test_location3.loiter_ccw);
154
    EXPECT_EQ(0, test_location3.loiter_xtrack);
155
    EXPECT_FALSE(test_location3.initialised());
156
    // EXPECT_EXIT(test_location3.change_alt_frame(Location::AltFrame::ABSOLUTE), PANIC something); // TODO check PANIC
157

158
    test_location3.set_alt_cm(-420, Location::AltFrame::ABSOLUTE);
159
    EXPECT_EQ(-420, test_location3.alt);
160
    EXPECT_EQ(0, test_location3.relative_alt);
161
    EXPECT_EQ(0, test_location3.terrain_alt);
162
    EXPECT_EQ(0, test_location3.origin_alt);
163
    EXPECT_EQ(Location::AltFrame::ABSOLUTE, test_location3.get_alt_frame());
164

165
    test_location3.set_alt_cm(420, Location::AltFrame::ABOVE_HOME);
166
    EXPECT_EQ(420, test_location3.alt);
167
    EXPECT_EQ(1, test_location3.relative_alt);
168
    EXPECT_EQ(0, test_location3.terrain_alt);
169
    EXPECT_EQ(0, test_location3.origin_alt);
170
    EXPECT_EQ(Location::AltFrame::ABOVE_HOME, test_location3.get_alt_frame());
171

172
    test_location3.set_alt_cm(-420, Location::AltFrame::ABOVE_ORIGIN);
173
    EXPECT_EQ(-420, test_location3.alt);
174
    EXPECT_EQ(0, test_location3.relative_alt);
175
    EXPECT_EQ(0, test_location3.terrain_alt);
176
    EXPECT_EQ(1, test_location3.origin_alt);
177
    EXPECT_EQ(Location::AltFrame::ABOVE_ORIGIN, test_location3.get_alt_frame());
178

179
    test_location3.set_alt_cm(420, Location::AltFrame::ABOVE_TERRAIN);
180
    EXPECT_EQ(420, test_location3.alt);
181
    EXPECT_EQ(1, test_location3.relative_alt);
182
    EXPECT_EQ(1, test_location3.terrain_alt);
183
    EXPECT_EQ(0, test_location3.origin_alt);
184
    EXPECT_EQ(Location::AltFrame::ABOVE_TERRAIN, test_location3.get_alt_frame());
185

186
    // No TERRAIN, NO HOME, NO ORIGIN
187
    AP::terrain()->set_enabled(false);
188
    for (auto current_frame = Location::AltFrame::ABSOLUTE;
189
         current_frame <= Location::AltFrame::ABOVE_TERRAIN;
190
         current_frame = static_cast<Location::AltFrame>(
191
                 (uint8_t) current_frame + 1)) {
192
        for (auto desired_frame = Location::AltFrame::ABSOLUTE;
193
             desired_frame <= Location::AltFrame::ABOVE_TERRAIN;
194
             desired_frame = static_cast<Location::AltFrame>(
195
                     (uint8_t) desired_frame + 1)) {
196
            test_location3.set_alt_cm(420, current_frame);
197
            if (current_frame == desired_frame) {
198
                EXPECT_TRUE(test_location3.change_alt_frame(desired_frame));
199
                continue;
200
            }
201
            if (current_frame == Location::AltFrame::ABOVE_TERRAIN
202
                    || desired_frame == Location::AltFrame::ABOVE_TERRAIN) {
203
                EXPECT_FALSE(test_location3.change_alt_frame(desired_frame));
204
            } else if (current_frame == Location::AltFrame::ABOVE_ORIGIN
205
                    || desired_frame == Location::AltFrame::ABOVE_ORIGIN) {
206
                EXPECT_FALSE(test_location3.change_alt_frame(desired_frame));
207
            } else if (current_frame == Location::AltFrame::ABOVE_HOME
208
                    || desired_frame == Location::AltFrame::ABOVE_HOME) {
209
                EXPECT_FALSE(test_location3.change_alt_frame(desired_frame));
210
            } else {
211
                EXPECT_TRUE(test_location3.change_alt_frame(desired_frame));
212
            }
213
        }
214
    }
215
    // NO TERRAIN, NO ORIGIN
216
    EXPECT_TRUE(vehicle.ahrs.set_home(test_home));
217
    for (auto current_frame = Location::AltFrame::ABSOLUTE;
218
         current_frame <= Location::AltFrame::ABOVE_TERRAIN;
219
         current_frame = static_cast<Location::AltFrame>(
220
                 (uint8_t) current_frame + 1)) {
221
        for (auto desired_frame = Location::AltFrame::ABSOLUTE;
222
             desired_frame <= Location::AltFrame::ABOVE_TERRAIN;
223
             desired_frame = static_cast<Location::AltFrame>(
224
                     (uint8_t) desired_frame + 1)) {
225
            test_location3.set_alt_cm(420, current_frame);
226
            if (current_frame == desired_frame) {
227
                EXPECT_TRUE(test_location3.change_alt_frame(desired_frame));
228
                continue;
229
            }
230
            if (current_frame == Location::AltFrame::ABOVE_TERRAIN
231
                    || desired_frame == Location::AltFrame::ABOVE_TERRAIN) {
232
                EXPECT_FALSE(test_location3.change_alt_frame(desired_frame));
233
            } else if (current_frame == Location::AltFrame::ABOVE_ORIGIN
234
                    || desired_frame == Location::AltFrame::ABOVE_ORIGIN) {
235
                EXPECT_FALSE(test_location3.change_alt_frame(desired_frame));
236
            } else {
237
                EXPECT_TRUE(test_location3.change_alt_frame(desired_frame));
238
            }
239

240
        }
241
    }
242
    // NO Origin
243
    AP::terrain()->set_enabled(true);
244
    for (auto current_frame = Location::AltFrame::ABSOLUTE;
245
         current_frame <= Location::AltFrame::ABOVE_TERRAIN;
246
         current_frame = static_cast<Location::AltFrame>(
247
                 (uint8_t) current_frame + 1)) {
248
        for (auto desired_frame = Location::AltFrame::ABSOLUTE;
249
             desired_frame <= Location::AltFrame::ABOVE_TERRAIN;
250
             desired_frame = static_cast<Location::AltFrame>(
251
                     (uint8_t) desired_frame + 1)) {
252
            test_location3.set_alt_cm(420, current_frame);
253
            if (current_frame == desired_frame) {
254
                EXPECT_TRUE(test_location3.change_alt_frame(desired_frame));
255
                continue;
256
            }
257
            if (current_frame == Location::AltFrame::ABOVE_ORIGIN
258
                    || desired_frame == Location::AltFrame::ABOVE_ORIGIN) {
259
                EXPECT_FALSE(test_location3.change_alt_frame(desired_frame));
260
            } else {
261
                EXPECT_TRUE(test_location3.change_alt_frame(desired_frame));
262
            }
263
        }
264
    }
265

266
    Vector2f test_vec2;
267
    EXPECT_FALSE(test_home.get_vector_xy_from_origin_NE(test_vec2));
268
    Vector3f test_vec3;
269
    EXPECT_FALSE(test_home.get_vector_from_origin_NEU(test_vec3));
270

271
    Location test_origin = test_home;
272
    test_origin.offset(2, 2);
273
    const Vector3f test_vecto{200, 200, 10};
274
    const Location test_location4{test_vecto, Location::AltFrame::ABOVE_ORIGIN};
275
    EXPECT_EQ(10, test_location4.alt);
276
    EXPECT_EQ(0, test_location4.relative_alt);
277
    EXPECT_EQ(0, test_location4.terrain_alt);
278
    EXPECT_EQ(1, test_location4.origin_alt);
279
    EXPECT_EQ(0, test_location4.loiter_ccw);
280
    EXPECT_EQ(0, test_location4.loiter_xtrack);
281
    EXPECT_TRUE(test_location4.initialised());
282

283
    // test set_alt_m API:
284
    Location loc = test_home;
285
    loc.set_alt_m(1.71, Location::AltFrame::ABSOLUTE);
286
    int32_t alt_in_cm_from_m;
287
    EXPECT_TRUE(loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_in_cm_from_m));
288
    EXPECT_EQ(171, alt_in_cm_from_m);
289

290
    // can't create a Location using a vector here as there's no origin for the vector to be relative to:
291
    // const Location test_location_empty{test_vect, Location::AltFrame::ABOVE_HOME};
292
    // EXPECT_FALSE(test_location_empty.get_vector_from_origin_NEU(test_vec3));
293
}
294

295
TEST(Location, Distance)
296
{
297
    const Location test_home{-35362938, 149165085, 100, Location::AltFrame::ABSOLUTE};
298
    const Location test_home2{-35363938, 149165085, 100, Location::AltFrame::ABSOLUTE};
299
    EXPECT_FLOAT_EQ(11.131885, test_home.get_distance(test_home2));
300
    EXPECT_FLOAT_EQ(0, test_home.get_distance(test_home));
301
    EXPECT_VECTOR2F_EQ(Vector2f(0, 0), test_home.get_distance_NE(test_home));
302
    EXPECT_VECTOR2F_EQ(Vector2f(-11.131885, 0), test_home.get_distance_NE(test_home2));
303
    EXPECT_VECTOR2F_EQ(Vector3f(0, 0, 0), test_home.get_distance_NED(test_home));
304
    EXPECT_VECTOR2F_EQ(Vector3f(-11.131885, 0, 0), test_home.get_distance_NED(test_home2));
305
    Location test_loc = test_home;
306
    test_loc.offset(-11.131886, 0);
307
    EXPECT_TRUE(test_loc.same_latlon_as(test_home2));
308
    test_loc = test_home;
309
    test_loc.offset(-11.131885, 0);
310
    test_loc.offset_bearing(0, 11.131885);
311
    EXPECT_TRUE(test_loc.same_latlon_as(test_home));
312

313
    test_loc.offset_bearing_and_pitch(0, 2, -11.14);
314
    EXPECT_TRUE(test_loc.same_latlon_as(test_home2));
315
    EXPECT_EQ(62, test_loc.alt);
316

317
    test_loc = Location(-35362633, 149165085, 0, Location::AltFrame::ABOVE_HOME);
318
    int32_t bearing = test_home.get_bearing_to(test_loc);
319
    EXPECT_EQ(0, bearing);
320

321
    test_loc = Location(-35363711, 149165085, 0, Location::AltFrame::ABOVE_HOME);
322
    bearing = test_home.get_bearing_to(test_loc);
323
    EXPECT_EQ(18000, bearing);
324

325
    test_loc = Location(-35362938, 149166085, 0, Location::AltFrame::ABOVE_HOME);
326
    bearing = test_home.get_bearing_to(test_loc);
327
    EXPECT_EQ(9000, bearing);
328

329
    test_loc = Location(-35362938, 149164085, 0, Location::AltFrame::ABOVE_HOME);
330
    bearing = test_home.get_bearing_to(test_loc);
331
    EXPECT_EQ(27000, bearing);
332

333
    test_loc = Location(-35361938, 149164085, 0, Location::AltFrame::ABOVE_HOME);
334
    bearing = test_home.get_bearing_to(test_loc);
335
    EXPECT_EQ(31503, bearing);
336
    const float bearing_rad = test_home.get_bearing(test_loc);
337
    EXPECT_FLOAT_EQ(5.4982867, bearing_rad);
338

339
}
340

341
TEST(Location, Sanitize)
342
{
343
    // we will sanitize test_loc with test_default_loc
344
    // test_home is just for reference
345
    const Location test_home{-35362938, 149165085, 100, Location::AltFrame::ABSOLUTE};
346
    EXPECT_TRUE(vehicle.ahrs.set_home(test_home));
347
    const Location test_default_loc{-35362938, 149165085, 200, Location::AltFrame::ABSOLUTE};
348
    Location test_loc;
349
    test_loc.set_alt_cm(0, Location::AltFrame::ABOVE_HOME);
350
    EXPECT_TRUE(test_loc.sanitize(test_default_loc));
351
    EXPECT_TRUE(test_loc.same_latlon_as(test_default_loc));
352
    int32_t default_loc_alt;
353
    // we should compare test_loc alt and test_default_loc alt in same frame , in this case, ABOVE HOME
354
    EXPECT_TRUE(test_default_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, default_loc_alt));
355
    EXPECT_EQ(test_loc.alt, default_loc_alt);
356
    test_loc = Location(91*1e7, 0, 0, Location::AltFrame::ABSOLUTE);
357
    EXPECT_TRUE(test_loc.sanitize(test_default_loc));
358
    EXPECT_TRUE(test_loc.same_latlon_as(test_default_loc));
359
    EXPECT_NE(test_default_loc.alt, test_loc.alt);
360
    test_loc = Location(0, 181*1e7, 0, Location::AltFrame::ABSOLUTE);
361
    EXPECT_TRUE(test_loc.sanitize(test_default_loc));
362
    EXPECT_TRUE(test_loc.same_latlon_as(test_default_loc));
363
    EXPECT_NE(test_default_loc.alt, test_loc.alt);
364
    test_loc = Location(42*1e7, 42*1e7, 420, Location::AltFrame::ABSOLUTE);
365
    EXPECT_FALSE(test_loc.sanitize(test_default_loc));
366
    EXPECT_FALSE(test_loc.same_latlon_as(test_default_loc));
367
    EXPECT_NE(test_default_loc.alt, test_loc.alt);
368
}
369

370
TEST(Location, Line)
371
{
372
    const Location test_home{35362938, 149165085, 100, Location::AltFrame::ABSOLUTE};
373
    const Location test_wp_last{35362960, 149165085, 100, Location::AltFrame::ABSOLUTE};
374
    Location test_wp{35362940, 149165085, 100, Location::AltFrame::ABSOLUTE};
375
    EXPECT_FALSE(test_wp.past_interval_finish_line(test_home, test_wp_last));
376
    EXPECT_TRUE(test_wp.past_interval_finish_line(test_home, test_home));
377
    test_wp.lat = 35362970;
378
    EXPECT_TRUE(test_wp.past_interval_finish_line(test_home, test_wp_last));
379
}
380

381
/*
382
  check if we obey basic euclidean geometry rules of position
383
  addition/subtraction
384
 */
385
TEST(Location, OffsetError)
386
{
387
    // test at 10km from origin
388
    const float ofs_ne = 10e3 / sqrtf(2.0);
389
    for (float lat = -80; lat <= 80; lat += 10.0) {
390
        Location origin{int32_t(lat*1e7), 0, 0, Location::AltFrame::ABOVE_HOME};
391
        Location loc = origin;
392
        loc.offset(ofs_ne, ofs_ne);
393
        Location loc2 = loc;
394
        loc2.offset(-ofs_ne, -ofs_ne);
395
        float dist = origin.get_distance(loc2);
396
        EXPECT_FLOAT_EQ(dist, 0);
397
    }
398
}
399

400
AP_GTEST_MAIN()
401

402
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

Product

Resources

Company

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more, all in one place. Commercial Alternative to JupyterHub.

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
