Line data Source code
1 : /*
2 : * Location.cpp
3 : */
4 :
5 : #include "Location.h"
6 :
7 : #ifndef HAL_BOOTLOADER_BUILD
8 :
9 : #include <AP_AHRS/AP_AHRS.h>
10 : #include <AP_Terrain/AP_Terrain.h>
11 :
12 : const Location definitely_zero{};
13 83981569 : bool Location::is_zero(void) const
14 : {
15 83981569 : return !memcmp(this, &definitely_zero, sizeof(*this));
16 : }
17 :
18 2250112989 : void Location::zero(void)
19 : {
20 2250112989 : memset(this, 0, sizeof(*this));
21 2250112989 : }
22 :
23 : // Construct location using position (NEU) from ekf_origin for the given altitude frame
24 126010199 : Location::Location(int32_t latitude, int32_t longitude, int32_t alt_in_cm, AltFrame frame)
25 : {
26 : // make sure we know what size the Location object is:
27 : ASSERT_STORAGE_SIZE(Location, 16);
28 :
29 126010199 : zero();
30 126010199 : lat = latitude;
31 126010199 : lng = longitude;
32 126010199 : set_alt_cm(alt_in_cm, frame);
33 126010199 : }
34 :
35 : #if AP_AHRS_ENABLED
36 3781 : Location::Location(const Vector3f &ekf_offset_neu_cm, AltFrame frame)
37 : {
38 3781 : zero();
39 :
40 : // store alt and alt frame
41 3781 : set_alt_cm(ekf_offset_neu_cm.z, frame);
42 :
43 : // calculate lat, lon
44 3781 : Location ekf_origin;
45 3781 : if (AP::ahrs().get_origin(ekf_origin)) {
46 3777 : lat = ekf_origin.lat;
47 3777 : lng = ekf_origin.lng;
48 3777 : offset(ekf_offset_neu_cm.x * 0.01, ekf_offset_neu_cm.y * 0.01);
49 : }
50 3781 : }
51 :
52 10408135 : Location::Location(const Vector3d &ekf_offset_neu_cm, AltFrame frame)
53 : {
54 10408135 : zero();
55 :
56 : // store alt and alt frame
57 10408135 : set_alt_cm(ekf_offset_neu_cm.z, frame);
58 :
59 : // calculate lat, lon
60 10408135 : Location ekf_origin;
61 10408135 : if (AP::ahrs().get_origin(ekf_origin)) {
62 10408135 : lat = ekf_origin.lat;
63 10408135 : lng = ekf_origin.lng;
64 10408135 : offset(ekf_offset_neu_cm.x * 0.01, ekf_offset_neu_cm.y * 0.01);
65 : }
66 10408135 : }
67 :
68 : // named constructors
69 0 : Location Location::from_ekf_offset_NED_m(const Vector3f& ekf_offset_ned_m, AltFrame frame)
70 : {
71 0 : const Vector3f ekf_offset_neu_cm(ekf_offset_ned_m.x * 100.0, ekf_offset_ned_m.y * 100.0, -ekf_offset_ned_m.z * 100.0);
72 0 : return Location(ekf_offset_neu_cm, frame);
73 : }
74 :
75 10408135 : Location Location::from_ekf_offset_NED_m(const Vector3d& ekf_offset_ned_m, AltFrame frame)
76 : {
77 10408135 : const Vector3d ekf_offset_neu_cm(ekf_offset_ned_m.x * 100.0, ekf_offset_ned_m.y * 100.0, -ekf_offset_ned_m.z * 100.0);
78 10408135 : return Location(ekf_offset_neu_cm, frame);
79 : }
80 :
81 : #endif // AP_AHRS_ENABLED
82 :
83 266589086 : void Location::set_alt_cm(int32_t alt_cm, AltFrame frame)
84 : {
85 266589086 : alt = alt_cm;
86 266589086 : relative_alt = false;
87 266589086 : terrain_alt = false;
88 266589086 : origin_alt = false;
89 266589086 : switch (frame) {
90 196983084 : case AltFrame::ABSOLUTE:
91 : // do nothing
92 196983084 : break;
93 28737970 : case AltFrame::ABOVE_HOME:
94 28737970 : relative_alt = true;
95 28737970 : break;
96 40804065 : case AltFrame::ABOVE_ORIGIN:
97 40804065 : origin_alt = true;
98 40804065 : break;
99 63960 : case AltFrame::ABOVE_TERRAIN:
100 : // we mark it as a relative altitude, as it doesn't have
101 : // home alt added
102 63960 : relative_alt = true;
103 63960 : terrain_alt = true;
104 63960 : break;
105 : }
106 266589086 : }
107 :
108 : // converts altitude to new frame
109 27631744 : bool Location::change_alt_frame(AltFrame desired_frame)
110 : {
111 : int32_t new_alt_cm;
112 27631744 : if (!get_alt_cm(desired_frame, new_alt_cm)) {
113 185871 : return false;
114 : }
115 27445873 : set_alt_cm(new_alt_cm, desired_frame);
116 27445873 : return true;
117 : }
118 :
119 186798 : void Location::copy_alt_from(const Location &other)
120 : {
121 186798 : alt = other.alt;
122 186798 : relative_alt = other.relative_alt;
123 186798 : terrain_alt = other.terrain_alt;
124 186798 : origin_alt = other.origin_alt;
125 186798 : }
126 :
127 : // get altitude frame
128 49251038 : Location::AltFrame Location::get_alt_frame() const
129 : {
130 49251038 : if (terrain_alt) {
131 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
132 421942 : if (!relative_alt) {
133 0 : AP_HAL::panic("terrain loc must be relative_alt1");
134 : }
135 : #endif
136 421955 : return AltFrame::ABOVE_TERRAIN;
137 : }
138 48829083 : if (origin_alt) {
139 27558144 : return AltFrame::ABOVE_ORIGIN;
140 : }
141 21270939 : if (relative_alt) {
142 8800376 : return AltFrame::ABOVE_HOME;
143 : }
144 12470563 : return AltFrame::ABSOLUTE;
145 : }
146 :
147 : /// get altitude in desired frame. Must not change ret_alt_cm unless true is returned!
148 47652619 : bool Location::get_alt_cm(AltFrame desired_frame, int32_t &ret_alt_cm) const
149 : {
150 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
151 47652565 : if (!initialised()) {
152 0 : AP_HAL::panic("Should not be called on invalid location: Location cannot be (0, 0, 0)");
153 : }
154 47652565 : if (terrain_alt && !relative_alt) {
155 0 : AP_HAL::panic("terrain loc must be relative_alt2");
156 : }
157 : #endif
158 47652619 : Location::AltFrame frame = get_alt_frame();
159 :
160 : // shortcut if desired and underlying frame are the same
161 47652616 : if (desired_frame == frame) {
162 11077831 : ret_alt_cm = alt;
163 11077831 : return true;
164 : }
165 :
166 : // check for terrain altitude
167 36574785 : float alt_terr_cm = 0;
168 36574785 : if (frame == AltFrame::ABOVE_TERRAIN || desired_frame == AltFrame::ABOVE_TERRAIN) {
169 : #if AP_TERRAIN_AVAILABLE
170 8718139 : AP_Terrain *terrain = AP::terrain();
171 8718143 : if (terrain == nullptr) {
172 0 : return false;
173 : }
174 8718143 : if (!terrain->height_amsl(*this, alt_terr_cm)) {
175 684795 : return false;
176 : }
177 : // convert terrain alt to cm
178 8033348 : alt_terr_cm *= 100.0f;
179 : #else
180 : return false;
181 : #endif
182 : }
183 :
184 : // convert alt to absolute
185 35889994 : int32_t alt_abs = 0;
186 35889994 : switch (frame) {
187 311742 : case AltFrame::ABSOLUTE:
188 311742 : alt_abs = alt;
189 311742 : break;
190 7708228 : case AltFrame::ABOVE_HOME:
191 : #if AP_AHRS_ENABLED
192 7708228 : if (!AP::ahrs().home_is_set()) {
193 418 : return false;
194 : }
195 7707810 : alt_abs = alt + AP::ahrs().get_home().alt;
196 : #else
197 : return false;
198 : #endif // AP_AHRS_ENABLED
199 7707810 : break;
200 27558132 : case AltFrame::ABOVE_ORIGIN:
201 : #if AP_AHRS_ENABLED
202 : {
203 : // fail if we cannot get ekf origin
204 27558132 : Location ekf_origin;
205 27558132 : if (!AP::ahrs().get_origin(ekf_origin)) {
206 185819 : return false;
207 : }
208 27372313 : alt_abs = alt + ekf_origin.alt;
209 : }
210 27372313 : break;
211 : #else
212 : return false;
213 : #endif // AP_AHRS_ENABLED
214 311892 : case AltFrame::ABOVE_TERRAIN:
215 311892 : alt_abs = alt + alt_terr_cm;
216 311892 : break;
217 : }
218 :
219 : // convert absolute to desired frame
220 35703757 : switch (desired_frame) {
221 913922 : case AltFrame::ABSOLUTE:
222 913922 : ret_alt_cm = alt_abs;
223 913922 : return true;
224 26583423 : case AltFrame::ABOVE_HOME:
225 : #if AP_AHRS_ENABLED
226 26583423 : if (!AP::ahrs().home_is_set()) {
227 257 : return false;
228 : }
229 26583166 : ret_alt_cm = alt_abs - AP::ahrs().get_home().alt;
230 : #else
231 : return false;
232 : #endif // AP_AHRS_ENABLED
233 26583165 : return true;
234 485080 : case AltFrame::ABOVE_ORIGIN:
235 : #if AP_AHRS_ENABLED
236 : {
237 : // fail if we cannot get ekf origin
238 485080 : Location ekf_origin;
239 485080 : if (!AP::ahrs().get_origin(ekf_origin)) {
240 3602 : return false;
241 : }
242 481478 : ret_alt_cm = alt_abs - ekf_origin.alt;
243 481478 : return true;
244 : }
245 : #else
246 : return false;
247 : #endif // AP_AHRS_ENABLED
248 7721332 : case AltFrame::ABOVE_TERRAIN:
249 7721332 : ret_alt_cm = alt_abs - alt_terr_cm;
250 7721332 : return true;
251 : }
252 : return false; // LCOV_EXCL_LINE - not reachable
253 : }
254 : // Must not change ret_alt_cm unless true is returned!
255 13586334 : bool Location::get_alt_m(AltFrame desired_frame, float &ret_alt) const
256 : {
257 : int32_t ret_alt_cm;
258 13586334 : if (!get_alt_cm(desired_frame, ret_alt_cm)) {
259 685155 : return false;
260 : }
261 12901179 : ret_alt = ret_alt_cm * 0.01;
262 12901179 : return true;
263 : }
264 :
265 : #if AP_AHRS_ENABLED
266 : // converts location to a vector from origin; if this method returns
267 : // false then vec_ne is unmodified
268 : template<typename T>
269 271054938 : bool Location::get_vector_xy_from_origin_NE_cm(T &vec_ne) const
270 : {
271 271054938 : Location ekf_origin;
272 271054937 : if (!AP::ahrs().get_origin(ekf_origin)) {
273 447959 : return false;
274 : }
275 270606978 : vec_ne.x = (lat-ekf_origin.lat) * LATLON_TO_CM;
276 270606978 : vec_ne.y = diff_longitude(lng,ekf_origin.lng) * LATLON_TO_CM * longitude_scale((lat+ekf_origin.lat)/2);
277 270606975 : return true;
278 : }
279 :
280 : // define for float and position vectors
281 : template bool Location::get_vector_xy_from_origin_NE_cm<Vector2f>(Vector2f &vec_ne) const;
282 : #if HAL_WITH_POSTYPE_DOUBLE
283 : template bool Location::get_vector_xy_from_origin_NE_cm<Vector2p>(Vector2p &vec_ne) const;
284 : #endif
285 :
286 : // converts location to a vector from origin; if this method returns
287 : // false then vec_neu is unmodified
288 : template<typename T>
289 25949 : bool Location::get_vector_from_origin_NEU_cm(T &vec_neu) const
290 : {
291 : // convert altitude
292 25949 : int32_t alt_above_origin_cm = 0;
293 25949 : if (!get_alt_cm(AltFrame::ABOVE_ORIGIN, alt_above_origin_cm)) {
294 823 : return false;
295 : }
296 :
297 : // convert lat, lon
298 25126 : if (!get_vector_xy_from_origin_NE_cm(vec_neu.xy())) {
299 0 : return false;
300 : }
301 :
302 25126 : vec_neu.z = alt_above_origin_cm;
303 :
304 25126 : return true;
305 : }
306 : template<typename T>
307 0 : bool Location::get_vector_from_origin_NEU(T &vec_neu) const
308 : {
309 0 : return get_vector_from_origin_NEU_cm(vec_neu);
310 : }
311 :
312 : // define for float and position vectors
313 : template bool Location::get_vector_from_origin_NEU_cm<Vector3f>(Vector3f &vec_neu) const;
314 : template bool Location::get_vector_from_origin_NEU<Vector3f>(Vector3f &vec_neu) const;
315 : #if HAL_WITH_POSTYPE_DOUBLE
316 : template bool Location::get_vector_from_origin_NEU_cm<Vector3p>(Vector3p &vec_neu) const;
317 : template bool Location::get_vector_from_origin_NEU<Vector3p>(Vector3p &vec_neu) const;
318 : #endif
319 :
320 : template<typename T>
321 1793 : bool Location::get_vector_xy_from_origin_NE_m(T &vec_ne) const
322 : {
323 1793 : if (!get_vector_xy_from_origin_NE_cm(vec_ne)) {
324 8 : return false;
325 : }
326 1785 : vec_ne *= 0.01;
327 1785 : return true;
328 : }
329 : template bool Location::get_vector_xy_from_origin_NE_m<Vector2f>(Vector2f &vec_ne) const;
330 : #if HAL_WITH_POSTYPE_DOUBLE
331 : template bool Location::get_vector_xy_from_origin_NE_m<Vector2p>(Vector2p &vec_ne) const;
332 : #endif
333 :
334 : template<typename T>
335 21 : bool Location::get_vector_from_origin_NED_m(T &vec_ned) const
336 : {
337 21 : if (!get_vector_from_origin_NEU_cm(vec_ned)) {
338 0 : return false;
339 : }
340 21 : vec_ned *= 0.01;
341 21 : vec_ned.z *= -1.0;
342 21 : return true;
343 : }
344 : // define for float and position vectors
345 : template bool Location::get_vector_from_origin_NED_m<Vector3f>(Vector3f &vec_ned) const;
346 : #if HAL_WITH_POSTYPE_DOUBLE
347 : template bool Location::get_vector_from_origin_NED_m<Vector3p>(Vector3p &vec_ned) const;
348 : #endif
349 :
350 : template<typename T>
351 919 : bool Location::get_vector_from_origin_NEU_m(T &vec_neu) const
352 : {
353 919 : if (!get_vector_from_origin_NEU_cm(vec_neu)) {
354 107 : return false;
355 : }
356 812 : vec_neu *= 0.01;
357 812 : return true;
358 : }
359 : // define for float and position vectors
360 : template bool Location::get_vector_from_origin_NEU_m<Vector3f>(Vector3f &vec_neu) const;
361 : #if HAL_WITH_POSTYPE_DOUBLE
362 : template bool Location::get_vector_from_origin_NEU_m<Vector3p>(Vector3p &vec_neu) const;
363 : #endif
364 :
365 :
366 : #endif // AP_AHRS_ENABLED
367 :
368 : // return horizontal distance in meters between two locations
369 3708376 : ftype Location::get_distance(const Location &loc2) const
370 : {
371 3708376 : ftype dlat = (ftype)(loc2.lat - lat);
372 3708376 : ftype dlng = ((ftype)diff_longitude(loc2.lng,lng)) * longitude_scale((lat+loc2.lat)/2);
373 3708375 : return norm(dlat, dlng) * LOCATION_SCALING_FACTOR;
374 : }
375 :
376 : /**
377 : * return the altitude difference in meters taking into account alt
378 : * frame. if loc2 is below this location then "distance" will be
379 : * positive. ie. this method returns how far above loc2 this location
380 : * is.
381 : *
382 : * @param loc2 [in] location to compare against
383 : * @param distance [out] the distance in meters between this location and loc2
384 : *
385 : * @return true if the distance was calculated successfully,
386 : * false if the altitude could not be resolved
387 : *
388 : */
389 138098 : bool Location::get_height_above(const Location &loc2, ftype &distance) const
390 : {
391 138098 : if (get_alt_frame() == loc2.get_alt_frame()) {
392 137512 : switch (get_alt_frame()) {
393 137438 : case AltFrame::ABSOLUTE:
394 : case AltFrame::ABOVE_HOME:
395 : case AltFrame::ABOVE_ORIGIN:
396 : // all of these use the same reference
397 137438 : distance = (alt - loc2.alt) * 0.01;
398 137438 : return true;
399 74 : case AltFrame::ABOVE_TERRAIN:
400 : // 1m above terrain here is not the same as 1m above
401 : // terrain at loc2, so convert both to absolute and then
402 : // subtract.
403 74 : break;
404 : }
405 : }
406 :
407 : int32_t alt1, alt2;
408 660 : if (!get_alt_cm(AltFrame::ABSOLUTE, alt1) || !loc2.get_alt_cm(AltFrame::ABSOLUTE, alt2)) {
409 19 : return false;
410 : }
411 641 : distance = (alt1 - alt2) * 0.01;
412 641 : return true;
413 : }
414 :
415 : /*
416 : return the distance in meters in North/East plane as a N/E vector
417 : from loc1 to loc2
418 : */
419 1018334479 : Vector2f Location::get_distance_NE(const Location &loc2) const
420 : {
421 2036668850 : return Vector2f((loc2.lat - lat) * LOCATION_SCALING_FACTOR,
422 1018334479 : diff_longitude(loc2.lng,lng) * LOCATION_SCALING_FACTOR * longitude_scale((loc2.lat+lat)/2));
423 : }
424 :
425 : // return the distance in meters in North/East/Down plane as a N/E/D vector to loc2, NOT CONSIDERING ALT FRAME!
426 246454 : Vector3f Location::get_distance_NED(const Location &loc2) const
427 : {
428 739362 : return Vector3f((loc2.lat - lat) * LOCATION_SCALING_FACTOR,
429 246454 : diff_longitude(loc2.lng,lng) * LOCATION_SCALING_FACTOR * longitude_scale((lat+loc2.lat)/2),
430 492908 : (alt - loc2.alt) * 0.01);
431 : }
432 :
433 36707525 : Vector3p Location::get_distance_NED_postype(const Location &loc2) const
434 : {
435 36707525 : return Vector3p((loc2.lat - lat) * LOCATION_SCALING_FACTOR,
436 36707525 : diff_longitude(loc2.lng,lng) * LOCATION_SCALING_FACTOR * longitude_scale((lat+loc2.lat)/2),
437 73415050 : (alt - loc2.alt) * 0.01);
438 : }
439 :
440 : // return the distance in meters in North/East/Down plane as a N/E/D vector to loc2
441 167668581 : Vector3d Location::get_distance_NED_double(const Location &loc2) const
442 : {
443 167668581 : return Vector3d((loc2.lat - lat) * double(LOCATION_SCALING_FACTOR),
444 167668581 : diff_longitude(loc2.lng,lng) * LOCATION_SCALING_FACTOR * longitude_scale((lat+loc2.lat)/2),
445 335337162 : (alt - loc2.alt) * 0.01);
446 : }
447 :
448 : // return the distance in meters in North/East/Down plane as a N/E/D vector to loc2 considering alt frame, if altitude cannot be resolved down distance is 0
449 56 : Vector3f Location::get_distance_NED_alt_frame(const Location &loc2) const
450 : {
451 56 : int32_t alt1, alt2 = 0;
452 56 : if (!get_alt_cm(AltFrame::ABSOLUTE, alt1) || !loc2.get_alt_cm(AltFrame::ABSOLUTE, alt2)) {
453 : // one or both of the altitudes are invalid, don't do alt distance calc
454 0 : alt1 = 0, alt2 = 0;
455 : }
456 168 : return Vector3f((loc2.lat - lat) * LOCATION_SCALING_FACTOR,
457 56 : diff_longitude(loc2.lng,lng) * LOCATION_SCALING_FACTOR * longitude_scale((loc2.lat+lat)/2),
458 112 : (alt1 - alt2) * 0.01);
459 : }
460 :
461 399082419 : Vector2d Location::get_distance_NE_double(const Location &loc2) const
462 : {
463 399082419 : return Vector2d((loc2.lat - lat) * double(LOCATION_SCALING_FACTOR),
464 798164838 : diff_longitude(loc2.lng,lng) * double(LOCATION_SCALING_FACTOR) * longitude_scale((lat+loc2.lat)/2));
465 : }
466 :
467 82249557 : Vector2p Location::get_distance_NE_postype(const Location &loc2) const
468 : {
469 82249557 : return Vector2p((loc2.lat - lat) * double(LOCATION_SCALING_FACTOR),
470 164499114 : diff_longitude(loc2.lng,lng) * double(LOCATION_SCALING_FACTOR) * longitude_scale((lat+loc2.lat)/2));
471 : }
472 :
473 1641296 : Vector2F Location::get_distance_NE_ftype(const Location &loc2) const
474 : {
475 1641296 : return Vector2F((loc2.lat - lat) * ftype(LOCATION_SCALING_FACTOR),
476 3282592 : diff_longitude(loc2.lng,lng) * ftype(LOCATION_SCALING_FACTOR) * longitude_scale((lat+loc2.lat)/2));
477 : }
478 :
479 : // extrapolate latitude/longitude given distances (in meters) north and east
480 1761066090 : void Location::offset_latlng(int32_t &lat, int32_t &lng, ftype ofs_north, ftype ofs_east)
481 : {
482 1761066090 : const int32_t dlat = ofs_north * LOCATION_SCALING_FACTOR_INV;
483 1761066090 : const int64_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale(lat+dlat/2);
484 1761064822 : lat += dlat;
485 1761064822 : lat = limit_lattitude(lat);
486 1761064399 : lng = wrap_longitude(dlng+lng);
487 1761064211 : }
488 :
489 : // extrapolate latitude/longitude given distances (in meters) north and east
490 1760996336 : void Location::offset(ftype ofs_north, ftype ofs_east)
491 : {
492 1760996336 : offset_latlng(lat, lng, ofs_north, ofs_east);
493 1760994816 : }
494 :
495 : // extrapolate latitude/longitude given distances (in meters) north
496 : // and east. Note that this is metres, *even for the altitude*.
497 64227 : void Location::offset(const Vector3p &ofs_ned)
498 : {
499 64227 : offset_latlng(lat, lng, ofs_ned.x, ofs_ned.y);
500 64227 : alt += -ofs_ned.z * 100; // m -> cm
501 64227 : }
502 :
503 : /*
504 : * extrapolate latitude/longitude given bearing and distance
505 : * Note that this function is accurate to about 1mm at a distance of
506 : * 100m. This function has the advantage that it works in relative
507 : * positions, so it keeps the accuracy even when dealing with small
508 : * distances and floating point numbers
509 : */
510 5416995 : void Location::offset_bearing(ftype bearing_deg, ftype distance)
511 : {
512 5416995 : const ftype ofs_north = cosF(radians(bearing_deg)) * distance;
513 5416993 : const ftype ofs_east = sinF(radians(bearing_deg)) * distance;
514 5416993 : offset(ofs_north, ofs_east);
515 5416984 : }
516 :
517 : // extrapolate latitude/longitude given bearing, pitch and distance
518 662 : void Location::offset_bearing_and_pitch(ftype bearing_deg, ftype pitch_deg, ftype distance)
519 : {
520 662 : const ftype ofs_north = cosF(radians(pitch_deg)) * cosF(radians(bearing_deg)) * distance;
521 662 : const ftype ofs_east = cosF(radians(pitch_deg)) * sinF(radians(bearing_deg)) * distance;
522 662 : offset(ofs_north, ofs_east);
523 662 : const int32_t dalt = sinF(radians(pitch_deg)) * distance *100.0f;
524 662 : alt += dalt;
525 662 : }
526 :
527 :
528 3743240217 : ftype Location::longitude_scale(int32_t lat)
529 : {
530 3743240217 : ftype scale = cosF(lat * (1.0e-7 * DEG_TO_RAD));
531 3743238845 : return MAX(scale, 0.01);
532 : }
533 :
534 : /*
535 : * convert invalid waypoint with useful data. return true if location changed
536 : */
537 343 : bool Location::sanitize(const Location &defaultLoc)
538 : {
539 343 : bool has_changed = false;
540 : // convert lat/lng=0 to mean current point
541 343 : if (lat == 0 && lng == 0) {
542 35 : lat = defaultLoc.lat;
543 35 : lng = defaultLoc.lng;
544 35 : has_changed = true;
545 : }
546 :
547 : // convert relative alt=0 to mean current alt
548 343 : if (alt == 0 && relative_alt) {
549 : int32_t defaultLoc_alt;
550 58 : if (defaultLoc.get_alt_cm(get_alt_frame(), defaultLoc_alt)) {
551 58 : alt = defaultLoc_alt;
552 58 : has_changed = true;
553 : }
554 : }
555 :
556 : // limit lat/lng to appropriate ranges
557 343 : if (!check_latlng()) {
558 4 : lat = defaultLoc.lat;
559 4 : lng = defaultLoc.lng;
560 4 : has_changed = true;
561 : }
562 :
563 343 : return has_changed;
564 : }
565 :
566 : // return bearing in radians from location to loc2, return is 0 to 2*Pi
567 1930326 : ftype Location::get_bearing(const Location &loc2) const
568 : {
569 1930326 : const int32_t off_x = diff_longitude(loc2.lng,lng);
570 1930326 : const int32_t off_y = (loc2.lat - lat) / loc2.longitude_scale((lat+loc2.lat)/2);
571 1930325 : ftype bearing = (M_PI*0.5) + atan2F(-off_y, off_x);
572 1930326 : if (bearing < 0) {
573 730039 : bearing += 2*M_PI;
574 : }
575 1930326 : return bearing;
576 : }
577 :
578 : /*
579 : return true if lat and lng match. Ignores altitude and options
580 : */
581 112801 : bool Location::same_latlon_as(const Location &loc2) const
582 : {
583 112801 : return (lat == loc2.lat) && (lng == loc2.lng);
584 : }
585 :
586 33420 : bool Location::same_alt_as(const Location &loc2) const
587 : {
588 : // fast path if the altitude frame is the same
589 33420 : if (this->get_alt_frame() == loc2.get_alt_frame()) {
590 33420 : return this->alt == loc2.alt;
591 : }
592 :
593 : ftype alt_diff;
594 0 : bool have_diff = this->get_height_above(loc2, alt_diff);
595 :
596 0 : const ftype tolerance = FLT_EPSILON;
597 0 : return have_diff && (fabsF(alt_diff) < tolerance);
598 : }
599 :
600 : // return true when lat and lng are within range
601 7163 : bool Location::check_latlng() const
602 : {
603 7163 : return check_lat(lat) && check_lng(lng);
604 : }
605 :
606 : // see if location is past a line perpendicular to
607 : // the line between point1 and point2 and passing through point2.
608 : // If point1 is our previous waypoint and point2 is our target waypoint
609 : // then this function returns true if we have flown past
610 : // the target waypoint
611 262933 : bool Location::past_interval_finish_line(const Location &point1, const Location &point2) const
612 : {
613 262933 : return this->line_path_proportion(point1, point2) >= 1.0f;
614 : }
615 :
616 : /*
617 : return the proportion we are along the path from point1 to
618 : point2, along a line parallel to point1<->point2.
619 :
620 : This will be more than 1 if we have passed point2
621 : */
622 900352 : float Location::line_path_proportion(const Location &point1, const Location &point2) const
623 : {
624 900352 : const Vector2f vec1 = point1.get_distance_NE(point2);
625 900352 : const Vector2f vec2 = point1.get_distance_NE(*this);
626 900352 : const ftype dsquared = sq(vec1.x) + sq(vec1.y);
627 900352 : if (dsquared < 0.001f) {
628 : // the two points are very close together
629 162316 : return 1.0f;
630 : }
631 738036 : return (vec1 * vec2) / dsquared;
632 : }
633 :
634 : /*
635 : wrap longitude for -180e7 to 180e7
636 : */
637 1761064031 : int32_t Location::wrap_longitude(int64_t lon)
638 : {
639 1761064031 : if (lon > 1800000000L) {
640 6 : lon = int32_t(lon-3600000000LL);
641 1761064025 : } else if (lon < -1800000000L) {
642 2 : lon = int32_t(lon+3600000000LL);
643 : }
644 1761064031 : return int32_t(lon);
645 : }
646 :
647 : /*
648 : get lon1-lon2, wrapping at -180e7 to 180e7
649 : */
650 1982192898 : int32_t Location::diff_longitude(int32_t lon1, int32_t lon2)
651 : {
652 1982192898 : if ((lon1 & 0x80000000) == (lon2 & 0x80000000)) {
653 : // common case of same sign
654 1982189935 : return lon1 - lon2;
655 : }
656 2963 : int64_t dlon = int64_t(lon1)-int64_t(lon2);
657 2963 : if (dlon > 1800000000LL) {
658 2408 : dlon -= 3600000000LL;
659 555 : } else if (dlon < -1800000000LL) {
660 0 : dlon += 3600000000LL;
661 : }
662 2963 : return int32_t(dlon);
663 : }
664 :
665 : /*
666 : limit latitude to -90e7 to 90e7
667 : */
668 1761064398 : int32_t Location::limit_lattitude(int32_t lat)
669 : {
670 1761064398 : if (lat > 900000000L) {
671 0 : lat = 1800000000LL - lat;
672 1761064398 : } else if (lat < -900000000L) {
673 0 : lat = -(1800000000LL + lat);
674 : }
675 1761064398 : return lat;
676 : }
677 :
678 : // update altitude and alt-frame base on this location's horizontal position between point1 and point2
679 : // this location's lat,lon is used to calculate the alt of the closest point on the line between point1 and point2
680 : // origin and destination's altitude frames must be the same
681 : // this alt-frame will be updated to match the destination alt frame
682 0 : void Location::linearly_interpolate_alt(const Location &point1, const Location &point2)
683 : {
684 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
685 : // alt frames must match; mixing frames produces meaningless interpolated altitudes
686 0 : if (point1.get_alt_frame() != point2.get_alt_frame()) {
687 0 : AP_HAL::panic("linearly_interpolate_alt: alt frames differ (point1=%u point2=%u)",
688 0 : (unsigned)point1.get_alt_frame(), (unsigned)point2.get_alt_frame());
689 : }
690 : #endif
691 : // new target's distance along the original track and then linear interpolate between the original origin and destination altitudes
692 0 : set_alt_cm(point1.alt + (point2.alt - point1.alt) * constrain_float(line_path_proportion(point1, point2), 0.0f, 1.0f), point2.get_alt_frame());
693 0 : }
694 :
695 : #endif // HAL_BOOTLOADER_BUILD
|
