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 : handle vehicle <-> GCS communications for terrain library
17 : */
18 :
19 : #include "AP_Terrain.h"
20 :
21 : #if AP_TERRAIN_AVAILABLE
22 :
23 : #include <AP_AHRS/AP_AHRS.h>
24 : #include <AP_HAL/AP_HAL.h>
25 : #include <AP_Common/AP_Common.h>
26 : #include <AP_Math/AP_Math.h>
27 : #include <GCS_MAVLink/GCS_MAVLink.h>
28 : #include <GCS_MAVLink/GCS.h>
29 :
30 : #include <assert.h>
31 : #include <stdio.h>
32 :
33 : extern const AP_HAL::HAL& hal;
34 :
35 : #if HAL_GCS_ENABLED
36 : /*
37 : request any missing 4x4 grids from a block, given a grid_cache
38 : */
39 9163999 : bool AP_Terrain::request_missing(GCS_MAVLINK &link, struct grid_cache &gcache)
40 : {
41 9163999 : if (!HAVE_PAYLOAD_SPACE(link.get_chan(), TERRAIN_REQUEST)) {
42 0 : return false;
43 : }
44 :
45 9163999 : struct grid_block &grid = gcache.grid;
46 :
47 9163999 : if (option_set(Options::DisableDownload)) {
48 0 : return false;
49 : }
50 :
51 9163999 : if (grid.spacing != grid_spacing) {
52 : // an invalid grid
53 240 : return false;
54 : }
55 :
56 : // see if we are waiting for disk read
57 9163759 : if (gcache.state == GRID_CACHE_DISKWAIT && !diskless()) {
58 : // don't request data from the GCS till we know it's not on disk
59 164 : return false;
60 : }
61 :
62 : // see if it is fully populated
63 9163595 : if ((grid.bitmap & bitmap_mask) == bitmap_mask) {
64 : // it is fully populated, nothing to do
65 9154742 : return false;
66 : }
67 :
68 : /*
69 : ask the GCS to send a set of 4x4 grids
70 : */
71 : const mavlink_terrain_request_t packet {
72 8853 : bitmap_mask & ~grid.bitmap,
73 8853 : grid.lat,
74 8853 : grid.lon,
75 17706 : (uint16_t)grid_spacing
76 8853 : };
77 8853 : mavlink_msg_terrain_request_send_struct(link.get_chan(), &packet);
78 8853 : last_request_time_ms[link.get_chan()] = AP_HAL::millis();
79 :
80 8853 : return true;
81 : }
82 :
83 : /*
84 : request any missing 4x4 grids from a block
85 : */
86 1601883 : bool AP_Terrain::request_missing(GCS_MAVLINK &link, const struct grid_info &info)
87 : {
88 : // find the grid
89 1601883 : struct grid_cache &gcache = find_grid_cache(info);
90 1601883 : return request_missing(link, gcache);
91 : }
92 :
93 : /*
94 : send any pending cache requests
95 : */
96 856503 : bool AP_Terrain::send_cache_request(GCS_MAVLINK &link)
97 : {
98 11096373 : for (uint16_t i=0; i<cache_size; i++) {
99 10245066 : if (cache[i].state >= GRID_CACHE_VALID) {
100 7562116 : if (request_missing(link, cache[i])) {
101 5196 : return true;
102 : }
103 : }
104 : }
105 851307 : return false;
106 : }
107 :
108 : /*
109 : send any pending terrain request to the GCS
110 : */
111 991097 : void AP_Terrain::send_request(GCS_MAVLINK &link)
112 : {
113 991097 : if (!allocate()) {
114 : // not enabled
115 193016 : return;
116 : }
117 :
118 : // see if we need to schedule some disk IO
119 930513 : schedule_disk_io();
120 :
121 930513 : Location loc;
122 930513 : if (!AP::ahrs().get_location(loc)) {
123 : // we don't know where we are. Request any cached blocks.
124 : // this allows for download of mission items when we have no GPS lock
125 56358 : send_cache_request(link);
126 56358 : return;
127 : }
128 :
129 : // did we request recently?
130 874155 : if (AP_HAL::millis() - last_request_time_ms[link.get_chan()] < 2000) {
131 : // too soon to request again
132 70353 : return;
133 : }
134 :
135 : // request any missing 4x4 blocks in the current grid
136 : struct grid_info info;
137 803802 : calculate_grid_info(loc, info);
138 :
139 803802 : if (request_missing(link, info)) {
140 3657 : return;
141 : }
142 :
143 : // check cache blocks that may have been setup by a TERRAIN_CHECK,
144 : // mission items, rally items, squares surrounding our current
145 : // location, favourite holiday destination, scripting, height
146 : // reference location, ....
147 800145 : if (send_cache_request(link)) {
148 2064 : return;
149 : }
150 :
151 : // request the current loc last to ensure it has highest last
152 : // access time
153 798081 : if (request_missing(link, info)) {
154 0 : return;
155 : }
156 : }
157 : #endif // HAL_GCS_ENABLED
158 :
159 : /*
160 : count bits in a uint64_t
161 : */
162 10167919 : uint8_t AP_Terrain::bitcount64(uint64_t b) const
163 : {
164 10167919 : return __builtin_popcount((unsigned)(b&0xFFFFFFFF)) + __builtin_popcount((unsigned)(b>>32));
165 : }
166 :
167 : /*
168 : get some statistics for TERRAIN_REPORT
169 : */
170 1185977 : void AP_Terrain::get_statistics(uint16_t &pending, uint16_t &loaded) const
171 : {
172 1185977 : pending = 0;
173 1185977 : loaded = 0;
174 14747873 : for (uint16_t i=0; i<cache_size; i++) {
175 13561896 : if (cache[i].grid.spacing != grid_spacing) {
176 3360917 : continue;
177 : }
178 10200979 : if (cache[i].state == GRID_CACHE_INVALID) {
179 0 : continue;
180 : }
181 10200979 : uint8_t maskbits = TERRAIN_GRID_BLOCK_MUL_X*TERRAIN_GRID_BLOCK_MUL_Y;
182 10200979 : if (cache[i].state == GRID_CACHE_DISKWAIT) {
183 33060 : pending += maskbits;
184 33060 : continue;
185 : }
186 10167919 : if (cache[i].state == GRID_CACHE_DIRTY) {
187 : // count dirty grids as a pending, so we know where there
188 : // are disk writes pending
189 0 : pending++;
190 : }
191 10167919 : uint8_t bitcount = bitcount64(cache[i].grid.bitmap);
192 10167919 : pending += maskbits - bitcount;
193 10167919 : loaded += bitcount;
194 : }
195 1185977 : }
196 :
197 : #if HAL_GCS_ENABLED
198 : /*
199 : handle terrain messages from GCS
200 : */
201 15 : void AP_Terrain::handle_message(GCS_MAVLINK &link, const mavlink_message_t &msg)
202 : {
203 15 : switch (msg.msgid) {
204 0 : case MAVLINK_MSG_ID_TERRAIN_DATA:
205 0 : return handle_terrain_data(msg);
206 15 : case MAVLINK_MSG_ID_TERRAIN_CHECK:
207 15 : return handle_terrain_check(link, msg);
208 : // default:
209 : // shouldn't have been called
210 : }
211 : }
212 :
213 : /*
214 : send a TERRAIN_REPORT for the current location
215 : */
216 991097 : void AP_Terrain::send_report(GCS_MAVLINK &link)
217 : {
218 991097 : Location loc;
219 991097 : if (!AP::ahrs().get_location(loc)) {
220 60221 : loc = {};
221 : }
222 :
223 991097 : send_terrain_report(link, loc, true);
224 991097 : }
225 :
226 : /*
227 : send a TERRAIN_REPORT for a location
228 : */
229 991112 : void AP_Terrain::send_terrain_report(GCS_MAVLINK &link, const Location &loc, bool extrapolate)
230 : {
231 : #if HAL_GCS_ENABLED
232 991112 : if (!HAVE_PAYLOAD_SPACE(link.get_chan(), TERRAIN_REPORT)) {
233 0 : return;
234 : }
235 :
236 991112 : float terrain_height = 0;
237 991112 : uint16_t spacing = 0;
238 991112 : if (height_amsl(loc, terrain_height)) {
239 : // non-zero spacing indicates we have data
240 890085 : spacing = grid_spacing;
241 101027 : } else if (extrapolate && have_current_loc_height) {
242 : // show the extrapolated height, so logs show what height is
243 : // being used for navigation
244 8863 : terrain_height = last_current_loc_height;
245 : }
246 : uint16_t pending, loaded;
247 991112 : get_statistics(pending, loaded);
248 :
249 991112 : float current_height = 0.0f;
250 991112 : height_above_terrain(current_height, extrapolate);
251 :
252 : const mavlink_terrain_report_t packet {
253 991112 : loc.lat,
254 991112 : loc.lng,
255 : terrain_height,
256 : current_height,
257 : spacing,
258 : pending,
259 : loaded
260 991112 : };
261 :
262 991112 : mavlink_msg_terrain_report_send_struct(link.get_chan(), &packet);
263 : #endif
264 : }
265 :
266 : /*
267 : handle TERRAIN_CHECK messages from GCS
268 : */
269 15 : void AP_Terrain::handle_terrain_check(GCS_MAVLINK &link, const mavlink_message_t &msg)
270 : {
271 : mavlink_terrain_check_t packet;
272 15 : mavlink_msg_terrain_check_decode(&msg, &packet);
273 15 : Location loc;
274 15 : loc.lat = packet.lat;
275 15 : loc.lng = packet.lon;
276 15 : send_terrain_report(link, loc, false);
277 15 : }
278 :
279 : /*
280 : handle TERRAIN_DATA messages from GCS
281 : */
282 0 : void AP_Terrain::handle_terrain_data(const mavlink_message_t &msg)
283 : {
284 : mavlink_terrain_data_t packet;
285 0 : mavlink_msg_terrain_data_decode(&msg, &packet);
286 :
287 : uint16_t i;
288 0 : for (i=0; i<cache_size; i++) {
289 0 : if (TERRAIN_LATLON_EQUAL(cache[i].grid.lat,packet.lat) &&
290 0 : TERRAIN_LATLON_EQUAL(cache[i].grid.lon,packet.lon) &&
291 0 : cache[i].grid.spacing == packet.grid_spacing &&
292 0 : grid_spacing == packet.grid_spacing &&
293 0 : packet.gridbit < 56) {
294 0 : break;
295 : }
296 : }
297 0 : if (i == cache_size) {
298 : // we don't have that grid, ignore data
299 0 : return;
300 : }
301 0 : struct grid_cache &gcache = cache[i];
302 0 : struct grid_block &grid = gcache.grid;
303 0 : uint8_t idx_x = (packet.gridbit / TERRAIN_GRID_BLOCK_MUL_Y) * TERRAIN_GRID_MAVLINK_SIZE;
304 0 : uint8_t idx_y = (packet.gridbit % TERRAIN_GRID_BLOCK_MUL_Y) * TERRAIN_GRID_MAVLINK_SIZE;
305 : ASSERT_RANGE(idx_x,0,(TERRAIN_GRID_BLOCK_MUL_X-1)*TERRAIN_GRID_MAVLINK_SIZE);
306 : ASSERT_RANGE(idx_y,0,(TERRAIN_GRID_BLOCK_MUL_Y-1)*TERRAIN_GRID_MAVLINK_SIZE);
307 0 : for (uint8_t x=0; x<TERRAIN_GRID_MAVLINK_SIZE; x++) {
308 0 : for (uint8_t y=0; y<TERRAIN_GRID_MAVLINK_SIZE; y++) {
309 0 : grid.height[idx_x+x][idx_y+y] = packet.data[x*TERRAIN_GRID_MAVLINK_SIZE+y];
310 : }
311 : }
312 0 : gcache.grid.bitmap |= ((uint64_t)1) << packet.gridbit;
313 :
314 : // mark dirty for disk IO
315 0 : if (!diskless()) {
316 0 : gcache.state = GRID_CACHE_DIRTY;
317 : }
318 :
319 : #if TERRAIN_DEBUG
320 : hal.console->printf("Filled bit %u idx_x=%u idx_y=%u\n",
321 : (unsigned)packet.gridbit, (unsigned)idx_x, (unsigned)idx_y);
322 : if (gcache.grid.bitmap == bitmap_mask) {
323 : hal.console->printf("--lat=%12.7f --lon=%12.7f %u\n",
324 : grid.lat*1.0e-7f,
325 : grid.lon*1.0e-7f,
326 : grid.height[0][0]);
327 : Location loc2;
328 : loc2.lat = grid.lat;
329 : loc2.lng = grid.lon;
330 : loc2.offset(28*grid_spacing, 32*grid_spacing);
331 : hal.console->printf("--lat=%12.7f --lon=%12.7f %u\n",
332 : loc2.lat*1.0e-7f,
333 : loc2.lng*1.0e-7f,
334 : grid.height[27][31]);
335 : }
336 : #endif
337 :
338 : // see if we need to schedule some disk IO
339 0 : update();
340 : }
341 : #endif // HAL_GCS_ENABLED
342 :
343 :
344 : #endif // AP_TERRAIN_AVAILABLE
|
