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 : #pragma once
16 :
17 : #include "AP_Terrain_config.h"
18 :
19 : #if AP_TERRAIN_AVAILABLE
20 :
21 : #include <AP_Common/AP_Common.h>
22 : #include <AP_Common/Location.h>
23 : #include <AP_Param/AP_Param.h>
24 : #include <GCS_MAVLink/GCS_MAVLink.h>
25 : #include <AP_Logger/AP_Logger_config.h>
26 :
27 : #define TERRAIN_DEBUG 0
28 :
29 :
30 : // MAVLink sends 4x4 grids
31 : #define TERRAIN_GRID_MAVLINK_SIZE 4
32 :
33 : // a 2k grid_block on disk contains 8x7 of the mavlink grids. Each
34 : // grid block overlaps by one with its neighbour. This ensures that
35 : // the altitude at any point can be calculated from a single grid
36 : // block
37 : #define TERRAIN_GRID_BLOCK_MUL_X 7
38 : #define TERRAIN_GRID_BLOCK_MUL_Y 8
39 :
40 : // this is the spacing between 32x28 grid blocks, in grid_spacing units
41 : #define TERRAIN_GRID_BLOCK_SPACING_X ((TERRAIN_GRID_BLOCK_MUL_X-1)*TERRAIN_GRID_MAVLINK_SIZE)
42 : #define TERRAIN_GRID_BLOCK_SPACING_Y ((TERRAIN_GRID_BLOCK_MUL_Y-1)*TERRAIN_GRID_MAVLINK_SIZE)
43 :
44 : // giving a total grid size of a disk grid_block of 32x28
45 : #define TERRAIN_GRID_BLOCK_SIZE_X (TERRAIN_GRID_MAVLINK_SIZE*TERRAIN_GRID_BLOCK_MUL_X)
46 : #define TERRAIN_GRID_BLOCK_SIZE_Y (TERRAIN_GRID_MAVLINK_SIZE*TERRAIN_GRID_BLOCK_MUL_Y)
47 :
48 : // number of grid_blocks in the LRU memory cache
49 : #ifndef TERRAIN_GRID_BLOCK_CACHE_SIZE
50 : #define TERRAIN_GRID_BLOCK_CACHE_SIZE 12
51 : #endif
52 :
53 : // format of grid on disk
54 : #define TERRAIN_GRID_FORMAT_VERSION 1
55 :
56 : // min minor version for data read from microSD
57 : // raise this to force a refresh of data from the terrain servers
58 : #define TERRAIN_VERSION_MINOR_MIN 1
59 :
60 : // we allow for a 2cm discrepancy in the grid corners. This is to
61 : // account for different rounding in terrain DAT file generators using
62 : // different programming languages
63 : #define TERRAIN_LATLON_EQUAL(v1, v2) (unsigned(labs((v1)-(v2))) <= unsigned(margin.get()*100))
64 :
65 : #if TERRAIN_DEBUG
66 : #include <assert.h>
67 : #define ASSERT_RANGE(v,minv,maxv) assert((v)<=(maxv)&&(v)>=(minv))
68 : #else
69 : #define ASSERT_RANGE(v,minv,maxv)
70 : #endif
71 :
72 :
73 : /*
74 : Data conventions in this library:
75 :
76 : array[x][y]: x is increasing north, y is increasing east
77 : array[x]: low order bits increase east first
78 : bitmap: low order bits increase east first
79 : file: first entries increase east, then north
80 : */
81 :
82 : class AP_Terrain {
83 : public:
84 : AP_Terrain();
85 :
86 : /* Do not allow copies */
87 : CLASS_NO_COPY(AP_Terrain);
88 :
89 808667440 : static AP_Terrain *get_singleton(void) { return singleton; }
90 :
91 : enum TerrainStatus {
92 : TerrainStatusDisabled = 0, // not enabled
93 : TerrainStatusUnhealthy = 1, // no terrain data for current location
94 : TerrainStatusOK = 2 // terrain data available
95 : };
96 :
97 : static const struct AP_Param::GroupInfo var_info[];
98 :
99 : // update terrain state. Should be called at 1Hz or more
100 : void update(void);
101 :
102 288426 : bool enabled() const { return enable; }
103 4 : void set_enabled(bool _enable) { enable.set(_enable); }
104 :
105 : // return status enum for health reporting
106 4364937 : enum TerrainStatus status(void) const { return system_status; }
107 :
108 : bool pre_arm_checks(char *failure_msg, uint8_t failure_msg_len) const;
109 :
110 : #if HAL_GCS_ENABLED
111 : // send any pending terrain request message
112 : bool send_cache_request(class GCS_MAVLINK &link);
113 : void send_request(GCS_MAVLINK &link);
114 :
115 : // handle terrain data and reports from GCS
116 : // send a terrain report for the current location, extrapolating height as we do for navigation:
117 : void send_report(GCS_MAVLINK &link);
118 : // send a terrain report or Location loc
119 : void send_terrain_report(GCS_MAVLINK &link, const Location &loc, bool extrapolate);
120 : void handle_terrain_check(GCS_MAVLINK &link, const mavlink_message_t &msg);
121 : void handle_terrain_data(const mavlink_message_t &msg);
122 : #endif
123 :
124 : /*
125 : find the terrain height in meters above sea level for a location
126 :
127 : return false if not available
128 : */
129 : bool height_amsl(const Location &loc, float &height, bool corrected = true);
130 :
131 : /*
132 : find difference between home terrain height and the terrain
133 : height at the current location in meters. A positive result
134 : means the terrain is higher than home.
135 :
136 : return false is terrain at the current location or at home
137 : location is not available
138 :
139 : If extrapolate is true then allow return of an extrapolated
140 : terrain altitude based on the last available data
141 : */
142 : bool height_terrain_difference_home(float &terrain_difference,
143 : bool extrapolate = false);
144 :
145 : /*
146 : return estimated equivalent relative-to-home altitude in meters
147 : of a given height above the terrain at the current location
148 :
149 : This function allows existing height controllers which work on
150 : barometric altitude (relative to home) to be used with terrain
151 : based target altitude, by translating the "above terrain" altitude
152 : into an equivalent barometric relative height.
153 :
154 : return false if terrain data is not available either at the given
155 : location or at the home location.
156 :
157 : If extrapolate is true then allow return of an extrapolated
158 : terrain altitude based on the last available data
159 : */
160 : bool height_relative_home_equivalent(float terrain_altitude,
161 : float &relative_altitude,
162 : bool extrapolate = false);
163 :
164 : /*
165 : return current height above terrain at current AHRS position.
166 :
167 : If extrapolate is true then extrapolate from most recently
168 : available terrain data if terrain data is not available for the
169 : current location.
170 :
171 : Return true if height is available, otherwise false.
172 : */
173 : bool height_above_terrain(float &terrain_altitude, bool extrapolate = false);
174 :
175 : /*
176 : calculate lookahead rise in terrain. This returns extra altitude
177 : needed to clear upcoming terrain in meters
178 : */
179 : float lookahead(float bearing, float distance, float climb_ratio);
180 :
181 : #if HAL_LOGGING_ENABLED
182 : /*
183 : log terrain status to AP_Logger
184 : */
185 : void log_terrain_data();
186 : #endif
187 :
188 : /*
189 : get some statistics for TERRAIN_REPORT
190 : */
191 : void get_statistics(uint16_t &pending, uint16_t &loaded) const;
192 :
193 : /*
194 : get grid spacing in meters
195 : */
196 299 : uint16_t get_grid_spacing() const { return MAX(grid_spacing, 0); };
197 :
198 : /*
199 : returns true if initialisation failed because out-of-memory
200 : */
201 49679 : bool init_failed() const { return memory_alloc_failed; }
202 :
203 : /*
204 : setup a reference location for terrain adjustment. This should
205 : be called when the vehicle is definately on the ground
206 : */
207 : void set_reference_location(void);
208 :
209 : #if HAL_GCS_ENABLED
210 : void handle_message(GCS_MAVLINK &link, const mavlink_message_t &msg);
211 : #endif // HAL_GCS_ENABLED
212 :
213 : private:
214 : // allocate the terrain subsystem data
215 : bool allocate(void);
216 :
217 : /*
218 : a grid block is a structure in a local file containing height
219 : information. Each grid block is 2048 in size, to keep file IO to
220 : block oriented SD cards efficient
221 : */
222 : struct PACKED grid_block {
223 : // bitmap of 4x4 grids filled in from GCS (56 bits are used)
224 : uint64_t bitmap;
225 :
226 : // south west corner of block in degrees*10^7
227 : int32_t lat;
228 : int32_t lon;
229 :
230 : // crc of whole block, taken with crc=0
231 : uint16_t crc;
232 :
233 : // format version number
234 : uint16_t version;
235 :
236 : // grid spacing in meters
237 : uint16_t spacing;
238 :
239 : // heights in meters over a 32*28 grid
240 : int16_t height[TERRAIN_GRID_BLOCK_SIZE_X][TERRAIN_GRID_BLOCK_SIZE_Y];
241 :
242 : // indices info 32x28 grids for this degree reference
243 : uint16_t grid_idx_x;
244 : uint16_t grid_idx_y;
245 :
246 : // rounded latitude/longitude in degrees.
247 : int16_t lon_degrees;
248 : int8_t lat_degrees;
249 :
250 : // minor version. Note! this and any bytes after this are
251 : // excluded from the CRC for backwards compatibility
252 : uint8_t version_minor;
253 : };
254 :
255 : /*
256 : grid_block for disk IO, aligned on 2048 byte boundaries
257 : */
258 : union grid_io_block {
259 : struct grid_block block;
260 : uint8_t buffer[2048];
261 : };
262 :
263 : enum GridCacheState {
264 : GRID_CACHE_INVALID=0, // when first initialised
265 : GRID_CACHE_DISKWAIT=1, // when waiting for disk read
266 : GRID_CACHE_VALID=2, // when at least partially valid
267 : GRID_CACHE_DIRTY=3 // when updates have been made, and
268 : // disk write needed
269 : };
270 :
271 : /*
272 : a grid_block plus some meta data used for requesting new blocks
273 : */
274 : struct grid_cache {
275 : struct grid_block grid;
276 :
277 : volatile enum GridCacheState state;
278 :
279 : // the last time access was requested to this block, used for LRU
280 : uint32_t last_access_ms;
281 : };
282 :
283 : /*
284 : grid_info is a broken down representation of a Location, giving
285 : the index terms for finding the right grid
286 : */
287 : struct grid_info {
288 : // rounded latitude/longitude in degrees.
289 : int8_t lat_degrees;
290 : int16_t lon_degrees;
291 :
292 : // lat and lon of SW corner of this 32*28 grid, *10^7 degrees
293 : int32_t grid_lat;
294 : int32_t grid_lon;
295 :
296 : // indices info 32x28 grids for this degree reference
297 : uint16_t grid_idx_x;
298 : uint16_t grid_idx_y;
299 :
300 : // indexes into 32x28 grid
301 : uint8_t idx_x; // north (0..27)
302 : uint8_t idx_y; // east (0..31)
303 :
304 : // fraction within the grid square
305 : float frac_x; // north (0..1)
306 : float frac_y; // east (0..1)
307 :
308 : // file offset of this grid
309 : uint32_t file_offset;
310 : };
311 :
312 : // given a location, fill a grid_info structure
313 : void calculate_grid_info(const Location &loc, struct grid_info &info) const;
314 :
315 : /*
316 : find a grid structure given a grid_info
317 : */
318 : struct grid_cache &find_grid_cache(const struct grid_info &info);
319 :
320 : /*
321 : calculate bit number in grid_block bitmap. This corresponds to a
322 : bit representing a 4x4 mavlink transmitted block
323 : */
324 : uint8_t grid_bitnum(uint8_t idx_x, uint8_t idx_y);
325 :
326 : /*
327 : given a grid_info check that a given idx_x/idx_y is available (set
328 : in the bitmap)
329 : */
330 : bool check_bitmap(const struct grid_block &grid, uint8_t idx_x, uint8_t idx_y);
331 :
332 : #if HAL_GCS_ENABLED
333 : /*
334 : request any missing 4x4 grids from a block
335 : */
336 : bool request_missing(class GCS_MAVLINK &link, struct grid_cache &gcache);
337 : bool request_missing(GCS_MAVLINK &link, const struct grid_info &info);
338 : #endif // HAL_GCS_ENABLED
339 :
340 : /*
341 : look for blocks that need to be read/written to disk
342 : */
343 : void schedule_disk_io(void);
344 :
345 : /*
346 : get some statistics for TERRAIN_REPORT
347 : */
348 : uint8_t bitcount64(uint64_t b) const;
349 :
350 : /*
351 : disk IO functions
352 : */
353 : int16_t find_io_idx(enum GridCacheState state);
354 : uint16_t get_block_crc(struct grid_block &block);
355 : void check_disk_read(void);
356 : void check_disk_write(void);
357 : void io_timer(void);
358 : void open_file(void);
359 : void seek_offset(void);
360 : uint32_t east_blocks(struct grid_block &block) const;
361 : void write_block(void);
362 : void read_block(void);
363 :
364 : // check for missing data in squares surrounding loc:
365 : bool update_surrounding_tiles(const Location &loc);
366 :
367 : /*
368 : check for missing mission terrain data
369 : */
370 : void update_mission_data(void);
371 :
372 : /*
373 : check for missing rally data
374 : */
375 : void update_rally_data(void);
376 :
377 : /*
378 : calculate reference offset if needed
379 : */
380 : void update_reference_offset(void);
381 :
382 :
383 : // parameters
384 : AP_Int8 enable;
385 : AP_Float margin;
386 : AP_Int16 grid_spacing; // meters between grid points
387 : AP_Int16 options; // option bits
388 : AP_Float offset_max;
389 : AP_Int16 config_cache_size;
390 :
391 : enum class Options {
392 : DisableDownload = (1U<<0),
393 : DisableDisk = (1U<<1),
394 : AcceptOldData = (1U<<2),
395 : };
396 :
397 11406156 : inline bool option_set(enum Options option) const {
398 11406156 : return (options.get() & uint16_t(option)) != 0;
399 : }
400 :
401 2241602 : inline bool diskless() const {
402 2241602 : return option_set(Options::DisableDisk);
403 : }
404 :
405 : // cache of grids in memory, LRU
406 : uint8_t cache_size = 0;
407 : struct grid_cache *cache = nullptr;
408 :
409 : // a grid_cache block waiting for disk IO
410 : enum DiskIoState {
411 : DiskIoIdle = 0,
412 : DiskIoWaitWrite = 1,
413 : DiskIoWaitRead = 2,
414 : DiskIoDoneRead = 3,
415 : DiskIoDoneWrite = 4
416 : };
417 : volatile enum DiskIoState disk_io_state;
418 : union grid_io_block disk_block;
419 :
420 : #if HAL_GCS_ENABLED
421 : // last time we asked for more grids
422 : uint32_t last_request_time_ms[MAVLINK_COMM_NUM_BUFFERS];
423 : #endif
424 :
425 : static const uint64_t bitmap_mask = (((uint64_t)1U)<<(TERRAIN_GRID_BLOCK_MUL_X*TERRAIN_GRID_BLOCK_MUL_Y)) - 1;
426 :
427 : // open file handle on degree file
428 : int fd;
429 :
430 : // has the timer been setup?
431 : bool timer_setup;
432 :
433 : // degrees lat and lon of file
434 : int8_t file_lat_degrees;
435 : int16_t file_lon_degrees;
436 :
437 : // do we have an IO failure
438 : volatile bool io_failure;
439 : uint32_t last_retry_ms;
440 :
441 : // have we created the terrain directory?
442 : bool directory_created;
443 :
444 : // cache the home altitude, as it is needed so often
445 : float home_height;
446 : Location home_loc;
447 : bool have_home_height;
448 :
449 : // reference position for terrain adjustment, set at arming
450 : bool have_reference_loc;
451 : Location reference_loc;
452 :
453 : // calculated reference offset
454 : bool have_reference_offset;
455 : float reference_offset;
456 :
457 :
458 : // cache the last terrain height (AMSL) of the AHRS current
459 : // location. This is used for extrapolation when terrain data is
460 : // temporarily unavailable
461 : bool have_current_loc_height;
462 : float last_current_loc_height;
463 :
464 : // true if we have all of the data for the squares around the
465 : // current location:
466 : bool have_surrounding_tiles;
467 :
468 : // next mission command to check
469 : uint16_t next_mission_index;
470 :
471 : // next mission position to check
472 : uint8_t next_mission_pos;
473 :
474 : // last time the mission changed
475 : uint32_t last_mission_change_ms;
476 :
477 : // grid spacing during mission check
478 : uint16_t last_mission_spacing;
479 :
480 : // next rally command to check
481 : uint16_t next_rally_index;
482 :
483 : // last time the rally points changed
484 : uint32_t last_rally_change_ms;
485 :
486 : // grid spacing during rally check
487 : uint16_t last_rally_spacing;
488 :
489 : char *file_path = nullptr;
490 :
491 : // status
492 : enum TerrainStatus system_status = TerrainStatusDisabled;
493 :
494 : // memory allocation status
495 : bool memory_alloc_failed;
496 :
497 : static AP_Terrain *singleton;
498 :
499 : // true if we have found old disk blocks when loading
500 : bool found_old_data;
501 : };
502 :
503 : namespace AP {
504 : AP_Terrain *terrain();
505 : };
506 :
507 : #endif // AP_TERRAIN_AVAILABLE
508 :
|
