From 2dc7f33041f21af6b37a97a856b8c6f3917132c8 Mon Sep 17 00:00:00 2001 From: Shiv Tyagi Date: Sun, 4 Jun 2023 14:41:49 +0530 Subject: [PATCH] AP_Filesystem: add filesystem backend for LocationDB --- libraries/AP_Filesystem/AP_Filesystem.cpp | 8 + .../AP_Filesystem_LocationDB.cpp | 256 ++++++++++++++++++ .../AP_Filesystem/AP_Filesystem_LocationDB.h | 71 +++++ .../AP_Filesystem/AP_Filesystem_config.h | 5 + 4 files changed, 340 insertions(+) create mode 100644 libraries/AP_Filesystem/AP_Filesystem_LocationDB.cpp create mode 100644 libraries/AP_Filesystem/AP_Filesystem_LocationDB.h diff --git a/libraries/AP_Filesystem/AP_Filesystem.cpp b/libraries/AP_Filesystem/AP_Filesystem.cpp index 258e39f7dddf1e..32012e92770759 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem.cpp @@ -56,6 +56,11 @@ static AP_Filesystem_Sys fs_sys; static AP_Filesystem_Mission fs_mission; #endif +#if AP_FILESYSTEM_LOCATIONDB_ENABLED +#include "AP_Filesystem_LocationDB.h" +static AP_Filesystem_LocationDB fs_locationdb; +#endif + /* mapping from filesystem prefix to backend */ @@ -74,6 +79,9 @@ const AP_Filesystem::Backend AP_Filesystem::backends[] = { #if AP_FILESYSTEM_MISSION_ENABLED { "@MISSION/", fs_mission }, #endif +#if AP_FILESYSTEM_LOCATIONDB_ENABLED + { "@LOCATIONDB/", fs_locationdb}, +#endif }; extern const AP_HAL::HAL& hal; diff --git a/libraries/AP_Filesystem/AP_Filesystem_LocationDB.cpp b/libraries/AP_Filesystem/AP_Filesystem_LocationDB.cpp new file mode 100644 index 00000000000000..76714c7aefa00a --- /dev/null +++ b/libraries/AP_Filesystem/AP_Filesystem_LocationDB.cpp @@ -0,0 +1,256 @@ +/* + 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 LocationDB + */ + +#include "AP_Filesystem_config.h" + +#if AP_FILESYSTEM_LOCATIONDB_ENABLED + +#include "AP_Filesystem.h" +#include "AP_Filesystem_LocationDB.h" +#include +#include + +extern const AP_HAL::HAL& hal; +extern int errno; + +#define IDLE_TIMEOUT_MS 30000 + +int AP_Filesystem_LocationDB::open(const char *fname, int flags, bool allow_absolute_paths) +{ + auto *db = AP::locationdb(); + if (!check_file_name(fname) || !db) { + errno = ENOENT; + return -1; + } + + bool readonly = ((flags & O_ACCMODE) == O_RDONLY); + uint32_t now = AP_HAL::millis(); + if (now - file.last_op_ms > IDLE_TIMEOUT_MS) { + file.open = false; + } + if (!readonly) { + // we only allow reads + return -1; + } + + struct rfile &r = file; + r.file_ofs = 0; + r.open = true; + r.num_items = db->size(); + r.last_op_ms = now; + + return 0; +} + +int AP_Filesystem_LocationDB::close(int fd) +{ + if (fd < 0 || fd >= 1 || !file.open) { + errno = EBADF; + return -1; + } + struct rfile &r = file; + r.open = false; + return 0; +} + +/* + packed format: + file header: + uint16_t magic = 0x2801 + uint16_t total_items + + per-entry is AP_LocationDB_Item + */ + +/* + read from file handle + */ +int32_t AP_Filesystem_LocationDB::read(int fd, void *buf, uint32_t count) +{ + if (fd < 0 || fd >= 1 || !file.open) { + errno = EBADF; + return -1; + } + + struct rfile &r = file; + + r.last_op_ms = AP_HAL::millis(); + + size_t header_total = 0; + uint8_t *ubuf = (uint8_t *)buf; + + if (r.file_ofs < sizeof(struct header)) { + struct header hdr {}; + 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); + const uint8_t item_size = sizeof(packet_t); + uint32_t total = 0; + + while (count > 0) { + uint32_t item_idx = data_ofs / item_size; + uint32_t item_ofs = data_ofs % item_size; + AP_LocationDB_Item item; + if (!get_item(item_idx, item)) { + break; + } + + packet_t packet = construct_packet_from_DBItem(item); + const uint8_t *ibuf = (const uint8_t *)&packet; + + const uint8_t compressed_packet_size = sizeof(packet_header) + packet.pkt_header.filled_fields_count * sizeof(float); + + if (item_ofs >= compressed_packet_size) { + // this item has been transferred + // move to next item + data_ofs += (item_size - item_ofs); + continue; + } + + const uint8_t n = MIN(compressed_packet_size - item_ofs, count); + memcpy(ubuf, &ibuf[item_ofs], n); + + ubuf += n; + count -= n; + total += n; + data_ofs += n; + } + + r.file_ofs = data_ofs + sizeof(struct header); + return total + header_total; +} + +int32_t AP_Filesystem_LocationDB::lseek(int fd, int32_t offset, int seek_from) +{ + if (fd < 0 || fd >= 1 || !file.open) { + errno = EBADF; + return -1; + } + struct rfile &r = file; + 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_LocationDB::stat(const char *name, struct stat *stbuf) +{ + auto *db = AP::locationdb(); + if (!check_file_name(name) || !db) { + errno = ENOENT; + return -1; + } + memset(stbuf, 0, sizeof(*stbuf)); + // give fixed size to avoid needing to scan entire file + stbuf->st_size = db->size() * AP_LOCATIONDB_ITEM_SIZE; + return 0; +} + +/* + check for the right file name + */ +bool AP_Filesystem_LocationDB::check_file_name(const char *name) +{ + if (strcmp(name, "locationdb.dat") == 0) { + return true; + } + return false; +} + +AP_Filesystem_LocationDB::packet_t AP_Filesystem_LocationDB::construct_packet_from_DBItem(AP_LocationDB_Item item) +{ + packet_t ret; + + ret.pkt_header.key = item.get_key(); + ret.pkt_header.timestamp_ms = item.get_timestamp_ms(); + ret.pkt_header.populated_fields = item._populated_fields; + + Vector3f pos; + Location item_loc; + uint8_t field_index = 0; + if (item.get_pos_cm_NEU(pos)) { + item_loc = Location(pos, Location::AltFrame::ABOVE_ORIGIN); +#if AP_AHRS_ENABLED + IGNORE_RETURN(item_loc.change_alt_frame(Location::AltFrame::ABSOLUTE)); +#endif + ret.fields[field_index++] = item_loc.lat; + ret.fields[field_index++] = item_loc.lng; + ret.fields[field_index++] = item_loc.alt; + } + + Vector3f vel; + if (item.get_vel_cm_NEU(vel)) { + ret.fields[field_index++] = vel.x; + ret.fields[field_index++] = vel.y; + ret.fields[field_index++] = vel.z; + } + + Vector3f acc; + if (item.get_acc_cm_NEU(acc)) { + ret.fields[field_index++] = acc.x; + ret.fields[field_index++] = acc.y; + ret.fields[field_index++] = acc.z; + } + + float heading; + if (item.get_heading_cdeg(heading)) { + ret.fields[field_index++] = heading; + } + + float radius; + if (item.get_radius_cm(radius)) { + ret.fields[field_index++] = radius; + } + + ret.pkt_header.filled_fields_count = field_index; + return ret; +} + +/* + get one item + */ +bool AP_Filesystem_LocationDB::get_item(uint16_t idx, AP_LocationDB_Item &item) const +{ + auto *db = AP::locationdb(); + if (!db) { + return false; + } + + return db->get_item_at_index(idx, item); +} + +#endif diff --git a/libraries/AP_Filesystem/AP_Filesystem_LocationDB.h b/libraries/AP_Filesystem/AP_Filesystem_LocationDB.h new file mode 100644 index 00000000000000..c83995c06e970c --- /dev/null +++ b/libraries/AP_Filesystem/AP_Filesystem_LocationDB.h @@ -0,0 +1,71 @@ +/* + 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_config.h" + +#if AP_FILESYSTEM_LOCATIONDB_ENABLED + +#include "AP_Filesystem_backend.h" +#include + +class AP_Filesystem_LocationDB : public AP_Filesystem_Backend +{ +public: + // functions that closely match the equivalent posix calls + int open(const char *fname, int flags, bool allow_absolute_paths = false) 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: + static constexpr uint16_t locationdb_magic = 0x2801; + + // header at front of the file + struct header { + uint16_t magic = locationdb_magic; + uint16_t num_items; + }; + + struct rfile { + bool open; + uint32_t file_ofs; + uint16_t num_items; + uint32_t last_op_ms; + } file; + + struct PACKED packet_header { + uint32_t key; + uint32_t timestamp_ms; + uint8_t populated_fields; + uint8_t filled_fields_count; + }; + + struct PACKED packet_t { + packet_header pkt_header; + float fields[11]; + }; + + bool check_file_name(const char *fname); + + // get one item + bool get_item(uint16_t idx, AP_LocationDB_Item &item) const; + packet_t construct_packet_from_DBItem(AP_LocationDB_Item item); + bool skip_byte(AP_LocationDB_Item &item, uint32_t byte_num) const; +}; + +#endif diff --git a/libraries/AP_Filesystem/AP_Filesystem_config.h b/libraries/AP_Filesystem/AP_Filesystem_config.h index 75f3aa001c224b..fddc1ec867aeca 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_config.h +++ b/libraries/AP_Filesystem/AP_Filesystem_config.h @@ -3,6 +3,7 @@ #include #include +#include // backends: @@ -34,6 +35,10 @@ #define AP_FILESYSTEM_SYS_ENABLED 1 #endif +#ifndef AP_FILESYSTEM_LOCATIONDB_ENABLED +#define AP_FILESYSTEM_LOCATIONDB_ENABLED AP_LOCATIONDB_ENABLED +#endif + // AP_FILESYSTEM_FILE_READING_ENABLED is true if you could expect to // be able to open and write a non-virtual file. Notably this // excludes virtual files like SYSFS, and the magic param/mission