Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include "AP_Terrain.h"
17 :
18 : #if AP_TERRAIN_AVAILABLE
19 :
20 : #include <AP_HAL/AP_HAL.h>
21 : #include <AP_Common/AP_Common.h>
22 : #include <AP_Math/AP_Math.h>
23 : #include <GCS_MAVLink/GCS_MAVLink.h>
24 : #include <GCS_MAVLink/GCS.h>
25 : #include <AP_Logger/AP_Logger.h>
26 : #include <AP_AHRS/AP_AHRS.h>
27 : #include <AP_Vehicle/AP_Vehicle_Type.h>
28 : #include <AP_Filesystem/AP_Filesystem.h>
29 : #include <AP_Rally/AP_Rally.h>
30 :
31 : extern const AP_HAL::HAL& hal;
32 :
33 : AP_Terrain *AP_Terrain::singleton;
34 :
35 : #if APM_BUILD_TYPE(APM_BUILD_ArduSub)
36 : #define TERRAIN_ENABLE_DEFAULT 0
37 : #else
38 : #define TERRAIN_ENABLE_DEFAULT 1
39 : #endif
40 :
41 : // table of user settable parameters
42 : const AP_Param::GroupInfo AP_Terrain::var_info[] = {
43 : // @Param: ENABLE
44 : // @DisplayName: Terrain data enable
45 : // @Description: enable terrain data. This enables the vehicle storing a database of terrain data on the SD card. The terrain data is requested from the ground station as needed, and stored for later use on the SD card. To be useful the ground station must support TERRAIN_REQUEST messages and have access to a terrain database, such as the SRTM database.
46 : // @Values: 0:Disable,1:Enable
47 : // @User: Advanced
48 : AP_GROUPINFO_FLAGS("ENABLE", 0, AP_Terrain, enable, TERRAIN_ENABLE_DEFAULT, AP_PARAM_FLAG_ENABLE),
49 :
50 : // @Param: SPACING
51 : // @DisplayName: Terrain grid spacing
52 : // @Description: Distance between terrain grid points in meters. This controls the horizontal resolution of the terrain data that is stored on te SD card and requested from the ground station. If your GCS is using the ArduPilot SRTM database like Mission Planner or MAVProxy, then a resolution of 100 meters is appropriate. Grid spacings lower than 100 meters waste SD card space if the GCS cannot provide that resolution. The grid spacing also controls how much data is kept in memory during flight. A larger grid spacing will allow for a larger amount of data in memory. A grid spacing of 100 meters results in the vehicle keeping 12 grid squares in memory with each grid square having a size of 2.7 kilometers by 3.2 kilometers. Any additional grid squares are stored on the SD once they are fetched from the GCS and will be loaded as needed.
53 : // @Units: m
54 : // @Increment: 1
55 : // @User: Advanced
56 : AP_GROUPINFO("SPACING", 1, AP_Terrain, grid_spacing, 100),
57 :
58 : // @Param: OPTIONS
59 : // @DisplayName: Terrain options
60 : // @Description: Options to change behavior of terrain system. The Accept Old Terrain Data option can be used to accept terrain data generated from before the terrain database bugs were fixed. The bugs in the data were all fixed from 24th February 2026. If you really want to risk using the old terrain data then you can set this option, otherwise remove the old terrain data by formatting your microSD card or renaming the /APM/TERRAIN folder. Then downloaded updated data from https://terrain.ardupilot.club, or let the automatic terrain download repopulate your terrain data.
61 : // @Bitmask: 0:Disable Download,1:Disable Disk,2:Accept Old Terrain Data
62 : // @User: Advanced
63 : AP_GROUPINFO("OPTIONS", 2, AP_Terrain, options, 0),
64 :
65 : // @Param: MARGIN
66 : // @DisplayName: Acceptance margin
67 : // @Description: Margin in centi-meters to accept terrain data from the GCS. This can be used to allow older terrain data generated with less accurate latitude/longitude scaling to be used
68 : // @Units: m
69 : // @Range: 0.05 50000
70 : // @User: Advanced
71 : AP_GROUPINFO("MARGIN", 3, AP_Terrain, margin, 0.05),
72 :
73 : // @Param: OFS_MAX
74 : // @DisplayName: Terrain reference offset maximum
75 : // @Description: The maximum adjustment of terrain altitude based on the assumption that the vehicle is on the ground when it is armed. When the vehicle is armed the location of the vehicle is recorded, and when terrain data is available for that location a height adjustment for terrain data is calculated that aligns the terrain height at that location with the altitude recorded at arming. This height adjustment is applied to all terrain data. This parameter clamps the amount of adjustment. A value of zero disables the use of terrain height adjustment.
76 : // @Units: m
77 : // @Range: 0 50
78 : // @User: Advanced
79 : AP_GROUPINFO("OFS_MAX", 4, AP_Terrain, offset_max, 30),
80 :
81 : // @Param: CACHE_SZ
82 : // @DisplayName: Terrain cache size
83 : // @Description: The number of 32x28 cache blocks to keep in memory. Each block uses about 1800 bytes of memory
84 : // @Range: 0 128
85 : // @User: Advanced
86 : AP_GROUPINFO("CACHE_SZ", 5, AP_Terrain, config_cache_size, TERRAIN_GRID_BLOCK_CACHE_SIZE),
87 :
88 : AP_GROUPEND
89 : };
90 :
91 : // constructor
92 1778 : AP_Terrain::AP_Terrain() :
93 1778 : disk_io_state(DiskIoIdle),
94 1778 : fd(-1)
95 : {
96 1778 : AP_Param::setup_object_defaults(this, var_info);
97 :
98 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
99 1777 : if (singleton != nullptr) {
100 0 : AP_HAL::panic("Terrain must be singleton");
101 : }
102 : #endif
103 1778 : singleton = this;
104 1778 : }
105 :
106 : /*
107 : return terrain height in meters above average sea level (WGS84) for
108 : a given position
109 :
110 : This is the base function that other height calculations are derived
111 : from. The functions below are more convenient for most uses
112 :
113 : This function costs about 20 microseconds on Pixhawk
114 : */
115 1359994592 : bool AP_Terrain::height_amsl(const Location &loc, float &height, bool corrected)
116 : {
117 1359994592 : if (!allocate()) {
118 10319891 : return false;
119 : }
120 :
121 1349674547 : const AP_AHRS &ahrs = AP::ahrs();
122 :
123 : // quick access for home altitude
124 1349674442 : if (loc.lat == home_loc.lat &&
125 383210685 : loc.lng == home_loc.lng) {
126 342349050 : height = home_height;
127 342349050 : if (corrected && have_reference_offset) {
128 2116194 : height += reference_offset;
129 : }
130 342349050 : return true;
131 : }
132 :
133 : struct grid_info info;
134 :
135 1007325392 : calculate_grid_info(loc, info);
136 :
137 : // find the grid
138 1007324413 : const struct grid_block &grid = find_grid_cache(info).grid;
139 :
140 : /*
141 : note that we rely on the one square overlap to ensure these
142 : calculations don't go past the end of the arrays
143 : */
144 : ASSERT_RANGE(info.idx_x, 0, TERRAIN_GRID_BLOCK_SIZE_X-2);
145 : ASSERT_RANGE(info.idx_y, 0, TERRAIN_GRID_BLOCK_SIZE_Y-2);
146 :
147 :
148 : // check we have all 4 required heights
149 1007324170 : if (!check_bitmap(grid, info.idx_x, info.idx_y) ||
150 926149768 : !check_bitmap(grid, info.idx_x, info.idx_y+1) ||
151 2859623652 : !check_bitmap(grid, info.idx_x+1, info.idx_y) ||
152 926149803 : !check_bitmap(grid, info.idx_x+1, info.idx_y+1)) {
153 81174291 : return false;
154 : }
155 :
156 : // hXY are the heights of the 4 surrounding grid points
157 926149697 : const auto h00 = grid.height[info.idx_x+0][info.idx_y+0];
158 926149697 : const auto h01 = grid.height[info.idx_x+0][info.idx_y+1];
159 926149697 : const auto h10 = grid.height[info.idx_x+1][info.idx_y+0];
160 926149697 : const auto h11 = grid.height[info.idx_x+1][info.idx_y+1];
161 :
162 : // do a simple dual linear interpolation. We could do something
163 : // fancier, but it probably isn't worth it as long as the
164 : // grid_spacing is kept small enough
165 926149697 : const float avg1 = (1.0f-info.frac_x) * h00 + info.frac_x * h10;
166 926149697 : const float avg2 = (1.0f-info.frac_x) * h01 + info.frac_x * h11;
167 926149697 : const float avg = (1.0f-info.frac_y) * avg1 + info.frac_y * avg2;
168 :
169 926149697 : height = avg;
170 :
171 967014037 : if (loc.lat == ahrs.get_home().lat &&
172 40864334 : loc.lng == ahrs.get_home().lng) {
173 : // remember home altitude as a special case
174 1473 : home_height = height;
175 1473 : home_loc = loc;
176 1473 : have_home_height = true;
177 : }
178 :
179 926149799 : if (corrected && have_reference_offset) {
180 21549845 : height += reference_offset;
181 : }
182 :
183 926149799 : return true;
184 : }
185 :
186 :
187 : /*
188 : find difference between home terrain height and the terrain
189 : height at the current location in meters. A positive result
190 : means the terrain is higher than home.
191 :
192 : return false is terrain at the current location or at home
193 : location is not available
194 :
195 : If extrapolate is true then allow return of an extrapolated
196 : terrain altitude based on the last available data
197 : */
198 59538 : bool AP_Terrain::height_terrain_difference_home(float &terrain_difference, bool extrapolate)
199 : {
200 59538 : const AP_AHRS &ahrs = AP::ahrs();
201 :
202 : float height_home, height_loc;
203 59538 : if (!height_amsl(ahrs.get_home(), height_home)) {
204 : // we don't know the height of home
205 0 : return false;
206 : }
207 :
208 59538 : Location loc;
209 59538 : if (!ahrs.get_location(loc)) {
210 : // we don't know where we are
211 0 : return false;
212 : }
213 :
214 59538 : if (!height_amsl(loc, height_loc)) {
215 0 : if (!extrapolate || !have_current_loc_height) {
216 : // we don't know the height of the given location
217 0 : return false;
218 : }
219 : // we don't have data at the current location, but the caller
220 : // has asked for extrapolation, so use the last available
221 : // terrain height. This can be used to fill in while new data
222 : // is fetched. It should be very rarely used
223 0 : height_loc = last_current_loc_height;
224 : }
225 :
226 59538 : terrain_difference = height_loc - height_home;
227 :
228 59538 : return true;
229 : }
230 :
231 : /*
232 : return current height above terrain at current AHRS
233 : position.
234 :
235 : If extrapolate is true then extrapolate from most recently
236 : available terrain data is terrain data is not available for the
237 : current location.
238 :
239 : Return true if height is available, otherwise false.
240 : */
241 6208143 : bool AP_Terrain::height_above_terrain(float &terrain_altitude, bool extrapolate)
242 : {
243 6208143 : const AP_AHRS &ahrs = AP::ahrs();
244 :
245 6208143 : Location current_loc;
246 6208143 : if (!ahrs.get_location(current_loc)) {
247 : // we don't know where we are
248 178654 : return false;
249 : }
250 :
251 : float theight_loc;
252 6029489 : if (!height_amsl(current_loc, theight_loc)) {
253 313099 : if (!extrapolate || !have_current_loc_height) {
254 286549 : return false;
255 : }
256 : // we don't have data at the current location, but the caller
257 : // has asked for extrapolation, so use the last available
258 : // terrain height. This can be used to fill in while new data
259 : // is fetched. It should be very rarely used
260 26550 : theight_loc = last_current_loc_height;
261 : }
262 :
263 5742940 : int32_t height_amsl_cm = 0;
264 5742940 : UNUSED_RESULT(current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, height_amsl_cm));
265 :
266 5742940 : terrain_altitude = height_amsl_cm*0.01 - theight_loc;
267 5742940 : return true;
268 : }
269 :
270 : /*
271 : return estimated equivalent relative-to-home altitude in meters
272 : of a given height above the terrain at the current location
273 :
274 : This function allows existing height controllers which work on
275 : barometric altitude (relative to home) to be used with terrain
276 : based target altitude, by translating the "above terrain" altitude
277 : into an equivalent barometric relative height.
278 :
279 : return false if terrain data is not available either at the given
280 : location or at the home location.
281 :
282 : If extrapolate is true then allow return of an extrapolated
283 : terrain altitude based on the last available data
284 : */
285 59538 : bool AP_Terrain::height_relative_home_equivalent(float terrain_altitude,
286 : float &relative_home_altitude,
287 : bool extrapolate)
288 : {
289 : float terrain_difference;
290 59538 : if (!height_terrain_difference_home(terrain_difference, extrapolate)) {
291 0 : return false;
292 : }
293 59538 : relative_home_altitude = terrain_altitude + terrain_difference;
294 :
295 : /*
296 : adjust for height of home above terrain height at home
297 : */
298 59538 : const AP_AHRS &ahrs = AP::ahrs();
299 59538 : const auto &home = ahrs.get_home();
300 59538 : int32_t home_height_amsl_cm = 0;
301 59538 : UNUSED_RESULT(home.get_alt_cm(Location::AltFrame::ABSOLUTE, home_height_amsl_cm));
302 :
303 : float theight_home;
304 59538 : if (!height_amsl(home, theight_home)) {
305 0 : return false;
306 : }
307 :
308 59538 : relative_home_altitude += theight_home - home_height_amsl_cm*0.01;
309 :
310 59538 : return true;
311 : }
312 :
313 :
314 : /*
315 : calculate lookahead rise in terrain. This returns extra altitude
316 : needed to clear upcoming terrain in meters
317 : */
318 47518 : float AP_Terrain::lookahead(float bearing, float distance, float climb_ratio)
319 : {
320 47518 : if (!allocate() || grid_spacing <= 0) {
321 0 : return 0;
322 : }
323 :
324 47518 : Location loc;
325 47518 : if (!AP::ahrs().get_location(loc)) {
326 : // we don't know where we are
327 0 : return 0;
328 : }
329 : float base_height;
330 47518 : if (!height_amsl(loc, base_height)) {
331 : // we don't know our current terrain height
332 0 : return 0;
333 : }
334 :
335 47518 : float climb = 0;
336 47518 : float lookahead_estimate = 0;
337 :
338 : // check for terrain at grid spacing intervals
339 780087 : while (distance > 0) {
340 732569 : loc.offset_bearing(bearing, grid_spacing);
341 732569 : climb += climb_ratio * grid_spacing;
342 732569 : distance -= grid_spacing;
343 : float height;
344 732569 : if (height_amsl(loc, height)) {
345 732401 : float rise = (height - base_height) - climb;
346 732401 : if (rise > lookahead_estimate) {
347 38423 : lookahead_estimate = rise;
348 : }
349 : }
350 : }
351 :
352 47518 : return lookahead_estimate;
353 : }
354 :
355 :
356 : /*
357 : 1hz update function. This is here to ensure progress is made on disk
358 : IO even if no MAVLink send_request() operations are called for a
359 : while.
360 : */
361 1361075 : void AP_Terrain::update(void)
362 : {
363 1361075 : if (!enable) { return; }
364 : // just schedule any needed disk IO
365 1293011 : schedule_disk_io();
366 :
367 1293011 : AP_AHRS &ahrs = AP::ahrs();
368 :
369 : // try to ensure the home location is populated
370 : float height;
371 1293011 : height_amsl(ahrs.get_home(), height);
372 :
373 : // update the cached current location height
374 1293011 : Location loc;
375 1293011 : bool pos_valid = ahrs.get_location(loc);
376 1293011 : bool terrain_valid = pos_valid && height_amsl(loc, height);
377 1293011 : if (pos_valid && terrain_valid) {
378 1127469 : last_current_loc_height = height;
379 1127469 : have_current_loc_height = true;
380 :
381 : // send terrain altitude to AHRS for optical flow when rangefinder is out of range
382 1127469 : ahrs.writeTerrainAMSL(height);
383 : }
384 :
385 : // check for pending mission data
386 1293011 : update_mission_data();
387 :
388 : #if HAL_RALLY_ENABLED
389 : // check for pending rally data
390 1293011 : update_rally_data();
391 : #endif
392 :
393 : // update tiles surrounding our current location:
394 1293011 : if (pos_valid) {
395 1202374 : have_surrounding_tiles = update_surrounding_tiles(loc);
396 : } else {
397 90637 : have_surrounding_tiles = false;
398 : }
399 :
400 : // update capabilities and status
401 1293011 : if (allocate()) {
402 1293011 : if (!pos_valid) {
403 : // we don't know where we are
404 90637 : system_status = TerrainStatusUnhealthy;
405 1202374 : } else if (!terrain_valid) {
406 : // we don't have terrain data at current location
407 74905 : system_status = TerrainStatusUnhealthy;
408 : } else {
409 1127469 : system_status = TerrainStatusOK;
410 : }
411 : } else {
412 0 : system_status = TerrainStatusDisabled;
413 : }
414 : }
415 :
416 1202374 : bool AP_Terrain::update_surrounding_tiles(const Location &loc)
417 : {
418 : // also request a larger set of up to 9 grids
419 1202374 : bool ret = true;
420 4809496 : for (int8_t x=-1; x<=1; x++) {
421 14428488 : for (int8_t y=-1; y<=1; y++) {
422 10821366 : Location loc2 = loc;
423 10821366 : loc2.offset(x*TERRAIN_GRID_BLOCK_SIZE_X*0.7f*grid_spacing,
424 10821366 : y*TERRAIN_GRID_BLOCK_SIZE_Y*0.7f*grid_spacing);
425 : float height;
426 10821366 : if (!height_amsl(loc2, height)) {
427 733766 : ret = false;
428 : }
429 : }
430 : }
431 1202374 : return ret;
432 : }
433 :
434 906 : bool AP_Terrain::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len) const
435 : {
436 : // check no outstanding requests for data:
437 : uint16_t terr_pending, terr_loaded;
438 906 : get_statistics(terr_pending, terr_loaded);
439 906 : if (terr_pending != 0 ||
440 736 : !have_current_loc_height ||
441 576 : !have_home_height ||
442 556 : next_mission_index != 0 ||
443 555 : next_rally_index != 0) {
444 351 : hal.util->snprintf(failure_msg, failure_msg_len, "waiting for terrain data");
445 351 : return false;
446 : }
447 555 : if (grid_spacing <= 0) {
448 0 : hal.util->snprintf(failure_msg, failure_msg_len, "TERRAIN_SPACING can't be <= 0");
449 0 : return false;
450 : }
451 :
452 555 : if (!option_set(Options::AcceptOldData) && found_old_data) {
453 : /*
454 : we have found old terrain data on the microSD card, warn the
455 : user that they have out of date terrain data that may
456 : contain errors.
457 : */
458 0 : hal.util->snprintf(failure_msg, failure_msg_len, "terrain data expired, possible errors");
459 0 : return false;
460 : }
461 :
462 555 : return true;
463 : }
464 :
465 : #if HAL_LOGGING_ENABLED
466 129567 : void AP_Terrain::log_terrain_data()
467 : {
468 129567 : if (!allocate()) {
469 13827 : return;
470 : }
471 123014 : Location loc;
472 123014 : if (!AP::ahrs().get_location(loc)) {
473 : // we don't know where we are
474 7274 : return;
475 : }
476 115740 : float terrain_height = 0;
477 115740 : float current_height = 0;
478 : uint16_t pending, loaded;
479 :
480 115740 : height_amsl(loc, terrain_height);
481 115740 : height_above_terrain(current_height, true);
482 115740 : get_statistics(pending, loaded);
483 :
484 115740 : struct log_TERRAIN pkt = {
485 : LOG_PACKET_HEADER_INIT(LOG_TERRAIN_MSG),
486 115740 : time_us : AP_HAL::micros64(),
487 115740 : status : (uint8_t)status(),
488 115740 : lat : loc.lat,
489 115740 : lng : loc.lng,
490 231480 : spacing : (uint16_t)grid_spacing,
491 : terrain_height : terrain_height,
492 : current_height : current_height,
493 : pending : pending,
494 : loaded : loaded,
495 115740 : reference_offset : have_reference_offset?reference_offset:0,
496 115740 : };
497 115740 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
498 : }
499 : #endif
500 :
501 : /*
502 : allocate terrain cache. Making this dynamically allocated allows
503 : memory to be saved when terrain functionality is disabled
504 : */
505 1364679315 : bool AP_Terrain::allocate(void)
506 : {
507 1364679315 : if (enable == 0 || memory_alloc_failed) {
508 10387028 : return false;
509 : }
510 1354292205 : if (cache != nullptr) {
511 1354290506 : return true;
512 : }
513 1699 : cache = (struct grid_cache *)calloc(config_cache_size, sizeof(cache[0]));
514 1694 : if (cache == nullptr) {
515 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Terrain: Allocation failed");
516 0 : memory_alloc_failed = true;
517 0 : return false;
518 : }
519 1694 : cache_size = config_cache_size;
520 1694 : return true;
521 : }
522 :
523 : /*
524 : setup a reference location for terrain adjustment. This should
525 : be called when the vehicle is definately on the ground
526 : */
527 1014 : void AP_Terrain::set_reference_location(void)
528 : {
529 1014 : const auto &ahrs = AP::ahrs();
530 :
531 : // check we have absolute position
532 : nav_filter_status status;
533 1014 : if (!ahrs.get_filter_status(status) ||
534 1014 : !status.flags.vert_pos ||
535 3024 : !status.flags.horiz_pos_abs ||
536 996 : !status.flags.attitude) {
537 102 : return;
538 : }
539 :
540 : // check we have a small 3D velocity
541 996 : Vector3f vel;
542 1992 : if (!ahrs.get_velocity_NED(vel) ||
543 996 : vel.length() > 3) {
544 84 : return;
545 : }
546 :
547 912 : have_reference_offset = false;
548 912 : have_reference_loc = ahrs.get_location(reference_loc);
549 :
550 912 : update_reference_offset();
551 : }
552 :
553 : /*
554 : get the offset between terrain height and reference alt at the
555 : reference location
556 : */
557 207677899 : void AP_Terrain::update_reference_offset(void)
558 : {
559 : // TERR_OFS_MAX of zero means no adjustment
560 207677899 : if (!is_positive(offset_max)) {
561 1921383 : have_reference_offset = false;
562 207677090 : return;
563 : }
564 :
565 : // allow for change to TERRAIN_OFS_MAX while flying
566 205756516 : if (have_reference_offset) {
567 138786350 : reference_offset = constrain_float(reference_offset, -offset_max, offset_max);
568 138786350 : return;
569 : }
570 :
571 66970166 : if (!have_reference_loc) {
572 : // no reference available yet
573 55516914 : return;
574 : }
575 :
576 : // calculate adjustment
577 : float height;
578 11453252 : if (!height_amsl(reference_loc, height)) {
579 11452443 : return;
580 : }
581 : int32_t alt_cm;
582 809 : if (!reference_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) {
583 0 : return;
584 : }
585 809 : const float adjustment = alt_cm*0.01 - height;
586 809 : reference_offset = constrain_float(adjustment, -offset_max, offset_max);
587 809 : if (fabsf(adjustment) > offset_max.get()+0.5) {
588 1 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Terrain: clamping offset %.0f to %.0f",
589 : adjustment, reference_offset);
590 : }
591 809 : have_reference_offset = true;
592 : }
593 :
594 :
595 : namespace AP {
596 :
597 623645235 : AP_Terrain *terrain()
598 : {
599 623645235 : return AP_Terrain::get_singleton();
600 : }
601 :
602 : };
603 :
604 : #endif // AP_TERRAIN_AVAILABLE
|
