dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #1083 from DonLakeFlyer/mavlink-ftp 

FTP List, Open, Read, Terminate commands working end to end with QGC
This commit is contained in:
Lorenz Meier 2014-06-28 11:13:33 +02:00
parent 7546b99a24 a398d5cbcc
commit 5192a5eea9
4 changed files with 121 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

179
src/modules/mavlink/mavlink_ftp.cpp
View File

@ -56,6 +56,11 @@ MavlinkFTP::MavlinkFTP()
dq_init(&_workFree);
sem_init(&_lock, 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++) {
_qFree(&_workBufs[i]);
@ -122,13 +127,11 @@ MavlinkFTP::_worker(Request *req)
break;
case kCmdTerminate:
if (!Session::terminate(hdr->session)) {
errorCode = kErrNoRequest;
}
errorCode = _workTerminate(req);
break;
case kCmdReset:
Session::reset();
errorCode = _workReset();
break;
case kCmdList:
@ -219,6 +222,12 @@ MavlinkFTP::_workList(Request *req)
// no more entries?
if (result == nullptr) {
if (hdr->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;
}
@ -257,19 +266,20 @@ MavlinkFTP::_workOpen(Request *req, bool create)
{
auto hdr = req->header();
// allocate session ID
int session = Session::allocate();
if (session < 0) {
int session_index = _findUnusedSession();
if (session_index < 0) {
return kErrNoSession;
}
// get the session to open the file
if (!Session::get(session)->open(req->dataAsCString(), create)) {
int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
int fd = ::open(req->dataAsCString(), oflag);
if (fd < 0) {
return create ? kErrPerm : kErrNotFile;
}
_session_fds[session_index] = fd;
// save the session ID in the reply
hdr->session = session;
hdr->session = session_index;
hdr->size = 0;
return kErrNone;
@ -280,29 +290,40 @@ MavlinkFTP::_workRead(Request *req)
{
auto hdr = req->header();
// look up session
auto session = Session::get(hdr->session);
if (session == nullptr) {
int session_index = hdr->session;
if (!_validSession(session_index)) {
return kErrNoSession;
}
// read from file
int result = session->read(hdr->offset, &hdr->data[0], hdr->size);
// Seek to the specified position
printf("Seek %d\n", hdr->offset);
if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
// Unable to see to the specified location
return kErrEOF;
}
if (result < 0) {
int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength);
if (bytes_read < 0) {
// Negative return indicates error other than eof
return kErrIO;
}
hdr->size = result;
printf("Read success %d\n", bytes_read);
hdr->size = bytes_read;
return kErrNone;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workWrite(Request *req)
{
#if 0
// NYI: Coming soon
auto hdr = req->header();
// look up session
auto session = Session::get(hdr->session);
auto session = getSession(hdr->session);
if (session == nullptr) {
return kErrNoSession;
}
@ -317,6 +338,9 @@ MavlinkFTP::_workWrite(Request *req)
hdr->size = result;
return kErrNone;
#else
return kErrPerm;
#endif
}
MavlinkFTP::ErrorCode
@ -328,93 +352,54 @@ MavlinkFTP::_workRemove(Request *req)
return kErrPerm;
}
MavlinkFTP::Session MavlinkFTP::Session::_sessions[MavlinkFTP::Session::kMaxSession];
MavlinkFTP::ErrorCode
MavlinkFTP::_workTerminate(Request *req)
{
auto hdr = req->header();
if (!_validSession(hdr->session)) {
return kErrNoSession;
}
::close(_session_fds[hdr->session]);
return kErrNone;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workReset(void)
{
for (size_t i=0; i<kMaxSession; i++) {
if (_session_fds[i] != -1) {
::close(_session_fds[i]);
_session_fds[i] = -1;
}
}
return kErrNone;
}
bool
MavlinkFTP::_validSession(unsigned index)
{
if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
return false;
}
return true;
}
int
MavlinkFTP::Session::allocate()
MavlinkFTP::_findUnusedSession(void)
{
for (unsigned i = 0; i < kMaxSession; i++) {
if (_sessions[i]._fd < 0) {
for (size_t i=0; i<kMaxSession; i++) {
if (_session_fds[i] == -1) {
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()
{

31
src/modules/mavlink/mavlink_ftp.h
View File

@ -115,27 +115,11 @@ private:
kErrPerm
};
class Session
{
public:
Session() : _fd(-1) {}
int _findUnusedSession(void);
bool _validSession(unsigned index);
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;
};
int _session_fds[kMaxSession];
class Request
{
@ -163,7 +147,11 @@ private:
unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
_mavlink->get_channel(), &msg, sequence(), rawData());
if (!_mavlink->message_buffer_write(&msg, len)) {
_mavlink->lockMessageBufferMutex();
bool fError = _mavlink->message_buffer_write(&msg, len);
_mavlink->unlockMessageBufferMutex();
if (!fError) {
warnx("FTP TX ERR");
} else {
warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
@ -211,6 +199,8 @@ private:
ErrorCode _workRead(Request *req);
ErrorCode _workWrite(Request *req);
ErrorCode _workRemove(Request *req);
ErrorCode _workTerminate(Request *req);
ErrorCode _workReset();
// work freelist
Request _workBufs[kRequestQueueSize];
@ -232,4 +222,5 @@ private:
_qUnlock();
return req;
}
};

44
src/modules/mavlink/mavlink_main.cpp
View File

@ -2074,38 +2074,46 @@ Mavlink::task_main(int argc, char *argv[])
if (_passing_on || _ftp_on) {
bool is_part;
void *read_ptr;
uint8_t *read_ptr;
uint8_t *write_ptr;
/* guard get ptr by mutex */
pthread_mutex_lock(&_message_buffer_mutex);
int available = message_buffer_get_ptr(&read_ptr, &is_part);
int available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
pthread_mutex_unlock(&_message_buffer_mutex);
if (available > 0) {
// Reconstruct message from buffer
// int oldseq = mavlink_get_channel_status(get_channel())->current_tx_seq;
mavlink_message_t msg;
write_ptr = (uint8_t*)&msg;
const mavlink_message_t* msg = (const mavlink_message_t*)read_ptr;
/* write first part of buffer */
_mavlink_resend_uart(_channel, msg);
// Pull a single message from the buffer
int read_count = available;
if (read_count > sizeof(mavlink_message_t)) {
read_count = sizeof(mavlink_message_t);
}
// mavlink_get_channel_status(get_channel())->current_tx_seq = oldseq;
// mavlink_msg_system_time_send(get_channel(), 255, 255);
memcpy(write_ptr, read_ptr, read_count);
message_buffer_mark_read(available);
// We hold the mutex until after we complete the second part of the buffer. If we don't
// we may end up breaking the empty slot overflow detection semantics when we mark the
// possibly partial read below.
pthread_mutex_lock(&_message_buffer_mutex);
message_buffer_mark_read(read_count);
/* write second part of buffer if there is some */
// XXX this doesn't quite work, as the resend UART call assumes a continous block
if (is_part) {
/* guard get ptr by mutex */
pthread_mutex_lock(&_message_buffer_mutex);
available = message_buffer_get_ptr(&read_ptr, &is_part);
pthread_mutex_unlock(&_message_buffer_mutex);
_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
if (is_part && read_count < sizeof(mavlink_message_t)) {
write_ptr += read_count;
available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
read_count = sizeof(mavlink_message_t) - read_count;
memcpy(write_ptr, read_ptr, read_count);
message_buffer_mark_read(available);
}
pthread_mutex_unlock(&_message_buffer_mutex);
_mavlink_resend_uart(_channel, &msg);
}
}

3
src/modules/mavlink/mavlink_main.h
View File

@ -229,6 +229,9 @@ public:
bool message_buffer_write(void *ptr, int size);
void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
protected:
Mavlink *next;