mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-23 14:57:35 +08:00
WIP: Mavlink file server
This commit is contained in:
px4dev
@@ -0,0 +1,377 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <crc32.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include "mavlink_ftp.h"
|
||||
|
||||
MavlinkFTP *MavlinkFTP::_server;
|
||||
|
||||
MavlinkFTP *
|
||||
MavlinkFTP::getServer()
|
||||
{
|
||||
// XXX this really cries out for some locking...
|
||||
if (_server == nullptr) {
|
||||
_server = new MavlinkFTP;
|
||||
}
|
||||
return _server;
|
||||
}
|
||||
|
||||
MavlinkFTP::MavlinkFTP()
|
||||
{
|
||||
// initialise the request freelist
|
||||
dq_init(&_workFree);
|
||||
sem_init(&_lock, 0, 1);
|
||||
|
||||
// drop work entries onto the free list
|
||||
for (unsigned i = 0; i < kRequestQueueSize; i++) {
|
||||
_qFree(&_workBufs[i]);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkFTP::handle_message(mavlink_message_t *msg, mavlink_channel_t channel)
|
||||
{
|
||||
// get a free request
|
||||
auto req = _dqFree();
|
||||
|
||||
// if we couldn't get a request slot, just drop it
|
||||
if (req != nullptr) {
|
||||
|
||||
// decode the request
|
||||
req->decode(channel, msg);
|
||||
|
||||
// and queue it for the worker
|
||||
work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkFTP::_workerTrampoline(void *arg)
|
||||
{
|
||||
auto req = reinterpret_cast<Request *>(arg);
|
||||
auto server = MavlinkFTP::getServer();
|
||||
|
||||
// call the server worker with the work item
|
||||
server->_worker(req);
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkFTP::_worker(Request *req)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
ErrorCode errorCode = kErrNone;
|
||||
uint32_t messageCRC;
|
||||
|
||||
// basic sanity checks; must validate length before use
|
||||
if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) {
|
||||
errorCode = kErrNoRequest;
|
||||
goto out;
|
||||
}
|
||||
|
||||
// check request CRC to make sure this is one of ours
|
||||
messageCRC = hdr->crc32;
|
||||
hdr->crc32 = 0;
|
||||
if (crc32(req->data(), req->dataSize()) != messageCRC) {
|
||||
errorCode = kErrNoRequest;
|
||||
goto out;
|
||||
}
|
||||
|
||||
switch (hdr->opcode) {
|
||||
case kCmdNone:
|
||||
break;
|
||||
|
||||
case kCmdTerminate:
|
||||
if (!Session::terminate(hdr->session)) {
|
||||
errorCode = kErrNoRequest;
|
||||
}
|
||||
break;
|
||||
|
||||
case kCmdReset:
|
||||
Session::reset();
|
||||
break;
|
||||
|
||||
case kCmdList:
|
||||
errorCode = _workList(req);
|
||||
break;
|
||||
|
||||
case kCmdOpen:
|
||||
errorCode = _workOpen(req, false);
|
||||
break;
|
||||
|
||||
case kCmdCreate:
|
||||
errorCode = _workOpen(req, true);
|
||||
break;
|
||||
|
||||
case kCmdRead:
|
||||
errorCode = _workRead(req);
|
||||
break;
|
||||
|
||||
case kCmdWrite:
|
||||
errorCode = _workWrite(req);
|
||||
break;
|
||||
|
||||
case kCmdRemove:
|
||||
errorCode = _workRemove(req);
|
||||
break;
|
||||
|
||||
default:
|
||||
errorCode = kErrNoRequest;
|
||||
break;
|
||||
}
|
||||
|
||||
out:
|
||||
// handle success vs. error
|
||||
if (errorCode == kErrNone) {
|
||||
hdr->opcode = kRspAck;
|
||||
} else {
|
||||
hdr->opcode = kRspNak;
|
||||
hdr->size = 1;
|
||||
hdr->data[0] = errorCode;
|
||||
}
|
||||
|
||||
// respond to the request
|
||||
_reply(req);
|
||||
|
||||
// free the request buffer back to the freelist
|
||||
_qFree(req);
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkFTP::_reply(Request *req)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
|
||||
// message is assumed to be already constructed in the request buffer, so generate the CRC
|
||||
hdr->crc32 = 0;
|
||||
hdr->crc32 = crc32(req->data(), req->dataSize());
|
||||
|
||||
// then pack and send the reply back to the request source
|
||||
mavlink_msg_encapsulated_data_send(req->channel, req->sequence(), req->data());
|
||||
}
|
||||
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workList(Request *req)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
|
||||
// open directory
|
||||
|
||||
// seek in directory
|
||||
|
||||
// read entries until buffer is full
|
||||
|
||||
// send reply
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workOpen(Request *req, bool create)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
|
||||
// allocate session ID
|
||||
int session = Session::allocate();
|
||||
if (session < 0) {
|
||||
return kErrNoSession;
|
||||
}
|
||||
|
||||
// get the session to open the file
|
||||
if (!Session::get(session)->open(req->dataAsCString(), create)) {
|
||||
return create ? kErrPerm : kErrNotFile;
|
||||
}
|
||||
|
||||
// save the session ID in the reply
|
||||
hdr->session = session;
|
||||
hdr->size = 0;
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workRead(Request *req)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
|
||||
// look up session
|
||||
auto session = Session::get(hdr->session);
|
||||
if (session == nullptr) {
|
||||
return kErrNoSession;
|
||||
}
|
||||
|
||||
// read from file
|
||||
int result = session->read(hdr->offset, &hdr->data[0], hdr->size);
|
||||
|
||||
if (result < 0) {
|
||||
return kErrIO;
|
||||
}
|
||||
hdr->size = result;
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workWrite(Request *req)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
|
||||
// look up session
|
||||
auto session = Session::get(hdr->session);
|
||||
if (session == nullptr) {
|
||||
return kErrNoSession;
|
||||
}
|
||||
|
||||
// append to file
|
||||
int result = session->append(hdr->offset, &hdr->data[0], hdr->size);
|
||||
|
||||
if (result < 0) {
|
||||
// XXX might also be no space, I/O, etc.
|
||||
return kErrNotAppend;
|
||||
}
|
||||
|
||||
hdr->size = result;
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workRemove(Request *req)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
|
||||
// for now, send error reply
|
||||
return kErrPerm;
|
||||
}
|
||||
|
||||
MavlinkFTP::Session MavlinkFTP::Session::_sessions[MavlinkFTP::Session::kMaxSession];
|
||||
|
||||
int
|
||||
MavlinkFTP::Session::allocate()
|
||||
{
|
||||
for (unsigned i = 0; i < kMaxSession; i++) {
|
||||
if (_sessions[i]._fd < 0) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
MavlinkFTP::Session *
|
||||
MavlinkFTP::Session::get(unsigned index)
|
||||
{
|
||||
if ((index >= kMaxSession) || (_sessions[index]._fd < 0)) {
|
||||
return nullptr;
|
||||
}
|
||||
return &_sessions[index];
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkFTP::Session::terminate()
|
||||
{
|
||||
// clean up aborted transfers?
|
||||
if (_fd >= 0) {
|
||||
close(_fd);
|
||||
_fd = -1;
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
MavlinkFTP::Session::terminate(unsigned index)
|
||||
{
|
||||
Session *session = get(index);
|
||||
|
||||
if (session == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
session->terminate();
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkFTP::Session::reset()
|
||||
{
|
||||
for (unsigned i = 0; i < kMaxSession; i++) {
|
||||
terminate(i);
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
MavlinkFTP::Session::open(const char *path, bool create)
|
||||
{
|
||||
int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
|
||||
|
||||
_fd = open(path, oflag);
|
||||
if (_fd < 0) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
int
|
||||
MavlinkFTP::Session::read(off_t offset, uint8_t *buf, uint8_t count)
|
||||
{
|
||||
// can we seek to the location?
|
||||
if (lseek(_fd, offset, SEEK_SET) < 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
return read(_fd, buf, count);
|
||||
}
|
||||
|
||||
int
|
||||
MavlinkFTP::Session::append(off_t offset, uint8_t *buf, uint8_t count)
|
||||
{
|
||||
// make sure that the requested offset matches our current position
|
||||
off_t pos = lseek(_fd, 0, SEEK_CUR);
|
||||
if (pos != offset) {
|
||||
return -1;
|
||||
}
|
||||
return write(_fd, buf, count);
|
||||
}
|
||||
|
||||
char *
|
||||
MavlinkFTP::Request::dataAsCString()
|
||||
{
|
||||
// guarantee nul termination
|
||||
if (header()->size < kMaxDataLength) {
|
||||
data()[header()->size] = '\0';
|
||||
} else {
|
||||
data()[kMaxDataLength - 1] = '\0';
|
||||
}
|
||||
|
||||
// and return data
|
||||
return (char *)data();
|
||||
}
|
||||
@@ -0,0 +1,211 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
/**
|
||||
* @file mavlink_ftp.h
|
||||
*
|
||||
* MAVLink remote file server.
|
||||
*
|
||||
* Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes
|
||||
* a session ID and sequence number.
|
||||
*
|
||||
* A limited number of requests (currently 2) may be outstanding at a time.
|
||||
* Additional messages will be discarded.
|
||||
*
|
||||
* Messages consist of a fixed header, followed by a data area.
|
||||
*
|
||||
*/
|
||||
|
||||
#include <dirent.h>
|
||||
#include <queue.h>
|
||||
|
||||
#include <nuttx/wqueue.h>
|
||||
|
||||
#include "mavlink_messages.h"
|
||||
|
||||
class MavlinkFTP
|
||||
{
|
||||
public:
|
||||
MavlinkFTP();
|
||||
|
||||
static MavlinkFTP *getServer();
|
||||
|
||||
// static interface
|
||||
void handle_message(mavlink_message_t *msg,
|
||||
mavlink_channel_t channel);
|
||||
|
||||
private:
|
||||
|
||||
static const unsigned kRequestQueueSize = 2;
|
||||
|
||||
static MavlinkFTP *_server;
|
||||
|
||||
struct RequestHeader
|
||||
{
|
||||
uint8_t magic;
|
||||
uint8_t session;
|
||||
uint8_t opcode;
|
||||
uint8_t size;
|
||||
uint32_t crc32;
|
||||
uint32_t offset;
|
||||
uint8_t data[];
|
||||
};
|
||||
|
||||
struct FileList
|
||||
{
|
||||
uint32_t fileSize;
|
||||
uint8_t nameLength;
|
||||
uint8_t name[];
|
||||
};
|
||||
|
||||
enum Opcode : uint8_t
|
||||
{
|
||||
kCmdNone, // ignored, always acked
|
||||
kCmdTerminate, // releases sessionID, closes file
|
||||
kCmdReset, // terminates all sessions
|
||||
kCmdList, // list files in <path> from <offset>
|
||||
kCmdOpen, // opens <path> for reading, returns <session>
|
||||
kCmdRead, // reads <size> bytes from <offset> in <session>
|
||||
kCmdCreate, // creates <path> for writing, returns <session>
|
||||
kCmdWrite, // appends <size> bytes at <offset> in <session>
|
||||
kCmdRemove, // remove file (only if created by server?)
|
||||
|
||||
kRspAck,
|
||||
kRspNak
|
||||
};
|
||||
|
||||
enum ErrorCode : uint8_t
|
||||
{
|
||||
kErrNone,
|
||||
kErrNoRequest,
|
||||
kErrNoSession,
|
||||
kErrSequence,
|
||||
kErrNotDir,
|
||||
kErrNotFile,
|
||||
kErrEOF,
|
||||
kErrNotAppend,
|
||||
kErrTooBig,
|
||||
kErrIO,
|
||||
kErrPerm
|
||||
};
|
||||
|
||||
class Session
|
||||
{
|
||||
public:
|
||||
Session() : _fd(-1) {}
|
||||
|
||||
static int allocate();
|
||||
static Session *get(unsigned index);
|
||||
static bool terminate(unsigned index);
|
||||
static void reset();
|
||||
|
||||
void terminate();
|
||||
bool open(const char *path, bool create);
|
||||
int read(off_t offset, uint8_t *buf, uint8_t count);
|
||||
int append(off_t offset, uint8_t *buf, uint8_t count);
|
||||
|
||||
private:
|
||||
static const unsigned kMaxSession = 2;
|
||||
static Session _sessions[kMaxSession];
|
||||
|
||||
int _fd;
|
||||
};
|
||||
|
||||
class Request
|
||||
{
|
||||
public:
|
||||
union {
|
||||
dq_entry_t entry;
|
||||
work_s work;
|
||||
};
|
||||
mavlink_channel_t channel;
|
||||
|
||||
void decode(mavlink_channel_t fromChannel, mavlink_message_t *fromMessage) {
|
||||
channel = fromChannel;
|
||||
mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
|
||||
}
|
||||
|
||||
RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
|
||||
uint8_t *data() { return &_message.data[0]; }
|
||||
unsigned dataSize() { return header()->size + sizeof(RequestHeader); }
|
||||
uint16_t sequence() const { return _message.seqnr; }
|
||||
|
||||
char *dataAsCString();
|
||||
|
||||
private:
|
||||
mavlink_encapsulated_data_t _message;
|
||||
|
||||
};
|
||||
|
||||
static const uint8_t kProtocolMagic = 'f';
|
||||
static const uint8_t kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader);
|
||||
|
||||
/// Request worker; runs on the low-priority work queue to service
|
||||
/// remote requests.
|
||||
///
|
||||
static void _workerTrampoline(void *arg);
|
||||
void _worker(Request *req);
|
||||
|
||||
/// Reply to a request (XXX should be a Request method)
|
||||
///
|
||||
void _reply(Request *req);
|
||||
|
||||
ErrorCode _workList(Request *req);
|
||||
ErrorCode _workOpen(Request *req, bool create);
|
||||
ErrorCode _workRead(Request *req);
|
||||
ErrorCode _workWrite(Request *req);
|
||||
ErrorCode _workRemove(Request *req);
|
||||
|
||||
// work freelist
|
||||
Request _workBufs[kRequestQueueSize];
|
||||
dq_queue_t _workFree;
|
||||
sem_t _lock;
|
||||
|
||||
void _qLock() { do {} while (sem_wait(&_lock) != 0); }
|
||||
void _qUnlock() { sem_post(&_lock); }
|
||||
|
||||
void _qFree(Request *req) {
|
||||
_qLock();
|
||||
dq_addlast(&req->entry, &_workFree);
|
||||
_qUnlock();
|
||||
}
|
||||
|
||||
Request *_dqFree() {
|
||||
_qLock();
|
||||
auto req = reinterpret_cast<Request *>(dq_remfirst(&_workFree));
|
||||
_qUnlock();
|
||||
return req;
|
||||
}
|
||||
};
|
||||
@@ -112,6 +112,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
||||
_hil_local_alt0(0.0)
|
||||
{
|
||||
memset(&hil_local_pos, 0, sizeof(hil_local_pos));
|
||||
|
||||
// make sure the FTP server is started
|
||||
(void)MavlinkFTP::getServer();
|
||||
}
|
||||
|
||||
MavlinkReceiver::~MavlinkReceiver()
|
||||
@@ -150,6 +153,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
||||
handle_message_manual_control(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
|
||||
MavlinkFTP::getServer()->handle_message(msg, _mavlink->get_channel());
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -68,6 +68,8 @@
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
|
||||
#include "mavlink_ftp.h"
|
||||
|
||||
class Mavlink;
|
||||
|
||||
class MavlinkReceiver
|
||||
|
||||
@@ -43,7 +43,8 @@ SRCS += mavlink_main.cpp \
|
||||
mavlink_messages.cpp \
|
||||
mavlink_stream.cpp \
|
||||
mavlink_rate_limiter.cpp \
|
||||
mavlink_commands.cpp
|
||||
mavlink_commands.cpp \
|
||||
mavlink_ftp.cpp
|
||||
|
||||
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
|
||||
|
||||
|
||||
Reference in New Issue
Block a user