AP_Filesystem: add filesystem backend for LocationDB

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;

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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  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 <GCS_MAVLink/GCS.h>
+#include <AP_AHRS/AP_AHRS.h>
+
+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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "AP_Filesystem_config.h"
+
+#if AP_FILESYSTEM_LOCATIONDB_ENABLED
+
+#include "AP_Filesystem_backend.h"
+#include <AP_LocationDB/AP_LocationDB.h>
+
+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

libraries/AP_Filesystem/AP_Filesystem_config.h

@@ -3,6 +3,7 @@
 #include <AP_HAL/AP_HAL_Boards.h>
 
 #include <AP_Mission/AP_Mission_config.h>
+#include <AP_LocationDB/AP_LocationDB_config.h>
 
 // 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