mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-16 15:33:33 +08:00
Support for building more modules with Linux
Added more queue support to linux/px4_layer. Use virt char devices for ms5611, and mavlink. Added more HRT functionality. uORB latency test now fails. Likely due to bad HRT impl for Linux. Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
@@ -39,7 +39,7 @@
|
||||
#include <dirent.h>
|
||||
#include <queue.h>
|
||||
|
||||
#include <nuttx/wqueue.h>
|
||||
#include <px4_workqueue.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include "mavlink_messages.h"
|
||||
|
||||
@@ -0,0 +1,861 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/// @file mavlink_ftp.cpp
|
||||
/// @author px4dev, Don Gagne <don@thegagnes.com>
|
||||
|
||||
#include <crc32.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/stat.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include "mavlink_ftp.h"
|
||||
#include "mavlink_tests/mavlink_ftp_test.h"
|
||||
|
||||
// Uncomment the line below to get better debug output. Never commit with this left on.
|
||||
//#define MAVLINK_FTP_DEBUG
|
||||
|
||||
MavlinkFTP *
|
||||
MavlinkFTP::get_server(void)
|
||||
{
|
||||
static MavlinkFTP server;
|
||||
return &server;
|
||||
}
|
||||
|
||||
MavlinkFTP::MavlinkFTP() :
|
||||
_request_bufs{},
|
||||
_request_queue{},
|
||||
_request_queue_sem{},
|
||||
_utRcvMsgFunc{},
|
||||
_ftp_test{}
|
||||
{
|
||||
// initialise the request freelist
|
||||
dq_init(&_request_queue);
|
||||
sem_init(&_request_queue_sem, 0, 1);
|
||||
|
||||
// initialize session list
|
||||
for (size_t i=0; i<kMaxSession; i++) {
|
||||
_session_fds[i] = -1;
|
||||
}
|
||||
|
||||
// drop work entries onto the free list
|
||||
for (unsigned i = 0; i < kRequestQueueSize; i++) {
|
||||
_return_request(&_request_bufs[i]);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#ifdef MAVLINK_FTP_UNIT_TEST
|
||||
void
|
||||
MavlinkFTP::set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test)
|
||||
{
|
||||
_utRcvMsgFunc = rcvMsgFunc;
|
||||
_ftp_test = ftp_test;
|
||||
}
|
||||
#endif
|
||||
|
||||
void
|
||||
MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
|
||||
{
|
||||
// get a free request
|
||||
struct Request* req = _get_request();
|
||||
|
||||
// if we couldn't get a request slot, just drop it
|
||||
if (req == nullptr) {
|
||||
warnx("Dropping FTP request: queue full\n");
|
||||
return;
|
||||
}
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
|
||||
mavlink_msg_file_transfer_protocol_decode(msg, &req->message);
|
||||
|
||||
#ifdef MAVLINK_FTP_UNIT_TEST
|
||||
if (!_utRcvMsgFunc) {
|
||||
warnx("Incorrectly written unit test\n");
|
||||
return;
|
||||
}
|
||||
// We use fake ids when unit testing
|
||||
req->serverSystemId = MavlinkFtpTest::serverSystemId;
|
||||
req->serverComponentId = MavlinkFtpTest::serverComponentId;
|
||||
req->serverChannel = MavlinkFtpTest::serverChannel;
|
||||
#else
|
||||
// Not unit testing, use the real thing
|
||||
req->serverSystemId = mavlink->get_system_id();
|
||||
req->serverComponentId = mavlink->get_component_id();
|
||||
req->serverChannel = mavlink->get_channel();
|
||||
#endif
|
||||
|
||||
// This is the system id we want to target when sending
|
||||
req->targetSystemId = msg->sysid;
|
||||
|
||||
if (req->message.target_system == req->serverSystemId) {
|
||||
req->mavlink = mavlink;
|
||||
#ifdef MAVLINK_FTP_UNIT_TEST
|
||||
// We are running in Unit Test mode. Don't queue, just call _worket directly.
|
||||
_process_request(req);
|
||||
#else
|
||||
// We are running in normal mode. Queue the request to the worker
|
||||
work_queue(LPWORK, &req->work, &MavlinkFTP::_worker_trampoline, req, 0);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
_return_request(req);
|
||||
}
|
||||
|
||||
/// @brief Queued static work queue routine to handle mavlink messages
|
||||
void
|
||||
MavlinkFTP::_worker_trampoline(void *arg)
|
||||
{
|
||||
Request* req = reinterpret_cast<Request *>(arg);
|
||||
MavlinkFTP* server = MavlinkFTP::get_server();
|
||||
|
||||
// call the server worker with the work item
|
||||
server->_process_request(req);
|
||||
}
|
||||
|
||||
/// @brief Processes an FTP message
|
||||
void
|
||||
MavlinkFTP::_process_request(Request *req)
|
||||
{
|
||||
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
|
||||
|
||||
ErrorCode errorCode = kErrNone;
|
||||
|
||||
// basic sanity checks; must validate length before use
|
||||
if (payload->size > kMaxDataLength) {
|
||||
errorCode = kErrInvalidDataSize;
|
||||
goto out;
|
||||
}
|
||||
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset);
|
||||
#endif
|
||||
|
||||
switch (payload->opcode) {
|
||||
case kCmdNone:
|
||||
break;
|
||||
|
||||
case kCmdTerminateSession:
|
||||
errorCode = _workTerminate(payload);
|
||||
break;
|
||||
|
||||
case kCmdResetSessions:
|
||||
errorCode = _workReset(payload);
|
||||
break;
|
||||
|
||||
case kCmdListDirectory:
|
||||
errorCode = _workList(payload);
|
||||
break;
|
||||
|
||||
case kCmdOpenFileRO:
|
||||
errorCode = _workOpen(payload, O_RDONLY);
|
||||
break;
|
||||
|
||||
case kCmdCreateFile:
|
||||
errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY);
|
||||
break;
|
||||
|
||||
case kCmdOpenFileWO:
|
||||
errorCode = _workOpen(payload, O_CREAT | O_WRONLY);
|
||||
break;
|
||||
|
||||
case kCmdReadFile:
|
||||
errorCode = _workRead(payload);
|
||||
break;
|
||||
|
||||
case kCmdWriteFile:
|
||||
errorCode = _workWrite(payload);
|
||||
break;
|
||||
|
||||
case kCmdRemoveFile:
|
||||
errorCode = _workRemoveFile(payload);
|
||||
break;
|
||||
|
||||
case kCmdRename:
|
||||
errorCode = _workRename(payload);
|
||||
break;
|
||||
|
||||
case kCmdTruncateFile:
|
||||
errorCode = _workTruncateFile(payload);
|
||||
break;
|
||||
|
||||
case kCmdCreateDirectory:
|
||||
errorCode = _workCreateDirectory(payload);
|
||||
break;
|
||||
|
||||
case kCmdRemoveDirectory:
|
||||
errorCode = _workRemoveDirectory(payload);
|
||||
break;
|
||||
|
||||
|
||||
case kCmdCalcFileCRC32:
|
||||
errorCode = _workCalcFileCRC32(payload);
|
||||
break;
|
||||
|
||||
default:
|
||||
errorCode = kErrUnknownCommand;
|
||||
break;
|
||||
}
|
||||
|
||||
out:
|
||||
// handle success vs. error
|
||||
if (errorCode == kErrNone) {
|
||||
payload->req_opcode = payload->opcode;
|
||||
payload->opcode = kRspAck;
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
warnx("FTP: ack\n");
|
||||
#endif
|
||||
} else {
|
||||
int r_errno = errno;
|
||||
warnx("FTP: nak %u", errorCode);
|
||||
payload->req_opcode = payload->opcode;
|
||||
payload->opcode = kRspNak;
|
||||
payload->size = 1;
|
||||
payload->data[0] = errorCode;
|
||||
if (errorCode == kErrFailErrno) {
|
||||
payload->size = 2;
|
||||
payload->data[1] = r_errno;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// respond to the request
|
||||
_reply(req);
|
||||
|
||||
_return_request(req);
|
||||
}
|
||||
|
||||
/// @brief Sends the specified FTP reponse message out through mavlink
|
||||
void
|
||||
MavlinkFTP::_reply(Request *req)
|
||||
{
|
||||
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
|
||||
|
||||
payload->seqNumber = payload->seqNumber + 1;
|
||||
|
||||
mavlink_message_t msg;
|
||||
msg.checksum = 0;
|
||||
#ifndef MAVLINK_FTP_UNIT_TEST
|
||||
uint16_t len =
|
||||
#endif
|
||||
mavlink_msg_file_transfer_protocol_pack_chan(req->serverSystemId, // Sender system id
|
||||
req->serverComponentId, // Sender component id
|
||||
req->serverChannel, // Channel to send on
|
||||
&msg, // Message to pack payload into
|
||||
0, // Target network
|
||||
req->targetSystemId, // Target system id
|
||||
0, // Target component id
|
||||
(const uint8_t*)payload); // Payload to pack into message
|
||||
|
||||
bool success = true;
|
||||
#ifdef MAVLINK_FTP_UNIT_TEST
|
||||
// Unit test hook is set, call that instead
|
||||
_utRcvMsgFunc(&msg, _ftp_test);
|
||||
#else
|
||||
Mavlink *mavlink = req->mavlink;
|
||||
|
||||
mavlink->lockMessageBufferMutex();
|
||||
success = mavlink->message_buffer_write(&msg, len);
|
||||
mavlink->unlockMessageBufferMutex();
|
||||
|
||||
#endif
|
||||
|
||||
if (!success) {
|
||||
warnx("FTP TX ERR");
|
||||
}
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
else {
|
||||
warnx("wrote: sys: %d, comp: %d, chan: %d, checksum: %d",
|
||||
req->serverSystemId,
|
||||
req->serverComponentId,
|
||||
req->serverChannel,
|
||||
msg.checksum);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
/// @brief Responds to a List command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workList(PayloadHeader* payload)
|
||||
{
|
||||
char dirPath[kMaxDataLength];
|
||||
strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength);
|
||||
|
||||
DIR *dp = opendir(dirPath);
|
||||
|
||||
if (dp == nullptr) {
|
||||
warnx("FTP: can't open path '%s'", dirPath);
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
warnx("FTP: list %s offset %d", dirPath, payload->offset);
|
||||
#endif
|
||||
|
||||
ErrorCode errorCode = kErrNone;
|
||||
struct dirent entry, *result = nullptr;
|
||||
unsigned offset = 0;
|
||||
|
||||
// move to the requested offset
|
||||
seekdir(dp, payload->offset);
|
||||
|
||||
for (;;) {
|
||||
// read the directory entry
|
||||
if (readdir_r(dp, &entry, &result)) {
|
||||
warnx("FTP: list %s readdir_r failure\n", dirPath);
|
||||
errorCode = kErrFailErrno;
|
||||
break;
|
||||
}
|
||||
|
||||
// no more entries?
|
||||
if (result == nullptr) {
|
||||
if (payload->offset != 0 && offset == 0) {
|
||||
// User is requesting subsequent dir entries but there were none. This means the user asked
|
||||
// to seek past EOF.
|
||||
errorCode = kErrEOF;
|
||||
}
|
||||
// Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that
|
||||
break;
|
||||
}
|
||||
|
||||
uint32_t fileSize = 0;
|
||||
char buf[256];
|
||||
char direntType;
|
||||
|
||||
// Determine the directory entry type
|
||||
switch (entry.d_type) {
|
||||
#ifdef __PX4_NUTTX
|
||||
case DTYPE_FILE:
|
||||
#else
|
||||
case DT_REG:
|
||||
#endif
|
||||
// For files we get the file size as well
|
||||
direntType = kDirentFile;
|
||||
snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name);
|
||||
struct stat st;
|
||||
if (stat(buf, &st) == 0) {
|
||||
fileSize = st.st_size;
|
||||
}
|
||||
break;
|
||||
#ifdef __PX4_NUTTX
|
||||
case DTYPE_DIRECTORY:
|
||||
#else
|
||||
case DT_DIR:
|
||||
#endif
|
||||
if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
|
||||
// Don't bother sending these back
|
||||
direntType = kDirentSkip;
|
||||
} else {
|
||||
direntType = kDirentDir;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
// We only send back file and diretory entries, skip everything else
|
||||
direntType = kDirentSkip;
|
||||
}
|
||||
|
||||
if (direntType == kDirentSkip) {
|
||||
// Skip send only dirent identifier
|
||||
buf[0] = '\0';
|
||||
} else if (direntType == kDirentFile) {
|
||||
// Files send filename and file length
|
||||
snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize);
|
||||
} else {
|
||||
// Everything else just sends name
|
||||
strncpy(buf, entry.d_name, sizeof(buf));
|
||||
buf[sizeof(buf)-1] = 0;
|
||||
}
|
||||
size_t nameLen = strlen(buf);
|
||||
|
||||
// Do we have room for the name, the one char directory identifier and the null terminator?
|
||||
if ((offset + nameLen + 2) > kMaxDataLength) {
|
||||
break;
|
||||
}
|
||||
|
||||
// Move the data into the buffer
|
||||
payload->data[offset++] = direntType;
|
||||
strcpy((char *)&payload->data[offset], buf);
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
printf("FTP: list %s %s\n", dirPath, (char *)&payload->data[offset-1]);
|
||||
#endif
|
||||
offset += nameLen + 1;
|
||||
}
|
||||
|
||||
closedir(dp);
|
||||
payload->size = offset;
|
||||
|
||||
return errorCode;
|
||||
}
|
||||
|
||||
/// @brief Responds to an Open command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
|
||||
{
|
||||
int session_index = _find_unused_session();
|
||||
if (session_index < 0) {
|
||||
warnx("FTP: Open failed - out of sessions\n");
|
||||
return kErrNoSessionsAvailable;
|
||||
}
|
||||
|
||||
char *filename = _data_as_cstring(payload);
|
||||
|
||||
uint32_t fileSize = 0;
|
||||
struct stat st;
|
||||
if (stat(filename, &st) != 0) {
|
||||
// fail only if requested open for read
|
||||
if (oflag & O_RDONLY)
|
||||
return kErrFailErrno;
|
||||
else
|
||||
st.st_size = 0;
|
||||
}
|
||||
fileSize = st.st_size;
|
||||
|
||||
int fd = ::open(filename, oflag);
|
||||
if (fd < 0) {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
_session_fds[session_index] = fd;
|
||||
|
||||
payload->session = session_index;
|
||||
payload->size = sizeof(uint32_t);
|
||||
*((uint32_t*)payload->data) = fileSize;
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Responds to a Read command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workRead(PayloadHeader* payload)
|
||||
{
|
||||
int session_index = payload->session;
|
||||
|
||||
if (!_valid_session(session_index)) {
|
||||
return kErrInvalidSession;
|
||||
}
|
||||
|
||||
// Seek to the specified position
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
warnx("seek %d", payload->offset);
|
||||
#endif
|
||||
if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) {
|
||||
// Unable to see to the specified location
|
||||
warnx("seek fail");
|
||||
return kErrEOF;
|
||||
}
|
||||
|
||||
int bytes_read = ::read(_session_fds[session_index], &payload->data[0], kMaxDataLength);
|
||||
if (bytes_read < 0) {
|
||||
// Negative return indicates error other than eof
|
||||
warnx("read fail %d", bytes_read);
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
payload->size = bytes_read;
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Responds to a Write command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workWrite(PayloadHeader* payload)
|
||||
{
|
||||
int session_index = payload->session;
|
||||
|
||||
if (!_valid_session(session_index)) {
|
||||
return kErrInvalidSession;
|
||||
}
|
||||
|
||||
// Seek to the specified position
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
warnx("seek %d", payload->offset);
|
||||
#endif
|
||||
if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) {
|
||||
// Unable to see to the specified location
|
||||
warnx("seek fail");
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
int bytes_written = ::write(_session_fds[session_index], &payload->data[0], payload->size);
|
||||
if (bytes_written < 0) {
|
||||
// Negative return indicates error other than eof
|
||||
warnx("write fail %d", bytes_written);
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
payload->size = sizeof(uint32_t);
|
||||
*((uint32_t*)payload->data) = bytes_written;
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Responds to a RemoveFile command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workRemoveFile(PayloadHeader* payload)
|
||||
{
|
||||
char file[kMaxDataLength];
|
||||
strncpy(file, _data_as_cstring(payload), kMaxDataLength);
|
||||
|
||||
if (unlink(file) == 0) {
|
||||
payload->size = 0;
|
||||
return kErrNone;
|
||||
} else {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Responds to a TruncateFile command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
|
||||
{
|
||||
char file[kMaxDataLength];
|
||||
const char temp_file[] = "/fs/microsd/.trunc.tmp";
|
||||
strncpy(file, _data_as_cstring(payload), kMaxDataLength);
|
||||
payload->size = 0;
|
||||
|
||||
// emulate truncate(file, payload->offset) by
|
||||
// copying to temp and overwrite with O_TRUNC flag.
|
||||
|
||||
struct stat st;
|
||||
if (stat(file, &st) != 0) {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
if (!S_ISREG(st.st_mode)) {
|
||||
errno = EISDIR;
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
// check perms allow us to write (not romfs)
|
||||
if (!(st.st_mode & (S_IWUSR | S_IWGRP | S_IWOTH))) {
|
||||
errno = EROFS;
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
if (payload->offset == (unsigned)st.st_size) {
|
||||
// nothing to do
|
||||
return kErrNone;
|
||||
}
|
||||
else if (payload->offset == 0) {
|
||||
// 1: truncate all data
|
||||
int fd = ::open(file, O_TRUNC | O_WRONLY);
|
||||
if (fd < 0) {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
::close(fd);
|
||||
return kErrNone;
|
||||
}
|
||||
else if (payload->offset > (unsigned)st.st_size) {
|
||||
// 2: extend file
|
||||
int fd = ::open(file, O_WRONLY);
|
||||
if (fd < 0) {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
if (lseek(fd, payload->offset - 1, SEEK_SET) < 0) {
|
||||
::close(fd);
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
bool ok = 1 == ::write(fd, "", 1);
|
||||
::close(fd);
|
||||
|
||||
return (ok)? kErrNone : kErrFailErrno;
|
||||
}
|
||||
else {
|
||||
// 3: truncate
|
||||
if (_copy_file(file, temp_file, payload->offset) != 0) {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
if (_copy_file(temp_file, file, payload->offset) != 0) {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
if (::unlink(temp_file) != 0) {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Responds to a Terminate command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workTerminate(PayloadHeader* payload)
|
||||
{
|
||||
if (!_valid_session(payload->session)) {
|
||||
return kErrInvalidSession;
|
||||
}
|
||||
|
||||
::close(_session_fds[payload->session]);
|
||||
_session_fds[payload->session] = -1;
|
||||
|
||||
payload->size = 0;
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Responds to a Reset command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workReset(PayloadHeader* payload)
|
||||
{
|
||||
for (size_t i=0; i<kMaxSession; i++) {
|
||||
if (_session_fds[i] != -1) {
|
||||
::close(_session_fds[i]);
|
||||
_session_fds[i] = -1;
|
||||
}
|
||||
}
|
||||
|
||||
payload->size = 0;
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Responds to a Rename command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workRename(PayloadHeader* payload)
|
||||
{
|
||||
char oldpath[kMaxDataLength];
|
||||
char newpath[kMaxDataLength];
|
||||
|
||||
char *ptr = _data_as_cstring(payload);
|
||||
size_t oldpath_sz = strlen(ptr);
|
||||
|
||||
if (oldpath_sz == payload->size) {
|
||||
// no newpath
|
||||
errno = EINVAL;
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
strncpy(oldpath, ptr, kMaxDataLength);
|
||||
strncpy(newpath, ptr + oldpath_sz + 1, kMaxDataLength);
|
||||
|
||||
if (rename(oldpath, newpath) == 0) {
|
||||
payload->size = 0;
|
||||
return kErrNone;
|
||||
} else {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Responds to a RemoveDirectory command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload)
|
||||
{
|
||||
char dir[kMaxDataLength];
|
||||
strncpy(dir, _data_as_cstring(payload), kMaxDataLength);
|
||||
|
||||
if (rmdir(dir) == 0) {
|
||||
payload->size = 0;
|
||||
return kErrNone;
|
||||
} else {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Responds to a CreateDirectory command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workCreateDirectory(PayloadHeader* payload)
|
||||
{
|
||||
char dir[kMaxDataLength];
|
||||
strncpy(dir, _data_as_cstring(payload), kMaxDataLength);
|
||||
|
||||
if (mkdir(dir, S_IRWXU | S_IRWXG | S_IRWXO) == 0) {
|
||||
payload->size = 0;
|
||||
return kErrNone;
|
||||
} else {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Responds to a CalcFileCRC32 command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload)
|
||||
{
|
||||
char file_buf[256];
|
||||
uint32_t checksum = 0;
|
||||
ssize_t bytes_read;
|
||||
strncpy(file_buf, _data_as_cstring(payload), kMaxDataLength);
|
||||
|
||||
int fd = ::open(file_buf, O_RDONLY);
|
||||
if (fd < 0) {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
do {
|
||||
bytes_read = ::read(fd, file_buf, sizeof(file_buf));
|
||||
if (bytes_read < 0) {
|
||||
int r_errno = errno;
|
||||
::close(fd);
|
||||
errno = r_errno;
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
checksum = crc32part((uint8_t*)file_buf, bytes_read, checksum);
|
||||
} while (bytes_read == sizeof(file_buf));
|
||||
|
||||
::close(fd);
|
||||
|
||||
payload->size = sizeof(uint32_t);
|
||||
*((uint32_t*)payload->data) = checksum;
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Returns true if the specified session is a valid open session
|
||||
bool
|
||||
MavlinkFTP::_valid_session(unsigned index)
|
||||
{
|
||||
if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Returns an unused session index
|
||||
int
|
||||
MavlinkFTP::_find_unused_session(void)
|
||||
{
|
||||
for (size_t i=0; i<kMaxSession; i++) {
|
||||
if (_session_fds[i] == -1) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
/// @brief Guarantees that the payload data is null terminated.
|
||||
/// @return Returns a pointer to the payload data as a char *
|
||||
char *
|
||||
MavlinkFTP::_data_as_cstring(PayloadHeader* payload)
|
||||
{
|
||||
// guarantee nul termination
|
||||
if (payload->size < kMaxDataLength) {
|
||||
payload->data[payload->size] = '\0';
|
||||
} else {
|
||||
payload->data[kMaxDataLength - 1] = '\0';
|
||||
}
|
||||
|
||||
// and return data
|
||||
return (char *)&(payload->data[0]);
|
||||
}
|
||||
|
||||
/// @brief Returns a unused Request entry. NULL if none available.
|
||||
MavlinkFTP::Request *
|
||||
MavlinkFTP::_get_request(void)
|
||||
{
|
||||
_lock_request_queue();
|
||||
Request* req = reinterpret_cast<Request *>(dq_remfirst(&_request_queue));
|
||||
_unlock_request_queue();
|
||||
return req;
|
||||
}
|
||||
|
||||
/// @brief Locks a semaphore to provide exclusive access to the request queue
|
||||
void
|
||||
MavlinkFTP::_lock_request_queue(void)
|
||||
{
|
||||
do {}
|
||||
while (sem_wait(&_request_queue_sem) != 0);
|
||||
}
|
||||
|
||||
/// @brief Unlocks the semaphore providing exclusive access to the request queue
|
||||
void
|
||||
MavlinkFTP::_unlock_request_queue(void)
|
||||
{
|
||||
sem_post(&_request_queue_sem);
|
||||
}
|
||||
|
||||
/// @brief Returns a no longer needed request to the queue
|
||||
void
|
||||
MavlinkFTP::_return_request(Request *req)
|
||||
{
|
||||
_lock_request_queue();
|
||||
dq_addlast(&req->work.dq, &_request_queue);
|
||||
_unlock_request_queue();
|
||||
}
|
||||
|
||||
/// @brief Copy file (with limited space)
|
||||
int
|
||||
MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length)
|
||||
{
|
||||
char buff[512];
|
||||
int src_fd = -1, dst_fd = -1;
|
||||
int op_errno = 0;
|
||||
|
||||
src_fd = ::open(src_path, O_RDONLY);
|
||||
if (src_fd < 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY, 0x0777);
|
||||
if (dst_fd < 0) {
|
||||
op_errno = errno;
|
||||
::close(src_fd);
|
||||
errno = op_errno;
|
||||
return -1;
|
||||
}
|
||||
|
||||
while (length > 0) {
|
||||
ssize_t bytes_read, bytes_written;
|
||||
size_t blen = (length > sizeof(buff))? sizeof(buff) : length;
|
||||
|
||||
bytes_read = ::read(src_fd, buff, blen);
|
||||
if (bytes_read == 0) {
|
||||
// EOF
|
||||
break;
|
||||
}
|
||||
else if (bytes_read < 0) {
|
||||
warnx("cp: read");
|
||||
op_errno = errno;
|
||||
break;
|
||||
}
|
||||
|
||||
bytes_written = ::write(dst_fd, buff, bytes_read);
|
||||
if (bytes_written != bytes_read) {
|
||||
warnx("cp: short write");
|
||||
op_errno = errno;
|
||||
break;
|
||||
}
|
||||
|
||||
length -= bytes_written;
|
||||
}
|
||||
|
||||
::close(src_fd);
|
||||
::close(dst_fd);
|
||||
|
||||
errno = op_errno;
|
||||
return (length > 0)? -1 : 0;
|
||||
}
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2015 Mark Charlebois. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -30,17 +30,9 @@
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mavlink_main.h
|
||||
* MAVLink 1.0 protocol interface definition.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
<<<<<<< HEAD
|
||||
#include <stdbool.h>
|
||||
#include <nuttx/fs/fs.h>
|
||||
#include <systemlib/param/param.h>
|
||||
@@ -413,3 +405,10 @@ private:
|
||||
Mavlink(const Mavlink&);
|
||||
Mavlink operator=(const Mavlink&);
|
||||
};
|
||||
=======
|
||||
#ifdef __PX4_NUTTX
|
||||
#include "mavlink_main_nuttx.h"
|
||||
#else
|
||||
#include "mavlink_main_linux.h"
|
||||
#endif
|
||||
>>>>>>> Support for building more modules with Linux
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,417 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mavlink_main_linux.h
|
||||
* MAVLink 1.0 protocol interface definition.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
* @author Mark Charlebois <charlebm@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <drivers/device/device.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <pthread.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
|
||||
#include "mavlink_bridge_header.h"
|
||||
#include "mavlink_orb_subscription.h"
|
||||
#include "mavlink_stream.h"
|
||||
#include "mavlink_messages.h"
|
||||
#include "mavlink_mission.h"
|
||||
#include "mavlink_parameters.h"
|
||||
|
||||
class Mavlink : public device::CDev
|
||||
{
|
||||
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
Mavlink();
|
||||
|
||||
/**
|
||||
* Destructor, also kills the mavlinks task.
|
||||
*/
|
||||
~Mavlink();
|
||||
|
||||
/**
|
||||
* Start the mavlink task.
|
||||
*
|
||||
* @return OK on success.
|
||||
*/
|
||||
static int start(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Display the mavlink status.
|
||||
*/
|
||||
void display_status();
|
||||
|
||||
static int stream_command(int argc, char *argv[]);
|
||||
|
||||
static int instance_count();
|
||||
|
||||
static Mavlink *new_instance();
|
||||
|
||||
static Mavlink *get_instance(unsigned instance);
|
||||
|
||||
static Mavlink *get_instance_for_device(const char *device_name);
|
||||
|
||||
static int destroy_all_instances();
|
||||
|
||||
static int get_status_all_instances();
|
||||
|
||||
static bool instance_exists(const char *device_name, Mavlink *self);
|
||||
|
||||
static void forward_message(const mavlink_message_t *msg, Mavlink *self);
|
||||
|
||||
static int get_uart_fd(unsigned index);
|
||||
|
||||
int get_uart_fd();
|
||||
|
||||
/**
|
||||
* Get the MAVLink system id.
|
||||
*
|
||||
* @return The system ID of this vehicle
|
||||
*/
|
||||
int get_system_id();
|
||||
|
||||
/**
|
||||
* Get the MAVLink component id.
|
||||
*
|
||||
* @return The component ID of this vehicle
|
||||
*/
|
||||
int get_component_id();
|
||||
|
||||
const char *_device_name;
|
||||
|
||||
enum MAVLINK_MODE {
|
||||
MAVLINK_MODE_NORMAL = 0,
|
||||
MAVLINK_MODE_CUSTOM,
|
||||
MAVLINK_MODE_ONBOARD
|
||||
};
|
||||
|
||||
void set_mode(enum MAVLINK_MODE);
|
||||
enum MAVLINK_MODE get_mode() { return _mode; }
|
||||
|
||||
bool get_hil_enabled() { return _hil_enabled; }
|
||||
|
||||
bool get_use_hil_gps() { return _use_hil_gps; }
|
||||
|
||||
bool get_forward_externalsp() { return _forward_externalsp; }
|
||||
|
||||
bool get_flow_control_enabled() { return _flow_control_enabled; }
|
||||
|
||||
bool get_forwarding_on() { return _forwarding_on; }
|
||||
|
||||
static int start_helper(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Handle parameter related messages.
|
||||
*/
|
||||
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
|
||||
|
||||
void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
|
||||
|
||||
/**
|
||||
* Enable / disable Hardware in the Loop simulation mode.
|
||||
*
|
||||
* @param hil_enabled The new HIL enable/disable state.
|
||||
* @return OK if the HIL state changed, ERROR if the
|
||||
* requested change could not be made or was
|
||||
* redundant.
|
||||
*/
|
||||
int set_hil_enabled(bool hil_enabled);
|
||||
|
||||
void send_message(const uint8_t msgid, const void *msg);
|
||||
|
||||
/**
|
||||
* Resend message as is, don't change sequence number and CRC.
|
||||
*/
|
||||
void resend_message(mavlink_message_t *msg);
|
||||
|
||||
void handle_message(const mavlink_message_t *msg);
|
||||
|
||||
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0);
|
||||
|
||||
int get_instance_id();
|
||||
|
||||
/**
|
||||
* Enable / disable hardware flow control.
|
||||
*
|
||||
* @param enabled True if hardware flow control should be enabled
|
||||
*/
|
||||
int enable_flow_control(bool enabled);
|
||||
|
||||
mavlink_channel_t get_channel();
|
||||
|
||||
void configure_stream_threadsafe(const char *stream_name, float rate);
|
||||
|
||||
bool _task_should_exit; /**< if true, mavlink task should exit */
|
||||
|
||||
int get_mavlink_fd() { return _mavlink_fd; }
|
||||
|
||||
/**
|
||||
* Send a status text with loglevel INFO
|
||||
*
|
||||
* @param string the message to send (will be capped by mavlink max string length)
|
||||
*/
|
||||
void send_statustext_info(const char *string);
|
||||
|
||||
/**
|
||||
* Send a status text with loglevel CRITICAL
|
||||
*
|
||||
* @param string the message to send (will be capped by mavlink max string length)
|
||||
*/
|
||||
void send_statustext_critical(const char *string);
|
||||
|
||||
/**
|
||||
* Send a status text with loglevel EMERGENCY
|
||||
*
|
||||
* @param string the message to send (will be capped by mavlink max string length)
|
||||
*/
|
||||
void send_statustext_emergency(const char *string);
|
||||
|
||||
/**
|
||||
* Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent
|
||||
* only on this mavlink connection. Useful for reporting communication specific, not system-wide info
|
||||
* only to client interested in it. Message will be not sent immediately but queued in buffer as
|
||||
* for mavlink_log_xxx().
|
||||
*
|
||||
* @param string the message to send (will be capped by mavlink max string length)
|
||||
* @param severity the log level
|
||||
*/
|
||||
void send_statustext(unsigned char severity, const char *string);
|
||||
|
||||
MavlinkStream * get_streams() const { return _streams; }
|
||||
|
||||
float get_rate_mult();
|
||||
|
||||
/* Functions for waiting to start transmission until message received. */
|
||||
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
|
||||
bool get_has_received_messages() { return _received_messages; }
|
||||
void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
|
||||
bool get_wait_to_transmit() { return _wait_to_transmit; }
|
||||
bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
|
||||
|
||||
bool message_buffer_write(const void *ptr, int size);
|
||||
|
||||
void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
|
||||
void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
|
||||
|
||||
/**
|
||||
* Count a transmision error
|
||||
*/
|
||||
void count_txerr();
|
||||
|
||||
/**
|
||||
* Count transmitted bytes
|
||||
*/
|
||||
void count_txbytes(unsigned n) { _bytes_tx += n; };
|
||||
|
||||
/**
|
||||
* Count bytes not transmitted because of errors
|
||||
*/
|
||||
void count_txerrbytes(unsigned n) { _bytes_txerr += n; };
|
||||
|
||||
/**
|
||||
* Count received bytes
|
||||
*/
|
||||
void count_rxbytes(unsigned n) { _bytes_rx += n; };
|
||||
|
||||
/**
|
||||
* Get the receive status of this MAVLink link
|
||||
*/
|
||||
struct telemetry_status_s& get_rx_status() { return _rstatus; }
|
||||
|
||||
struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; }
|
||||
|
||||
unsigned get_system_type() { return _system_type; }
|
||||
|
||||
protected:
|
||||
Mavlink *next;
|
||||
|
||||
private:
|
||||
int _instance_id;
|
||||
|
||||
int _mavlink_fd;
|
||||
bool _task_running;
|
||||
|
||||
/* states */
|
||||
bool _hil_enabled; /**< Hardware In the Loop mode */
|
||||
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
|
||||
bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */
|
||||
bool _is_usb_uart; /**< Port is USB */
|
||||
bool _wait_to_transmit; /**< Wait to transmit until received messages. */
|
||||
bool _received_messages; /**< Whether we've received valid mavlink messages. */
|
||||
|
||||
unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */
|
||||
|
||||
MavlinkOrbSubscription *_subscriptions;
|
||||
MavlinkStream *_streams;
|
||||
|
||||
MavlinkMissionManager *_mission_manager;
|
||||
MavlinkParametersManager *_parameters_manager;
|
||||
|
||||
MAVLINK_MODE _mode;
|
||||
|
||||
mavlink_channel_t _channel;
|
||||
|
||||
struct mavlink_logbuffer _logbuffer;
|
||||
unsigned int _total_counter;
|
||||
|
||||
pthread_t _receive_thread;
|
||||
|
||||
bool _verbose;
|
||||
bool _forwarding_on;
|
||||
bool _passing_on;
|
||||
bool _ftp_on;
|
||||
int _uart_fd;
|
||||
int _baudrate;
|
||||
int _datarate; ///< data rate for normal streams (attitude, position, etc.)
|
||||
int _datarate_events; ///< data rate for params, waypoints, text messages
|
||||
float _rate_mult;
|
||||
|
||||
/**
|
||||
* If the queue index is not at 0, the queue sending
|
||||
* logic will send parameters from the current index
|
||||
* to len - 1, the end of the param list.
|
||||
*/
|
||||
unsigned int _mavlink_param_queue_index;
|
||||
|
||||
bool mavlink_link_termination_allowed;
|
||||
|
||||
char *_subscribe_to_stream;
|
||||
float _subscribe_to_stream_rate;
|
||||
|
||||
bool _flow_control_enabled;
|
||||
uint64_t _last_write_success_time;
|
||||
uint64_t _last_write_try_time;
|
||||
|
||||
unsigned _bytes_tx;
|
||||
unsigned _bytes_txerr;
|
||||
unsigned _bytes_rx;
|
||||
uint64_t _bytes_timestamp;
|
||||
float _rate_tx;
|
||||
float _rate_txerr;
|
||||
float _rate_rx;
|
||||
|
||||
struct telemetry_status_s _rstatus; ///< receive status
|
||||
|
||||
struct mavlink_message_buffer {
|
||||
int write_ptr;
|
||||
int read_ptr;
|
||||
int size;
|
||||
char *data;
|
||||
};
|
||||
|
||||
mavlink_message_buffer _message_buffer;
|
||||
|
||||
pthread_mutex_t _message_buffer_mutex;
|
||||
pthread_mutex_t _send_mutex;
|
||||
|
||||
bool _param_initialized;
|
||||
param_t _param_system_id;
|
||||
param_t _param_component_id;
|
||||
param_t _param_system_type;
|
||||
param_t _param_use_hil_gps;
|
||||
param_t _param_forward_externalsp;
|
||||
|
||||
unsigned _system_type;
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
perf_counter_t _txerr_perf; /**< TX error counter */
|
||||
|
||||
void mavlink_update_system();
|
||||
|
||||
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
|
||||
|
||||
/**
|
||||
* Get the free space in the transmit buffer
|
||||
*
|
||||
* @return free space in the UART TX buffer
|
||||
*/
|
||||
unsigned get_free_tx_buf();
|
||||
|
||||
static unsigned int interval_from_rate(float rate);
|
||||
|
||||
int configure_stream(const char *stream_name, const float rate);
|
||||
|
||||
/**
|
||||
* Adjust the stream rates based on the current rate
|
||||
*
|
||||
* @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease
|
||||
*/
|
||||
void adjust_stream_rates(const float multiplier);
|
||||
|
||||
int message_buffer_init(int size);
|
||||
|
||||
void message_buffer_destroy();
|
||||
|
||||
int message_buffer_count();
|
||||
|
||||
int message_buffer_is_empty();
|
||||
|
||||
int message_buffer_get_ptr(void **ptr, bool *is_part);
|
||||
|
||||
void message_buffer_mark_read(int n);
|
||||
|
||||
void pass_message(const mavlink_message_t *msg);
|
||||
|
||||
/**
|
||||
* Update rate mult so total bitrate will be equal to _datarate.
|
||||
*/
|
||||
void update_rate_mult();
|
||||
|
||||
virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg);
|
||||
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
|
||||
|
||||
/**
|
||||
* Main mavlink task.
|
||||
*/
|
||||
int task_main(int argc, char *argv[]);
|
||||
|
||||
/* do not allow copying this class */
|
||||
Mavlink(const Mavlink&);
|
||||
Mavlink operator=(const Mavlink&);
|
||||
};
|
||||
@@ -0,0 +1,415 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mavlink_main_nuttx.h
|
||||
* MAVLink 1.0 protocol interface definition.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <nuttx/fs/fs.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <pthread.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
|
||||
#include "mavlink_bridge_header.h"
|
||||
#include "mavlink_orb_subscription.h"
|
||||
#include "mavlink_stream.h"
|
||||
#include "mavlink_messages.h"
|
||||
#include "mavlink_mission.h"
|
||||
#include "mavlink_parameters.h"
|
||||
|
||||
class Mavlink
|
||||
{
|
||||
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
Mavlink();
|
||||
|
||||
/**
|
||||
* Destructor, also kills the mavlinks task.
|
||||
*/
|
||||
~Mavlink();
|
||||
|
||||
/**
|
||||
* Start the mavlink task.
|
||||
*
|
||||
* @return OK on success.
|
||||
*/
|
||||
static int start(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Display the mavlink status.
|
||||
*/
|
||||
void display_status();
|
||||
|
||||
static int stream_command(int argc, char *argv[]);
|
||||
|
||||
static int instance_count();
|
||||
|
||||
static Mavlink *new_instance();
|
||||
|
||||
static Mavlink *get_instance(unsigned instance);
|
||||
|
||||
static Mavlink *get_instance_for_device(const char *device_name);
|
||||
|
||||
static int destroy_all_instances();
|
||||
|
||||
static int get_status_all_instances();
|
||||
|
||||
static bool instance_exists(const char *device_name, Mavlink *self);
|
||||
|
||||
static void forward_message(const mavlink_message_t *msg, Mavlink *self);
|
||||
|
||||
static int get_uart_fd(unsigned index);
|
||||
|
||||
int get_uart_fd();
|
||||
|
||||
/**
|
||||
* Get the MAVLink system id.
|
||||
*
|
||||
* @return The system ID of this vehicle
|
||||
*/
|
||||
int get_system_id();
|
||||
|
||||
/**
|
||||
* Get the MAVLink component id.
|
||||
*
|
||||
* @return The component ID of this vehicle
|
||||
*/
|
||||
int get_component_id();
|
||||
|
||||
const char *_device_name;
|
||||
|
||||
enum MAVLINK_MODE {
|
||||
MAVLINK_MODE_NORMAL = 0,
|
||||
MAVLINK_MODE_CUSTOM,
|
||||
MAVLINK_MODE_ONBOARD
|
||||
};
|
||||
|
||||
void set_mode(enum MAVLINK_MODE);
|
||||
enum MAVLINK_MODE get_mode() { return _mode; }
|
||||
|
||||
bool get_hil_enabled() { return _hil_enabled; }
|
||||
|
||||
bool get_use_hil_gps() { return _use_hil_gps; }
|
||||
|
||||
bool get_forward_externalsp() { return _forward_externalsp; }
|
||||
|
||||
bool get_flow_control_enabled() { return _flow_control_enabled; }
|
||||
|
||||
bool get_forwarding_on() { return _forwarding_on; }
|
||||
|
||||
/**
|
||||
* Get the free space in the transmit buffer
|
||||
*
|
||||
* @return free space in the UART TX buffer
|
||||
*/
|
||||
unsigned get_free_tx_buf();
|
||||
|
||||
static int start_helper(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Handle parameter related messages.
|
||||
*/
|
||||
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
|
||||
|
||||
void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
|
||||
|
||||
/**
|
||||
* Enable / disable Hardware in the Loop simulation mode.
|
||||
*
|
||||
* @param hil_enabled The new HIL enable/disable state.
|
||||
* @return OK if the HIL state changed, ERROR if the
|
||||
* requested change could not be made or was
|
||||
* redundant.
|
||||
*/
|
||||
int set_hil_enabled(bool hil_enabled);
|
||||
|
||||
void send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0);
|
||||
|
||||
/**
|
||||
* Resend message as is, don't change sequence number and CRC.
|
||||
*/
|
||||
void resend_message(mavlink_message_t *msg);
|
||||
|
||||
void handle_message(const mavlink_message_t *msg);
|
||||
|
||||
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0);
|
||||
|
||||
int get_instance_id();
|
||||
|
||||
/**
|
||||
* Enable / disable hardware flow control.
|
||||
*
|
||||
* @param enabled True if hardware flow control should be enabled
|
||||
*/
|
||||
int enable_flow_control(bool enabled);
|
||||
|
||||
mavlink_channel_t get_channel();
|
||||
|
||||
void configure_stream_threadsafe(const char *stream_name, float rate);
|
||||
|
||||
bool _task_should_exit; /**< if true, mavlink task should exit */
|
||||
|
||||
int get_mavlink_fd() { return _mavlink_fd; }
|
||||
|
||||
/**
|
||||
* Send a status text with loglevel INFO
|
||||
*
|
||||
* @param string the message to send (will be capped by mavlink max string length)
|
||||
*/
|
||||
void send_statustext_info(const char *string);
|
||||
|
||||
/**
|
||||
* Send a status text with loglevel CRITICAL
|
||||
*
|
||||
* @param string the message to send (will be capped by mavlink max string length)
|
||||
*/
|
||||
void send_statustext_critical(const char *string);
|
||||
|
||||
/**
|
||||
* Send a status text with loglevel EMERGENCY
|
||||
*
|
||||
* @param string the message to send (will be capped by mavlink max string length)
|
||||
*/
|
||||
void send_statustext_emergency(const char *string);
|
||||
|
||||
/**
|
||||
* Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent
|
||||
* only on this mavlink connection. Useful for reporting communication specific, not system-wide info
|
||||
* only to client interested in it. Message will be not sent immediately but queued in buffer as
|
||||
* for mavlink_log_xxx().
|
||||
*
|
||||
* @param string the message to send (will be capped by mavlink max string length)
|
||||
* @param severity the log level
|
||||
*/
|
||||
void send_statustext(unsigned char severity, const char *string);
|
||||
|
||||
MavlinkStream * get_streams() const { return _streams; }
|
||||
|
||||
float get_rate_mult();
|
||||
|
||||
/* Functions for waiting to start transmission until message received. */
|
||||
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
|
||||
bool get_has_received_messages() { return _received_messages; }
|
||||
void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
|
||||
bool get_wait_to_transmit() { return _wait_to_transmit; }
|
||||
bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
|
||||
|
||||
bool message_buffer_write(const void *ptr, int size);
|
||||
|
||||
void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
|
||||
void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
|
||||
|
||||
/**
|
||||
* Count a transmision error
|
||||
*/
|
||||
void count_txerr();
|
||||
|
||||
/**
|
||||
* Count transmitted bytes
|
||||
*/
|
||||
void count_txbytes(unsigned n) { _bytes_tx += n; };
|
||||
|
||||
/**
|
||||
* Count bytes not transmitted because of errors
|
||||
*/
|
||||
void count_txerrbytes(unsigned n) { _bytes_txerr += n; };
|
||||
|
||||
/**
|
||||
* Count received bytes
|
||||
*/
|
||||
void count_rxbytes(unsigned n) { _bytes_rx += n; };
|
||||
|
||||
/**
|
||||
* Get the receive status of this MAVLink link
|
||||
*/
|
||||
struct telemetry_status_s& get_rx_status() { return _rstatus; }
|
||||
|
||||
struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; }
|
||||
|
||||
unsigned get_system_type() { return _system_type; }
|
||||
|
||||
protected:
|
||||
Mavlink *next;
|
||||
|
||||
private:
|
||||
int _instance_id;
|
||||
|
||||
int _mavlink_fd;
|
||||
bool _task_running;
|
||||
|
||||
/* states */
|
||||
bool _hil_enabled; /**< Hardware In the Loop mode */
|
||||
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
|
||||
bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */
|
||||
bool _is_usb_uart; /**< Port is USB */
|
||||
bool _wait_to_transmit; /**< Wait to transmit until received messages. */
|
||||
bool _received_messages; /**< Whether we've received valid mavlink messages. */
|
||||
|
||||
unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */
|
||||
|
||||
MavlinkOrbSubscription *_subscriptions;
|
||||
MavlinkStream *_streams;
|
||||
|
||||
MavlinkMissionManager *_mission_manager;
|
||||
MavlinkParametersManager *_parameters_manager;
|
||||
|
||||
MAVLINK_MODE _mode;
|
||||
|
||||
mavlink_channel_t _channel;
|
||||
|
||||
struct mavlink_logbuffer _logbuffer;
|
||||
unsigned int _total_counter;
|
||||
|
||||
pthread_t _receive_thread;
|
||||
|
||||
bool _verbose;
|
||||
bool _forwarding_on;
|
||||
bool _passing_on;
|
||||
bool _ftp_on;
|
||||
int _uart_fd;
|
||||
int _baudrate;
|
||||
int _datarate; ///< data rate for normal streams (attitude, position, etc.)
|
||||
int _datarate_events; ///< data rate for params, waypoints, text messages
|
||||
float _rate_mult;
|
||||
|
||||
/**
|
||||
* If the queue index is not at 0, the queue sending
|
||||
* logic will send parameters from the current index
|
||||
* to len - 1, the end of the param list.
|
||||
*/
|
||||
unsigned int _mavlink_param_queue_index;
|
||||
|
||||
bool mavlink_link_termination_allowed;
|
||||
|
||||
char *_subscribe_to_stream;
|
||||
float _subscribe_to_stream_rate;
|
||||
|
||||
bool _flow_control_enabled;
|
||||
uint64_t _last_write_success_time;
|
||||
uint64_t _last_write_try_time;
|
||||
|
||||
unsigned _bytes_tx;
|
||||
unsigned _bytes_txerr;
|
||||
unsigned _bytes_rx;
|
||||
uint64_t _bytes_timestamp;
|
||||
float _rate_tx;
|
||||
float _rate_txerr;
|
||||
float _rate_rx;
|
||||
|
||||
struct telemetry_status_s _rstatus; ///< receive status
|
||||
|
||||
struct mavlink_message_buffer {
|
||||
int write_ptr;
|
||||
int read_ptr;
|
||||
int size;
|
||||
char *data;
|
||||
};
|
||||
|
||||
mavlink_message_buffer _message_buffer;
|
||||
|
||||
pthread_mutex_t _message_buffer_mutex;
|
||||
pthread_mutex_t _send_mutex;
|
||||
|
||||
bool _param_initialized;
|
||||
param_t _param_system_id;
|
||||
param_t _param_component_id;
|
||||
param_t _param_system_type;
|
||||
param_t _param_use_hil_gps;
|
||||
param_t _param_forward_externalsp;
|
||||
|
||||
unsigned _system_type;
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
perf_counter_t _txerr_perf; /**< TX error counter */
|
||||
|
||||
void mavlink_update_system();
|
||||
|
||||
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
|
||||
|
||||
static unsigned int interval_from_rate(float rate);
|
||||
|
||||
int configure_stream(const char *stream_name, const float rate);
|
||||
|
||||
/**
|
||||
* Adjust the stream rates based on the current rate
|
||||
*
|
||||
* @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease
|
||||
*/
|
||||
void adjust_stream_rates(const float multiplier);
|
||||
|
||||
int message_buffer_init(int size);
|
||||
|
||||
void message_buffer_destroy();
|
||||
|
||||
int message_buffer_count();
|
||||
|
||||
int message_buffer_is_empty();
|
||||
|
||||
int message_buffer_get_ptr(void **ptr, bool *is_part);
|
||||
|
||||
void message_buffer_mark_read(int n);
|
||||
|
||||
void pass_message(const mavlink_message_t *msg);
|
||||
|
||||
/**
|
||||
* Update rate mult so total bitrate will be equal to _datarate.
|
||||
*/
|
||||
void update_rate_mult();
|
||||
|
||||
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
|
||||
|
||||
/**
|
||||
* Main mavlink task.
|
||||
*/
|
||||
int task_main(int argc, char *argv[]);
|
||||
|
||||
/* do not allow copying this class */
|
||||
Mavlink(const Mavlink&);
|
||||
Mavlink operator=(const Mavlink&);
|
||||
};
|
||||
@@ -59,7 +59,6 @@
|
||||
#include <time.h>
|
||||
#include <float.h>
|
||||
#include <unistd.h>
|
||||
#include <nuttx/sched.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <termios.h>
|
||||
#include <errno.h>
|
||||
@@ -992,7 +991,7 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg)
|
||||
clock_gettime(CLOCK_REALTIME, &tv);
|
||||
|
||||
// date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
|
||||
bool onb_unix_valid = tv.tv_sec > PX4_EPOCH_SECS;
|
||||
bool onb_unix_valid = (unsigned long long)tv.tv_sec > PX4_EPOCH_SECS;
|
||||
bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL;
|
||||
|
||||
if (!onb_unix_valid && ofb_unix_valid) {
|
||||
@@ -1496,17 +1495,17 @@ MavlinkReceiver::receive_thread(void *arg)
|
||||
sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
|
||||
prctl(PR_SET_NAME, thread_name, getpid());
|
||||
|
||||
struct pollfd fds[1];
|
||||
px4_pollfd_struct_t fds[1];
|
||||
fds[0].fd = uart_fd;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
ssize_t nread = 0;
|
||||
|
||||
while (!_mavlink->_task_should_exit) {
|
||||
if (poll(fds, 1, timeout) > 0) {
|
||||
if (px4_poll(fds, 1, timeout) > 0) {
|
||||
|
||||
/* non-blocking read. read may return negative values */
|
||||
if ((nread = read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
|
||||
if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
|
||||
/* to avoid reading very small chunks wait for data before reading */
|
||||
usleep(1000);
|
||||
}
|
||||
|
||||
@@ -36,16 +36,22 @@
|
||||
#
|
||||
|
||||
MODULE_COMMAND = mavlink
|
||||
SRCS += mavlink_main.cpp \
|
||||
mavlink.c \
|
||||
ifeq ($(PX$_TARGET_OS),nuttx)
|
||||
SRCS += mavlink_main_nuttx.cpp \
|
||||
mavlink_ftp_nuttx.cpp
|
||||
else
|
||||
SRCS += mavlink_main_linux.cpp \
|
||||
mavlink_ftp_linux.cpp
|
||||
endif
|
||||
|
||||
SRCS += mavlink.c \
|
||||
mavlink_receiver.cpp \
|
||||
mavlink_mission.cpp \
|
||||
mavlink_parameters.cpp \
|
||||
mavlink_orb_subscription.cpp \
|
||||
mavlink_messages.cpp \
|
||||
mavlink_stream.cpp \
|
||||
mavlink_rate_limiter.cpp \
|
||||
mavlink_ftp.cpp
|
||||
mavlink_rate_limiter.cpp
|
||||
|
||||
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
|
||||
|
||||
|
||||
@@ -38,8 +38,12 @@
|
||||
MODULE_COMMAND = sensors
|
||||
MODULE_PRIORITY = "SCHED_PRIORITY_MAX-5"
|
||||
|
||||
SRCS = sensors.cpp \
|
||||
sensor_params.c
|
||||
ifeq ($(PX$_TARGET_OS),nuttx)
|
||||
SRCS = sensors_nuttx.cpp
|
||||
else
|
||||
SRCS = sensors_linux.cpp
|
||||
endif
|
||||
SRCS += sensor_params.c
|
||||
|
||||
MODULE_STACKSIZE = 1200
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user