diff --git a/libraries/AP_Filesystem/AP_Filesystem.cpp b/libraries/AP_Filesystem/AP_Filesystem.cpp index 4488f699ad..a3a8e8933b 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem.cpp @@ -43,6 +43,9 @@ static AP_Filesystem_Param fs_param; #include "AP_Filesystem_Sys.h" static AP_Filesystem_Sys fs_sys; +#include "AP_Filesystem_Mission.h" +static AP_Filesystem_Mission fs_mission; + /* mapping from filesystem prefix to backend */ @@ -54,6 +57,7 @@ const AP_Filesystem::Backend AP_Filesystem::backends[] = { { "@PARAM/", fs_param }, { "@SYS/", fs_sys }, { "@SYS", fs_sys }, + { "@MISSION/", fs_mission }, }; #define MAX_FD_PER_BACKEND 256U diff --git a/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp b/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp new file mode 100644 index 0000000000..5c6512f8cb --- /dev/null +++ b/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp @@ -0,0 +1,247 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + ArduPilot filesystem interface for mission/fence/rally + */ +#include "AP_Filesystem.h" +#include "AP_Filesystem_Mission.h" +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; +extern int errno; + +int AP_Filesystem_Mission::open(const char *fname, int flags) +{ + enum MAV_MISSION_TYPE mtype; + + if (!check_file_name(fname, mtype)) { + errno = ENOENT; + return -1; + } + if ((flags & O_ACCMODE) != O_RDONLY) { + return -1; + } + uint8_t idx; + for (idx=0; idx= max_open_file || !file[fd].open) { + errno = EBADF; + return -1; + } + struct rfile &r = file[fd]; + r.open = false; + return 0; +} + +/* + packed format: + file header: + uint16_t magic = 0x671b + uint16_t data_type MAV_MISSION_TYPE_* + uint32_t total_items + + per-entry is mavlink packed item + */ + +/* + read from file handle + */ +int32_t AP_Filesystem_Mission::read(int fd, void *buf, uint32_t count) +{ + if (fd < 0 || fd >= max_open_file || !file[fd].open) { + errno = EBADF; + return -1; + } + + struct rfile &r = file[fd]; + size_t header_total = 0; + uint8_t *ubuf = (uint8_t *)buf; + + if (r.file_ofs < sizeof(struct header)) { + struct header hdr; + hdr.data_type = uint16_t(r.mtype); + hdr.num_items = r.num_items; + uint8_t n = MIN(sizeof(hdr) - r.file_ofs, count); + const uint8_t *b = (const uint8_t *)&hdr; + memcpy(ubuf, &b[r.file_ofs], n); + count -= n; + header_total += n; + r.file_ofs += n; + ubuf += n; + if (count == 0) { + return header_total; + } + } + + uint32_t data_ofs = r.file_ofs - sizeof(struct header); + mavlink_mission_item_int_t item; + const uint8_t item_size = MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN; + uint32_t item_ofs = data_ofs % item_size; + uint32_t total = 0; + + while (count > 0) { + uint32_t item_idx = data_ofs / item_size; + + const uint8_t *ibuf = (const uint8_t *)&item; + if (!get_item(item_idx, r.mtype, item)) { + break; + } + const uint8_t n = MIN(item_size - item_ofs, count); + memcpy(ubuf, &ibuf[item_ofs], n); + ubuf += n; + count -= n; + total += n; + item_ofs = 0; + data_ofs += n; + } + + r.file_ofs += total; + return total + header_total; +} + +int32_t AP_Filesystem_Mission::lseek(int fd, int32_t offset, int seek_from) +{ + if (fd < 0 || fd >= max_open_file || !file[fd].open) { + errno = EBADF; + return -1; + } + struct rfile &r = file[fd]; + switch (seek_from) { + case SEEK_SET: + r.file_ofs = offset; + break; + case SEEK_CUR: + r.file_ofs += offset; + break; + case SEEK_END: + errno = EINVAL; + return -1; + } + return r.file_ofs; +} + +int AP_Filesystem_Mission::stat(const char *name, struct stat *stbuf) +{ + enum MAV_MISSION_TYPE mtype; + if (!check_file_name(name, mtype)) { + errno = ENOENT; + return -1; + } + memset(stbuf, 0, sizeof(*stbuf)); + // give fixed size to avoid needing to scan entire file + stbuf->st_size = 1024*1024; + return 0; +} + +/* + check for the right file name + */ +bool AP_Filesystem_Mission::check_file_name(const char *name, enum MAV_MISSION_TYPE &mtype) +{ + if (strcmp(name, "mission.dat") == 0) { + mtype = MAV_MISSION_TYPE_MISSION; + return true; + } + if (strcmp(name, "fence.dat") == 0) { + mtype = MAV_MISSION_TYPE_FENCE; + return true; + } + if (strcmp(name, "rally.dat") == 0) { + mtype = MAV_MISSION_TYPE_RALLY; + return true; + } + return false; +} + +/* + get one item + */ +bool AP_Filesystem_Mission::get_item(uint32_t idx, enum MAV_MISSION_TYPE mtype, mavlink_mission_item_int_t &item) +{ + switch (mtype) { + case MAV_MISSION_TYPE_MISSION: { + auto *mission = AP::mission(); + if (!mission) { + return false; + } + return mission->get_item(idx, item); + } + case MAV_MISSION_TYPE_FENCE: + return MissionItemProtocol_Fence::get_item_as_mission_item(idx, item); + + case MAV_MISSION_TYPE_RALLY: + return MissionItemProtocol_Rally::get_item_as_mission_item(idx, item); + + default: + break; + } + return false; +} + +// get number of items +uint32_t AP_Filesystem_Mission::get_num_items(enum MAV_MISSION_TYPE mtype) const +{ + switch (mtype) { + case MAV_MISSION_TYPE_MISSION: { + auto *mission = AP::mission(); + if (!mission) { + return 0; + } + return mission->num_commands(); + } + + case MAV_MISSION_TYPE_FENCE: { + auto *fence = AP::fence(); + if (fence == nullptr) { + return 0; + } + return fence->polyfence().num_stored_items(); + } + + case MAV_MISSION_TYPE_RALLY: { + auto *rally = AP::rally(); + if (rally == nullptr) { + return 0; + } + return rally->get_rally_total(); + } + + default: + break; + } + return 0; +} diff --git a/libraries/AP_Filesystem/AP_Filesystem_Mission.h b/libraries/AP_Filesystem/AP_Filesystem_Mission.h new file mode 100644 index 0000000000..aaa215a641 --- /dev/null +++ b/libraries/AP_Filesystem/AP_Filesystem_Mission.h @@ -0,0 +1,56 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#pragma once + +#include "AP_Filesystem_backend.h" +#include + +class AP_Filesystem_Mission : public AP_Filesystem_Backend +{ +public: + // functions that closely match the equivalent posix calls + int open(const char *fname, int flags) override; + int close(int fd) override; + int32_t read(int fd, void *buf, uint32_t count) override; + int32_t lseek(int fd, int32_t offset, int whence) override; + int stat(const char *pathname, struct stat *stbuf) override; + +private: + // only allow up to 4 files at a time + static constexpr uint8_t max_open_file = 4; + + // header at front of the file + struct header { + uint16_t magic = 0x763d; + uint16_t data_type; // MAV_MISSION_TYPE_* + uint32_t num_items; + }; + + struct rfile { + bool open; + uint32_t file_ofs; + uint32_t num_items; + enum MAV_MISSION_TYPE mtype; + } file[max_open_file]; + + bool check_file_name(const char *fname, enum MAV_MISSION_TYPE &mtype); + + // get one item + bool get_item(uint32_t idx, enum MAV_MISSION_TYPE mtype, mavlink_mission_item_int_t &item); + + // get number of items + uint32_t get_num_items(enum MAV_MISSION_TYPE mtype) const; +};