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 : ArduPilot filesystem interface for mission/fence/rally
17 : */
18 :
19 : #include "AP_Filesystem_config.h"
20 :
21 : #if AP_FILESYSTEM_MISSION_ENABLED
22 :
23 : #include "AP_Filesystem.h"
24 : #include "AP_Filesystem_Mission.h"
25 : #include <AP_Mission/AP_Mission.h>
26 : #include <AC_Fence/AC_Fence.h>
27 : #include <AP_Rally/AP_Rally.h>
28 : #include <GCS_MAVLink/MissionItemProtocol_Rally.h>
29 : #include <GCS_MAVLink/MissionItemProtocol_Fence.h>
30 : #include <GCS_MAVLink/GCS.h>
31 :
32 : extern const AP_HAL::HAL& hal;
33 :
34 : // QURT HAL already has a declaration of errno in errno.h
35 : #if CONFIG_HAL_BOARD != HAL_BOARD_QURT
36 : extern int errno;
37 : #endif
38 :
39 : #define IDLE_TIMEOUT_MS 30000
40 :
41 3 : int AP_Filesystem_Mission::open(const char *fname, int flags, bool allow_absolute_paths)
42 : {
43 : enum MAV_MISSION_TYPE mtype;
44 :
45 3 : if (!check_file_name(fname, mtype)) {
46 0 : errno = ENOENT;
47 0 : return -1;
48 : }
49 : uint8_t idx;
50 3 : bool readonly = ((flags & O_ACCMODE) == O_RDONLY);
51 3 : uint32_t now = AP_HAL::millis();
52 3 : for (idx=0; idx<max_open_file; idx++) {
53 3 : if (now - file[idx].last_op_ms > IDLE_TIMEOUT_MS) {
54 1 : file[idx].open = false;
55 1 : delete file[idx].writebuf;
56 1 : file[idx].writebuf = nullptr;
57 : }
58 3 : if (!readonly && file[idx].writebuf != nullptr) {
59 : // only one upload at a time
60 0 : return -1;
61 : }
62 3 : if (!file[idx].open) {
63 3 : break;
64 : }
65 : }
66 3 : if (idx == max_open_file) {
67 0 : errno = ENFILE;
68 0 : return -1;
69 : }
70 3 : struct rfile &r = file[idx];
71 3 : r.file_ofs = 0;
72 3 : r.open = true;
73 3 : r.mtype = mtype;
74 3 : r.num_items = get_num_items(r.mtype);
75 3 : if (!readonly) {
76 : // setup for upload
77 0 : r.writebuf = NEW_NOTHROW ExpandingString();
78 : } else {
79 3 : r.writebuf = nullptr;
80 : }
81 3 : r.last_op_ms = now;
82 :
83 3 : return idx;
84 : }
85 :
86 3 : int AP_Filesystem_Mission::close(int fd)
87 : {
88 3 : if (fd < 0 || fd >= max_open_file || !file[fd].open) {
89 0 : errno = EBADF;
90 0 : return -1;
91 : }
92 3 : struct rfile &r = file[fd];
93 3 : r.open = false;
94 3 : if (r.writebuf != nullptr) {
95 0 : bool ok = finish_upload(r);
96 0 : delete r.writebuf;
97 0 : r.writebuf = nullptr;
98 0 : if (!ok) {
99 0 : errno = EINVAL;
100 0 : return -1;
101 : }
102 : }
103 3 : return 0;
104 : }
105 :
106 : /*
107 : packed format:
108 : file header:
109 : uint16_t magic = 0x671b
110 : uint16_t data_type MAV_MISSION_TYPE_*
111 : uint32_t total_items
112 :
113 : per-entry is mavlink packed item
114 : */
115 :
116 : /*
117 : read from file handle
118 : */
119 100 : int32_t AP_Filesystem_Mission::read(int fd, void *buf, uint32_t count)
120 : {
121 100 : if (fd < 0 || fd >= max_open_file || !file[fd].open) {
122 0 : errno = EBADF;
123 0 : return -1;
124 : }
125 :
126 100 : struct rfile &r = file[fd];
127 :
128 100 : if (r.writebuf != nullptr) {
129 0 : errno = EBADF;
130 0 : return -1;
131 : }
132 :
133 100 : r.last_op_ms = AP_HAL::millis();
134 :
135 100 : size_t header_total = 0;
136 100 : uint8_t *ubuf = (uint8_t *)buf;
137 :
138 100 : if (r.file_ofs < sizeof(struct header)) {
139 3 : struct header hdr {};
140 3 : hdr.data_type = uint16_t(r.mtype);
141 3 : hdr.num_items = r.num_items;
142 3 : uint8_t n = MIN(sizeof(hdr) - r.file_ofs, count);
143 3 : const uint8_t *b = (const uint8_t *)&hdr;
144 3 : memcpy(ubuf, &b[r.file_ofs], n);
145 3 : count -= n;
146 3 : header_total += n;
147 3 : r.file_ofs += n;
148 3 : ubuf += n;
149 3 : if (count == 0) {
150 0 : return header_total;
151 : }
152 : }
153 :
154 100 : uint32_t data_ofs = r.file_ofs - sizeof(struct header);
155 100 : mavlink_mission_item_int_t item {};
156 100 : const uint8_t item_size = MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN;
157 100 : uint32_t item_ofs = data_ofs % item_size;
158 100 : uint32_t total = 0;
159 :
160 796 : while (count > 0) {
161 701 : uint32_t item_idx = data_ofs / item_size;
162 :
163 701 : const uint8_t *ibuf = (const uint8_t *)&item;
164 701 : if (!get_item(item_idx, r.mtype, item)) {
165 5 : break;
166 : }
167 696 : const uint8_t n = MIN(item_size - item_ofs, count);
168 696 : memcpy(ubuf, &ibuf[item_ofs], n);
169 696 : ubuf += n;
170 696 : count -= n;
171 696 : total += n;
172 696 : item_ofs = 0;
173 696 : data_ofs += n;
174 : }
175 :
176 100 : r.file_ofs += total;
177 100 : return total + header_total;
178 : }
179 :
180 3 : int32_t AP_Filesystem_Mission::lseek(int fd, int32_t offset, int seek_from)
181 : {
182 3 : if (fd < 0 || fd >= max_open_file || !file[fd].open) {
183 0 : errno = EBADF;
184 0 : return -1;
185 : }
186 3 : struct rfile &r = file[fd];
187 3 : switch (seek_from) {
188 3 : case SEEK_SET:
189 3 : r.file_ofs = offset;
190 3 : break;
191 0 : case SEEK_CUR:
192 0 : r.file_ofs += offset;
193 0 : break;
194 0 : case SEEK_END:
195 0 : errno = EINVAL;
196 0 : return -1;
197 : }
198 3 : return r.file_ofs;
199 : }
200 :
201 3 : int AP_Filesystem_Mission::stat(const char *name, struct stat *stbuf)
202 : {
203 : enum MAV_MISSION_TYPE mtype;
204 3 : if (!check_file_name(name, mtype)) {
205 0 : errno = ENOENT;
206 0 : return -1;
207 : }
208 3 : memset(stbuf, 0, sizeof(*stbuf));
209 3 : stbuf->st_size = sizeof(struct header) + get_num_items(mtype) * MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN;
210 3 : return 0;
211 : }
212 :
213 : /*
214 : check for the right file name
215 : */
216 6 : bool AP_Filesystem_Mission::check_file_name(const char *name, enum MAV_MISSION_TYPE &mtype)
217 : {
218 : #if AP_MISSION_ENABLED
219 6 : if (strcmp(name, "mission.dat") == 0) {
220 6 : mtype = MAV_MISSION_TYPE_MISSION;
221 6 : return true;
222 : }
223 : #endif
224 : #if AP_FENCE_ENABLED
225 0 : if (strcmp(name, "fence.dat") == 0) {
226 0 : mtype = MAV_MISSION_TYPE_FENCE;
227 0 : return true;
228 : }
229 : #endif
230 : #if HAL_RALLY_ENABLED
231 0 : if (strcmp(name, "rally.dat") == 0) {
232 0 : mtype = MAV_MISSION_TYPE_RALLY;
233 0 : return true;
234 : }
235 : #endif
236 0 : return false;
237 : }
238 :
239 : /*
240 : get one item
241 : */
242 701 : bool AP_Filesystem_Mission::get_item(uint32_t idx, enum MAV_MISSION_TYPE mtype, mavlink_mission_item_int_t &item) const
243 : {
244 701 : switch (mtype) {
245 701 : case MAV_MISSION_TYPE_MISSION: {
246 701 : return AP::mission().get_item(idx, item);
247 : }
248 : #if AP_FENCE_ENABLED
249 0 : case MAV_MISSION_TYPE_FENCE:
250 0 : return MissionItemProtocol_Fence::get_item_as_mission_item(idx, item);
251 : #endif
252 : #if HAL_RALLY_ENABLED
253 0 : case MAV_MISSION_TYPE_RALLY:
254 0 : return MissionItemProtocol_Rally::get_item_as_mission_item(idx, item);
255 : #endif
256 0 : default:
257 0 : break;
258 : }
259 0 : return false;
260 : }
261 :
262 : // get number of items
263 6 : uint32_t AP_Filesystem_Mission::get_num_items(enum MAV_MISSION_TYPE mtype) const
264 : {
265 6 : switch (mtype) {
266 : #if AP_MISSION_ENABLED
267 6 : case MAV_MISSION_TYPE_MISSION: {
268 6 : return AP::mission().num_commands();
269 : }
270 : #endif
271 :
272 : #if AP_FENCE_ENABLED
273 0 : case MAV_MISSION_TYPE_FENCE: {
274 0 : auto *fence = AP::fence();
275 0 : if (fence == nullptr) {
276 0 : return 0;
277 : }
278 0 : return fence->polyfence().num_stored_items();
279 : }
280 : #endif
281 :
282 : #if HAL_RALLY_ENABLED
283 0 : case MAV_MISSION_TYPE_RALLY: {
284 0 : auto *rally = AP::rally();
285 0 : if (rally == nullptr) {
286 0 : return 0;
287 : }
288 0 : return rally->get_rally_total();
289 : }
290 : #endif
291 :
292 0 : default:
293 0 : break;
294 : }
295 0 : return 0;
296 : }
297 :
298 : /*
299 : support mission upload
300 : */
301 0 : int32_t AP_Filesystem_Mission::write(int fd, const void *buf, uint32_t count)
302 : {
303 0 : if (fd < 0 || fd >= max_open_file || !file[fd].open) {
304 0 : errno = EBADF;
305 0 : return -1;
306 : }
307 0 : struct rfile &r = file[fd];
308 0 : if (r.writebuf == nullptr) {
309 0 : errno = EBADF;
310 0 : return -1;
311 : }
312 0 : r.last_op_ms = AP_HAL::millis();
313 0 : struct header hdr;
314 0 : if (r.file_ofs == 0 && count >= sizeof(hdr)) {
315 : // pre-expand the buffer to the full size when we get the header
316 0 : memcpy(&hdr, buf, sizeof(hdr));
317 0 : if (hdr.num_items < 0xFFFF) {
318 0 : const uint32_t flen = sizeof(hdr) + hdr.num_items * MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN;
319 0 : if (flen > r.writebuf->get_length()) {
320 0 : if (!r.writebuf->append(nullptr, flen - r.writebuf->get_length())) {
321 : // not enough memory
322 0 : errno = ENOSPC;
323 0 : return -1;
324 : }
325 : }
326 : }
327 : }
328 0 : if (r.file_ofs + count > r.writebuf->get_length()) {
329 0 : if (!r.writebuf->append(nullptr, r.file_ofs + count - r.writebuf->get_length())) {
330 0 : errno = ENOSPC;
331 0 : return -1;
332 : }
333 : }
334 0 : uint8_t *b = (uint8_t *)r.writebuf->get_writeable_string();
335 0 : memcpy(&b[r.file_ofs], buf, count);
336 0 : r.file_ofs += count;
337 0 : return count;
338 : }
339 :
340 : // see if a block of memory is all zero
341 0 : bool AP_Filesystem_Mission::all_zero(const uint8_t *b, uint8_t len) const
342 : {
343 0 : while (len--) {
344 0 : if (*b++ != 0) {
345 0 : return false;
346 : }
347 : }
348 0 : return true;
349 : }
350 :
351 : /*
352 : finish mission upload
353 : */
354 0 : bool AP_Filesystem_Mission::finish_upload(const rfile &r)
355 : {
356 0 : const uint32_t flen = r.writebuf->get_length();
357 0 : const uint8_t *b = (const uint8_t *)r.writebuf->get_string();
358 0 : struct header hdr;
359 0 : if (flen < sizeof(hdr)) {
360 0 : return false;
361 : }
362 0 : const uint8_t item_size = MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN;
363 0 : const uint32_t nitems = (flen - sizeof(hdr)) / item_size;
364 :
365 0 : memcpy(&hdr, b, sizeof(hdr));
366 0 : if (hdr.magic != mission_magic) {
367 0 : return false;
368 : }
369 0 : if (nitems != hdr.num_items) {
370 0 : return false;
371 : }
372 :
373 : // if any item is all zeros then reject, it means client didn't
374 : // fill in the whole file
375 0 : for (uint32_t i=0; i<nitems; i++) {
376 0 : const uint8_t *b2 = b + sizeof(hdr) + i*item_size;
377 0 : if (all_zero(b2, item_size)) {
378 0 : return false;
379 : }
380 : }
381 :
382 0 : switch (r.mtype) {
383 : #if AP_MISSION_ENABLED
384 0 : case MAV_MISSION_TYPE_MISSION:
385 0 : return finish_upload_mission(hdr, r, b);
386 : #endif
387 : #if HAL_RALLY_ENABLED
388 0 : case MAV_MISSION_TYPE_RALLY:
389 0 : return finish_upload_rally(hdr, r, b);
390 : #endif
391 : #if AP_FENCE_ENABLED
392 0 : case MAV_MISSION_TYPE_FENCE:
393 0 : return finish_upload_fence(hdr, r, b);
394 : #endif
395 0 : default:
396 : // really should not get here....
397 0 : break;
398 : }
399 :
400 0 : return false;
401 : }
402 :
403 : #if AP_MISSION_ENABLED
404 0 : bool AP_Filesystem_Mission::finish_upload_mission(const struct header &hdr, const rfile &r, const uint8_t *b)
405 : {
406 0 : auto &mission = AP::mission();
407 0 : WITH_SEMAPHORE(mission.get_semaphore());
408 0 : if ((hdr.options & unsigned(Options::NO_CLEAR)) == 0) {
409 0 : mission.clear();
410 : }
411 0 : for (uint32_t i=0; i<hdr.num_items; i++) {
412 0 : mavlink_mission_item_int_t m {};
413 0 : AP_Mission::Mission_Command cmd;
414 0 : const uint8_t item_size = MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN;
415 0 : memcpy(&m, &b[sizeof(hdr)+i*item_size], item_size);
416 0 : const MAV_MISSION_RESULT res = AP_Mission::mavlink_int_to_mission_cmd(m, cmd);
417 0 : if (res != MAV_MISSION_ACCEPTED) {
418 0 : return false;
419 : }
420 0 : if (cmd.id == MAV_CMD_DO_JUMP &&
421 0 : (cmd.content.jump.target >= hdr.num_items || cmd.content.jump.target == 0)) {
422 0 : return false;
423 : }
424 0 : uint16_t idx = i + hdr.start;
425 0 : if (idx == mission.num_commands()) {
426 0 : if (!mission.add_cmd(cmd)) {
427 0 : return false;
428 : }
429 : } else {
430 0 : if (!mission.replace_cmd(idx, cmd)) {
431 0 : return false;
432 : }
433 : }
434 : }
435 0 : return true;
436 0 : }
437 : #endif // AP_MISSION_ENABLED
438 :
439 : #if AP_FENCE_ENABLED
440 0 : bool AP_Filesystem_Mission::finish_upload_fence(const struct header &hdr, const rfile &r, const uint8_t *b)
441 : {
442 0 : bool success = false;
443 :
444 0 : AC_PolyFenceItem *new_items = nullptr;
445 :
446 0 : auto *fence = AP::fence();
447 0 : if (fence == nullptr) {
448 0 : goto OUT;
449 : }
450 :
451 0 : if ((hdr.options & unsigned(Options::NO_CLEAR)) != 0) {
452 : // Only complete fences can be uploaded for now.
453 0 : goto OUT;
454 : }
455 :
456 : // passing nullptr and 0 items through to Polyfence loader is
457 : // absolutely OK:
458 0 : if (hdr.num_items != 0) {
459 0 : new_items = NEW_NOTHROW AC_PolyFenceItem[hdr.num_items];
460 0 : if (new_items == nullptr) {
461 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Out of memory for upload");
462 0 : goto OUT;
463 : }
464 : }
465 :
466 : // convert from MISSION_ITEM_INT to AC_PolyFenceItem:
467 0 : for (uint32_t i=0; i<hdr.num_items; i++) {
468 0 : mavlink_mission_item_int_t m {};
469 0 : const uint8_t item_size = MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN;
470 0 : memcpy(&m, &b[sizeof(hdr)+i*item_size], item_size);
471 0 : const MAV_MISSION_RESULT res = MissionItemProtocol_Fence::convert_MISSION_ITEM_INT_to_AC_PolyFenceItem(m, new_items[i]);
472 0 : if (res != MAV_MISSION_ACCEPTED) {
473 0 : goto OUT;
474 : }
475 : }
476 :
477 0 : success = fence->polyfence().write_fence(new_items, hdr.num_items);
478 :
479 0 : OUT:
480 :
481 0 : delete[] new_items;
482 :
483 0 : return success;
484 : }
485 : #endif // AP_FENCE_ENABLED
486 :
487 : #if HAL_RALLY_ENABLED
488 0 : bool AP_Filesystem_Mission::finish_upload_rally(const struct header &hdr, const rfile &r, const uint8_t *b)
489 : {
490 0 : bool success = false;
491 :
492 0 : auto *rally = AP::rally();
493 0 : if (rally == nullptr) {
494 0 : goto OUT;
495 : }
496 :
497 0 : if ((hdr.options & unsigned(Options::NO_CLEAR)) != 0) {
498 : //only complete sets of rally points can be added ATM
499 0 : goto OUT;
500 : }
501 :
502 0 : rally->truncate(0);
503 :
504 0 : for (uint32_t i=0; i<hdr.num_items; i++) {
505 0 : mavlink_mission_item_int_t m {};
506 : RallyLocation cmd;
507 0 : const uint8_t item_size = MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN;
508 0 : memcpy(&m, &b[sizeof(hdr)+i*item_size], item_size);
509 0 : const MAV_MISSION_RESULT res = MissionItemProtocol_Rally::convert_MISSION_ITEM_INT_to_RallyLocation(m, cmd);
510 0 : if (res != MAV_MISSION_ACCEPTED) {
511 0 : goto OUT;
512 : }
513 :
514 0 : if (!rally->append(cmd)) {
515 0 : goto OUT;
516 : }
517 : }
518 0 : success = true;
519 :
520 0 : OUT:
521 0 : return success;
522 : }
523 : #endif // HAL_RALLY_ENABLED
524 :
525 : #endif // AP_FILESYSTEM_MISSION_ENABLED
|
