diff options
Diffstat (limited to 'src/modules/mavlink')
23 files changed, 3893 insertions, 1677 deletions
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index e49288a74..9afe74af1 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -68,14 +68,17 @@ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); * @group MAVLink */ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0); +/** + * Forward external setpoint messages + * If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard + * control mode + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); mavlink_system_t mavlink_system = { 100, - 50, - MAV_TYPE_FIXED_WING, - 0, - 0, - 0 + 50 }; // System ID, 1-255, Component/Subsystem ID, 1-255 /* diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 374d1511c..0cd08769e 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -44,7 +44,12 @@ __BEGIN_DECLS -#define MAVLINK_USE_CONVENIENCE_FUNCTIONS +/* + * We are NOT using convenience functions, + * but instead send messages with a custom function. + * So we do NOT do this: + * #define MAVLINK_USE_CONVENIENCE_FUNCTIONS + */ /* use efficient approach, see mavlink_helpers.h */ #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 6a2c900af..f17497aa8 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -31,34 +31,39 @@ * ****************************************************************************/ +/// @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" -MavlinkFTP *MavlinkFTP::_server; +// Uncomment the line below to get better debug output. Never commit with this left on. +//#define MAVLINK_FTP_DEBUG MavlinkFTP * -MavlinkFTP::getServer() +MavlinkFTP::get_server(void) { - // XXX this really cries out for some locking... - if (_server == nullptr) { - _server = new MavlinkFTP; - } - return _server; + static MavlinkFTP server; + return &server; } MavlinkFTP::MavlinkFTP() : - _session_fds{}, - _workBufs{}, - _workFree{}, - _lock{} + _request_bufs{}, + _request_queue{}, + _request_queue_sem{}, + _utRcvMsgFunc{}, + _ftp_test{} { // initialise the request freelist - dq_init(&_workFree); - sem_init(&_lock, 0, 1); + dq_init(&_request_queue); + sem_init(&_request_queue_sem, 0, 1); // initialize session list for (size_t i=0; i<kMaxSession; i++) { @@ -67,173 +72,278 @@ MavlinkFTP::MavlinkFTP() : // drop work entries onto the free list for (unsigned i = 0; i < kRequestQueueSize; i++) { - _qFree(&_workBufs[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 - auto req = _dqFree(); + struct Request* req = _get_request(); // if we couldn't get a request slot, just drop it - if (req != nullptr) { - - // decode the request - if (req->decode(mavlink, msg)) { + if (req == nullptr) { + warnx("Dropping FTP request: queue full\n"); + return; + } - // and queue it for the worker - work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0); - } else { - _qFree(req); + 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::_workerTrampoline(void *arg) +MavlinkFTP::_worker_trampoline(void *arg) { - auto req = reinterpret_cast<Request *>(arg); - auto server = MavlinkFTP::getServer(); + Request* req = reinterpret_cast<Request *>(arg); + MavlinkFTP* server = MavlinkFTP::get_server(); // call the server worker with the work item - server->_worker(req); + server->_process_request(req); } +/// @brief Processes an FTP message void -MavlinkFTP::_worker(Request *req) +MavlinkFTP::_process_request(Request *req) { - auto hdr = req->header(); + PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]); + ErrorCode errorCode = kErrNone; - uint32_t messageCRC; // basic sanity checks; must validate length before use - if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) { - errorCode = kErrNoRequest; + if (payload->size > kMaxDataLength) { + errorCode = kErrInvalidDataSize; goto out; } - // check request CRC to make sure this is one of ours - messageCRC = hdr->crc32; - hdr->crc32 = 0; - if (crc32(req->rawData(), req->dataSize()) != messageCRC) { - errorCode = kErrNoRequest; - goto out; - warnx("ftp: bad crc"); - } - - //printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); +#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 (hdr->opcode) { + switch (payload->opcode) { case kCmdNone: break; - case kCmdTerminate: - errorCode = _workTerminate(req); + case kCmdTerminateSession: + errorCode = _workTerminate(payload); break; - case kCmdReset: - errorCode = _workReset(); + case kCmdResetSessions: + errorCode = _workReset(payload); break; - case kCmdList: - errorCode = _workList(req); + case kCmdListDirectory: + errorCode = _workList(payload); break; - case kCmdOpen: - errorCode = _workOpen(req, false); + case kCmdOpenFileRO: + errorCode = _workOpen(payload, O_RDONLY); break; - case kCmdCreate: - errorCode = _workOpen(req, true); + case kCmdCreateFile: + errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY); break; - case kCmdRead: - errorCode = _workRead(req); + case kCmdOpenFileWO: + errorCode = _workOpen(payload, O_CREAT | O_WRONLY); break; - case kCmdWrite: - errorCode = _workWrite(req); + case kCmdReadFile: + errorCode = _workRead(payload); break; - case kCmdRemove: - errorCode = _workRemove(req); + 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 = kErrNoRequest; + errorCode = kErrUnknownCommand; break; } out: // handle success vs. error if (errorCode == kErrNone) { - hdr->opcode = kRspAck; - //warnx("FTP: ack\n"); + 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); - hdr->opcode = kRspNak; - hdr->size = 1; - hdr->data[0] = 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); - // free the request buffer back to the freelist - _qFree(req); + _return_request(req); } +/// @brief Sends the specified FTP reponse message out through mavlink 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->rawData(), req->dataSize()); + 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 - // then pack and send the reply back to the request source - req->reply(); + 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(Request *req) +MavlinkFTP::_workList(PayloadHeader* payload) { - auto hdr = req->header(); - char dirPath[kMaxDataLength]; - strncpy(dirPath, req->dataAsCString(), kMaxDataLength); + strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength); DIR *dp = opendir(dirPath); if (dp == nullptr) { warnx("FTP: can't open path '%s'", dirPath); - return kErrNotDir; + return kErrFailErrno; } - //warnx("FTP: list %s offset %d", dirPath, hdr->offset); +#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, hdr->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 = kErrIO; + errorCode = kErrFailErrno; break; } // no more entries? if (result == nullptr) { - if (hdr->offset != 0 && offset == 0) { + 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; @@ -242,144 +352,277 @@ MavlinkFTP::_workList(Request *req) break; } - // name too big to fit? - if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) { - break; - } + uint32_t fileSize = 0; + char buf[256]; + char direntType; - // store the type marker + // Determine the directory entry type switch (entry.d_type) { case DTYPE_FILE: - hdr->data[offset++] = kDirentFile; + // 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; case DTYPE_DIRECTORY: - hdr->data[offset++] = kDirentDir; + if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { + // Don't bother sending these back + direntType = kDirentSkip; + } else { + direntType = kDirentDir; + } break; default: - hdr->data[offset++] = kDirentUnknown; - break; + // 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); - // copy the name, which we know will fit - strcpy((char *)&hdr->data[offset], entry.d_name); - //printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); - offset += strlen(entry.d_name) + 1; + // 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); - hdr->size = offset; + payload->size = offset; return errorCode; } +/// @brief Responds to an Open command MavlinkFTP::ErrorCode -MavlinkFTP::_workOpen(Request *req, bool create) +MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag) { - auto hdr = req->header(); - - int session_index = _findUnusedSession(); + int session_index = _find_unused_session(); if (session_index < 0) { - return kErrNoSession; + warnx("FTP: Open failed - out of sessions\n"); + return kErrNoSessionsAvailable; } - int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; - - int fd = ::open(req->dataAsCString(), oflag); + 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 create ? kErrPerm : kErrNotFile; + return kErrFailErrno; } _session_fds[session_index] = fd; - hdr->session = session_index; - hdr->size = 0; + 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(Request *req) +MavlinkFTP::_workRead(PayloadHeader* payload) { - auto hdr = req->header(); - - int session_index = hdr->session; + int session_index = payload->session; - if (!_validSession(session_index)) { - return kErrNoSession; + if (!_valid_session(session_index)) { + return kErrInvalidSession; } // Seek to the specified position - //warnx("seek %d", hdr->offset); - if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { +#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], &hdr->data[0], kMaxDataLength); + 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 kErrIO; + return kErrFailErrno; } - hdr->size = bytes_read; + payload->size = bytes_read; return kErrNone; } +/// @brief Responds to a Write command MavlinkFTP::ErrorCode -MavlinkFTP::_workWrite(Request *req) +MavlinkFTP::_workWrite(PayloadHeader* payload) { -#if 0 - // NYI: Coming soon - auto hdr = req->header(); + int session_index = payload->session; - // look up session - auto session = getSession(hdr->session); - if (session == nullptr) { - return kErrNoSession; + if (!_valid_session(session_index)) { + return kErrInvalidSession; } - // append to file - int result = session->append(hdr->offset, &hdr->data[0], hdr->size); + // 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; + } - if (result < 0) { - // XXX might also be no space, I/O, etc. - return kErrNotAppend; + 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; } - hdr->size = result; + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = bytes_written; + return kErrNone; -#else - return kErrPerm; -#endif } +/// @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::_workRemove(Request *req) +MavlinkFTP::_workTruncateFile(PayloadHeader* payload) { - //auto hdr = req->header(); + 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; + } - // for now, send error reply - return kErrPerm; + ::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(Request *req) +MavlinkFTP::_workTerminate(PayloadHeader* payload) { - auto hdr = req->header(); - - if (!_validSession(hdr->session)) { - return kErrNoSession; + if (!_valid_session(payload->session)) { + return kErrInvalidSession; } - ::close(_session_fds[hdr->session]); + ::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(void) +MavlinkFTP::_workReset(PayloadHeader* payload) { for (size_t i=0; i<kMaxSession; i++) { if (_session_fds[i] != -1) { @@ -388,11 +631,104 @@ MavlinkFTP::_workReset(void) } } + 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::_validSession(unsigned index) +MavlinkFTP::_valid_session(unsigned index) { if ((index >= kMaxSession) || (_session_fds[index] < 0)) { return false; @@ -400,8 +736,9 @@ MavlinkFTP::_validSession(unsigned index) return true; } +/// @brief Returns an unused session index int -MavlinkFTP::_findUnusedSession(void) +MavlinkFTP::_find_unused_session(void) { for (size_t i=0; i<kMaxSession; i++) { if (_session_fds[i] == -1) { @@ -412,16 +749,105 @@ MavlinkFTP::_findUnusedSession(void) return -1; } +/// @brief Guarantees that the payload data is null terminated. +/// @return Returns a pointer to the payload data as a char * char * -MavlinkFTP::Request::dataAsCString() +MavlinkFTP::_data_as_cstring(PayloadHeader* payload) { // guarantee nul termination - if (header()->size < kMaxDataLength) { - requestData()[header()->size] = '\0'; + if (payload->size < kMaxDataLength) { + payload->data[payload->size] = '\0'; } else { - requestData()[kMaxDataLength - 1] = '\0'; + payload->data[kMaxDataLength - 1] = '\0'; } // and return data - return (char *)&(header()->data[0]); + 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, ssize_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); + 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; } diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 59237a4c4..bef6775a9 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -33,20 +33,8 @@ #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. - * - */ +/// @file mavlink_ftp.h +/// @author px4dev, Don Gagne <don@thegagnes.com> #include <dirent.h> #include <queue.h> @@ -55,173 +43,138 @@ #include <systemlib/err.h> #include "mavlink_messages.h" +#include "mavlink_main.h" + +class MavlinkFtpTest; +/// @brief MAVLink remote file server. Support FTP like commands using MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL message. +/// A limited number of requests (kRequestQueueSize) may be outstanding at a time. Additional messages will be discarded. class MavlinkFTP { public: + /// @brief Returns the one Mavlink FTP server in the system. + static MavlinkFTP* get_server(void); + + /// @brief Contructor is only public so unit test code can new objects. MavlinkFTP(); - - static MavlinkFTP *getServer(); - - // static interface - void handle_message(Mavlink* mavlink, - mavlink_message_t *msg); - -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[]; - }; - + + /// @brief Adds the specified message to the work queue. + void handle_message(Mavlink* mavlink, mavlink_message_t *msg); + + typedef void (*ReceiveMessageFunc_t)(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest); + + /// @brief Sets up the server to run in unit test mode. + /// @param rcvmsgFunc Function which will be called to handle outgoing mavlink messages. + /// @param ftp_test MavlinkFtpTest object which the function is associated with + void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test); + + /// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. We pad the structure ourselves to + /// 32 bit alignment to avoid usage of any pack pragmas. + struct PayloadHeader + { + uint16_t seqNumber; ///< sequence number for message + uint8_t session; ///< Session id for read and write commands + uint8_t opcode; ///< Command opcode + uint8_t size; ///< Size of data + uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message + uint8_t padding[2]; ///< 32 bit aligment padding + uint32_t offset; ///< Offsets for List and Read commands + uint8_t data[]; ///< command data, varies by Opcode + }; + + /// @brief Command opcodes 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 + kCmdNone, ///< ignored, always acked + kCmdTerminateSession, ///< Terminates open Read session + kCmdResetSessions, ///< Terminates all open Read sessions + kCmdListDirectory, ///< List files in <path> from <offset> + kCmdOpenFileRO, ///< Opens file at <path> for reading, returns <session> + kCmdReadFile, ///< Reads <size> bytes from <offset> in <session> + kCmdCreateFile, ///< Creates file at <path> for writing, returns <session> + kCmdWriteFile, ///< Writes <size> bytes to <offset> in <session> + kCmdRemoveFile, ///< Remove file at <path> + kCmdCreateDirectory, ///< Creates directory at <path> + kCmdRemoveDirectory, ///< Removes Directory at <path>, must be empty + kCmdOpenFileWO, ///< Opens file at <path> for writing, returns <session> + kCmdTruncateFile, ///< Truncate file at <path> to <offset> length + kCmdRename, ///< Rename <path1> to <path2> + kCmdCalcFileCRC32, ///< Calculate CRC32 for file at <path> + + kRspAck = 128, ///< Ack response + kRspNak ///< Nak response }; - + + /// @brief Error codes returned in Nak response PayloadHeader.data[0]. enum ErrorCode : uint8_t - { + { kErrNone, - kErrNoRequest, - kErrNoSession, - kErrSequence, - kErrNotDir, - kErrNotFile, - kErrEOF, - kErrNotAppend, - kErrTooBig, - kErrIO, - kErrPerm - }; - - int _findUnusedSession(void); - bool _validSession(unsigned index); - - static const unsigned kMaxSession = 2; - int _session_fds[kMaxSession]; - - class Request + kErrFail, ///< Unknown failure + kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1] + kErrInvalidDataSize, ///< PayloadHeader.size is invalid + kErrInvalidSession, ///< Session is not currently open + kErrNoSessionsAvailable, ///< All available Sessions in use + kErrEOF, ///< Offset past end of file for List and Read commands + kErrUnknownCommand ///< Unknown command opcode + }; + +private: + /// @brief Unit of work which is queued to work_queue + struct Request { - public: - union { - dq_entry_t entry; - work_s work; - }; - - bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) { - if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) { - _mavlink = mavlink; - mavlink_msg_encapsulated_data_decode(fromMessage, &_message); - return true; - } - return false; - } - - void reply() { - - // XXX the proper way would be an IOCTL / uORB call, rather than exploiting the - // flat memory architecture, as we're operating between threads here. - mavlink_message_t msg; - msg.checksum = 0; - unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), - _mavlink->get_channel(), &msg, sequence(), rawData()); - - _mavlink->lockMessageBufferMutex(); - bool success = _mavlink->message_buffer_write(&msg, len); - _mavlink->unlockMessageBufferMutex(); - - if (!success) { - warnx("FTP TX ERR"); - } - // else { - // warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", - // _mavlink->get_system_id(), - // _mavlink->get_component_id(), - // _mavlink->get_channel(), - // len, - // msg.checksum); - // } - } - - uint8_t *rawData() { return &_message.data[0]; } - RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.data[0]); } - uint8_t *requestData() { return &(header()->data[0]); } - unsigned dataSize() { return header()->size + sizeof(RequestHeader); } - uint16_t sequence() const { return _message.seqnr; } - mavlink_channel_t channel() { return _mavlink->get_channel(); } - - char *dataAsCString(); - - private: - Mavlink *_mavlink; - mavlink_encapsulated_data_t _message; - + work_s work; ///< work queue entry + Mavlink *mavlink; ///< Mavlink to reply to + uint8_t serverSystemId; ///< System ID to send from + uint8_t serverComponentId; ///< Component ID to send from + uint8_t serverChannel; ///< Channel to send to + uint8_t targetSystemId; ///< System ID to target reply to + + mavlink_file_transfer_protocol_t message; ///< Protocol message }; - - static const uint8_t kProtocolMagic = 'f'; - static const char kDirentFile = 'F'; - static const char kDirentDir = 'D'; - static const char kDirentUnknown = 'U'; - 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); - ErrorCode _workTerminate(Request *req); - ErrorCode _workReset(); - - // 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; - } - + + Request *_get_request(void); + void _return_request(Request *req); + void _lock_request_queue(void); + void _unlock_request_queue(void); + + char *_data_as_cstring(PayloadHeader* payload); + + static void _worker_trampoline(void *arg); + void _process_request(Request *req); + void _reply(Request *req); + int _copy_file(const char *src_path, const char *dst_path, ssize_t length); + + ErrorCode _workList(PayloadHeader *payload); + ErrorCode _workOpen(PayloadHeader *payload, int oflag); + ErrorCode _workRead(PayloadHeader *payload); + ErrorCode _workWrite(PayloadHeader *payload); + ErrorCode _workTerminate(PayloadHeader *payload); + ErrorCode _workReset(PayloadHeader* payload); + ErrorCode _workRemoveDirectory(PayloadHeader *payload); + ErrorCode _workCreateDirectory(PayloadHeader *payload); + ErrorCode _workRemoveFile(PayloadHeader *payload); + ErrorCode _workTruncateFile(PayloadHeader *payload); + ErrorCode _workRename(PayloadHeader *payload); + ErrorCode _workCalcFileCRC32(PayloadHeader *payload); + + static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests + Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work + dq_queue_t _request_queue; ///< Queue of available Request buffers + sem_t _request_queue_sem; ///< Semaphore for locking access to _request_queue + + int _find_unused_session(void); + bool _valid_session(unsigned index); + + static const char kDirentFile = 'F'; ///< Identifies File returned from List command + static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command + static const char kDirentSkip = 'S'; ///< Identifies Skipped entry from List command + + /// @brief Maximum data size in RequestHeader::data + static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader); + + static const unsigned kMaxSession = 2; ///< Max number of active sessions + int _session_fds[kMaxSession]; ///< Session file descriptors, 0 for empty slot + + ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending + MavlinkFtpTest *_ftp_test; ///< Additional parameter to _utRcvMsgFunc; }; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0c6f8c42f..29b7ec7b7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -78,7 +78,6 @@ #include "mavlink_messages.h" #include "mavlink_receiver.h" #include "mavlink_rate_limiter.h" -#include "mavlink_commands.h" #ifndef MAVLINK_CRC_EXTRA #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems @@ -91,14 +90,19 @@ static const int ERROR = -1; #define DEFAULT_DEVICE_NAME "/dev/ttyS1" -#define MAX_DATA_RATE 10000 // max data rate in bytes/s +#define MAX_DATA_RATE 20000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate +#define TX_BUFFER_GAP MAVLINK_MAX_PACKET_LEN + static Mavlink *_mavlink_instances = nullptr; /* TODO: if this is a class member it crashes */ static struct file_operations fops; +static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; + /** * mavlink app start / stop handling function * @@ -108,113 +112,6 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); extern mavlink_system_t mavlink_system; -static uint64_t last_write_success_times[6] = {0}; -static uint64_t last_write_try_times[6] = {0}; - -/* - * Internal function to send the bytes through the right serial port - */ -void -mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) -{ - - Mavlink *instance; - - switch (channel) { - case MAVLINK_COMM_0: - instance = Mavlink::get_instance(0); - break; - - case MAVLINK_COMM_1: - instance = Mavlink::get_instance(1); - break; - - case MAVLINK_COMM_2: - instance = Mavlink::get_instance(2); - break; - - case MAVLINK_COMM_3: - instance = Mavlink::get_instance(3); - break; -#ifdef MAVLINK_COMM_4 - - case MAVLINK_COMM_4: - instance = Mavlink::get_instance(4); - break; -#endif -#ifdef MAVLINK_COMM_5 - - case MAVLINK_COMM_5: - instance = Mavlink::get_instance(5); - break; -#endif -#ifdef MAVLINK_COMM_6 - - case MAVLINK_COMM_6: - instance = Mavlink::get_instance(6); - break; -#endif - - default: - return; - } - - int uart = instance->get_uart_fd(); - - ssize_t desired = (sizeof(uint8_t) * length); - - /* - * Check if the OS buffer is full and disable HW - * flow control if it continues to be full - */ - int buf_free = 0; - - if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { - - /* Disable hardware flow control: - * if no successful write since a defined time - * and if the last try was not the last successful write - */ - if (last_write_try_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL && - last_write_success_times[(unsigned)channel] != - last_write_try_times[(unsigned)channel]) { - warnx("DISABLING HARDWARE FLOW CONTROL"); - instance->enable_flow_control(false); - } - - } - - /* If the wait until transmit flag is on, only transmit after we've received messages. - Otherwise, transmit all the time. */ - if (instance->should_transmit()) { - last_write_try_times[(unsigned)channel] = hrt_absolute_time(); - - /* check if there is space in the buffer, let it overflow else */ - if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { - - if (buf_free < desired) { - /* we don't want to send anything just in half, so return */ - instance->count_txerr(); - instance->count_txerrbytes(desired); - return; - } - } - - ssize_t ret = write(uart, ch, desired); - - if (ret != desired) { - instance->count_txerr(); - instance->count_txerrbytes(desired); - - } else { - last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; - instance->count_txbytes(desired); - } - } -} - static void usage(void); Mavlink::Mavlink() : @@ -226,6 +123,7 @@ Mavlink::Mavlink() : _task_running(false), _hil_enabled(false), _use_hil_gps(false), + _forward_externalsp(false), _is_usb_uart(false), _wait_to_transmit(false), _received_messages(false), @@ -233,44 +131,50 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), _mission_manager(nullptr), - _mission_pub(-1), - _mission_result_sub(-1), + _parameters_manager(nullptr), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), _logbuffer {}, - _total_counter(0), - _receive_thread {}, - _verbose(false), - _forwarding_on(false), - _passing_on(false), - _ftp_on(false), - _uart_fd(-1), - _baudrate(57600), - _datarate(10000), - _mavlink_param_queue_index(0), - mavlink_link_termination_allowed(false), - _subscribe_to_stream(nullptr), - _subscribe_to_stream_rate(0.0f), - _flow_control_enabled(true), - _bytes_tx(0), - _bytes_txerr(0), - _bytes_rx(0), - _bytes_timestamp(0), - _rate_tx(0.0f), - _rate_txerr(0.0f), - _rate_rx(0.0f), - _rstatus {}, - _message_buffer {}, - _message_buffer_mutex {}, - _param_initialized(false), - _param_system_id(0), - _param_component_id(0), - _param_system_type(0), - _param_use_hil_gps(0), - - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), - _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) + _total_counter(0), + _receive_thread {}, + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _ftp_on(false), + _uart_fd(-1), + _baudrate(57600), + _datarate(1000), + _datarate_events(500), + _rate_mult(1.0f), + _mavlink_param_queue_index(0), + mavlink_link_termination_allowed(false), + _subscribe_to_stream(nullptr), + _subscribe_to_stream_rate(0.0f), + _flow_control_enabled(true), + _last_write_success_time(0), + _last_write_try_time(0), + _bytes_tx(0), + _bytes_txerr(0), + _bytes_rx(0), + _bytes_timestamp(0), + _rate_tx(0.0f), + _rate_txerr(0.0f), + _rate_rx(0.0f), + _rstatus {}, + _message_buffer {}, + _message_buffer_mutex {}, + _send_mutex {}, + _param_initialized(false), + _param_system_id(0), + _param_component_id(0), + _param_system_type(MAV_TYPE_FIXED_WING), + _param_use_hil_gps(0), + _param_forward_externalsp(0), + _system_type(0), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), + _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; @@ -316,6 +220,8 @@ Mavlink::Mavlink() : errx(1, "instance ID is out of range"); break; } + + _rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; } Mavlink::~Mavlink() @@ -483,7 +389,12 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { if (inst != self) { - inst->pass_message(msg); + + /* if not in normal mode, we are an onboard link + * onboard links should only pass on messages from the same system ID */ + if (!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) { + inst->pass_message(msg); + } } } } @@ -531,10 +442,26 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { const char *txt = (const char *)arg; -// printf("logmsg: %s\n", txt); struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); - msg.severity = (unsigned char)cmd; + + switch (cmd) { + case MAVLINK_IOC_SEND_TEXT_INFO: + msg.severity = MAV_SEVERITY_INFO; + break; + + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + msg.severity = MAV_SEVERITY_CRITICAL; + break; + + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + msg.severity = MAV_SEVERITY_EMERGENCY; + break; + + default: + msg.severity = MAV_SEVERITY_INFO; + break; + } Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { @@ -559,35 +486,58 @@ void Mavlink::mavlink_update_system(void) _param_component_id = param_find("MAV_COMP_ID"); _param_system_type = param_find("MAV_TYPE"); _param_use_hil_gps = param_find("MAV_USEHILGPS"); - _param_initialized = true; + _param_forward_externalsp = param_find("MAV_FWDEXTSP"); } /* update system and component id */ int32_t system_id; param_get(_param_system_id, &system_id); - if (system_id > 0 && system_id < 255) { - mavlink_system.sysid = system_id; - } - int32_t component_id; param_get(_param_component_id, &component_id); - if (component_id > 0 && component_id < 255) { - mavlink_system.compid = component_id; + + /* only allow system ID and component ID updates + * after reboot - not during operation */ + if (!_param_initialized) { + if (system_id > 0 && system_id < 255) { + mavlink_system.sysid = system_id; + } + + if (component_id > 0 && component_id < 255) { + mavlink_system.compid = component_id; + } + + _param_initialized = true; + } + + /* warn users that they need to reboot to take this + * into effect + */ + if (system_id != mavlink_system.sysid) { + send_statustext_critical("Save params and reboot to change SYSID"); + } + + if (component_id != mavlink_system.compid) { + send_statustext_critical("Save params and reboot to change COMPID"); } int32_t system_type; param_get(_param_system_type, &system_type); if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { - mavlink_system.type = system_type; + _system_type = system_type; } int32_t use_hil_gps; param_get(_param_use_hil_gps, &use_hil_gps); _use_hil_gps = (bool)use_hil_gps; + + int32_t forward_externalsp; + param_get(_param_forward_externalsp, &forward_externalsp); + + _forward_externalsp = (bool)forward_externalsp; } int Mavlink::get_system_id() @@ -711,6 +661,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * if (enable_flow_control(true)) { warnx("hardware flow control not supported"); } + + } else { + _flow_control_enabled = false; } return _uart_fd; @@ -752,8 +705,7 @@ Mavlink::set_hil_enabled(bool hil_enabled) /* enable HIL */ if (hil_enabled && !_hil_enabled) { _hil_enabled = true; - float rate_mult = _datarate / 1000.0f; - configure_stream("HIL_CONTROLS", 15.0f * rate_mult); + configure_stream("HIL_CONTROLS", 200.0f); } /* disable HIL */ @@ -768,248 +720,188 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } -void -Mavlink::send_message(const mavlink_message_t *msg) +unsigned +Mavlink::get_free_tx_buf() { - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + /* + * Check if the OS buffer is full and disable HW + * flow control if it continues to be full + */ + int buf_free = 0; + (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); + + if (get_flow_control_enabled() && buf_free < TX_BUFFER_GAP) { + /* Disable hardware flow control: + * if no successful write since a defined time + * and if the last try was not the last successful write + */ + if (_last_write_try_time != 0 && + hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL && + _last_write_success_time != _last_write_try_time) { + warnx("DISABLING HARDWARE FLOW CONTROL"); + enable_flow_control(false); + } + } - uint16_t len = mavlink_msg_to_send_buffer(buf, msg); - mavlink_send_uart_bytes(_channel, buf, len); + return buf_free; } void -Mavlink::handle_message(const mavlink_message_t *msg) +Mavlink::send_message(const uint8_t msgid, const void *msg) { - /* handle packet with mission manager */ - _mission_manager->handle_message(msg); + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; + } - /* handle packet with parameter component */ - mavlink_pm_message_handler(_channel, msg); + pthread_mutex_lock(&_send_mutex); - if (get_forwarding_on()) { - /* forward any messages to other mavlink instances */ - Mavlink::forward_message(msg, this); + unsigned buf_free = get_free_tx_buf(); + + uint8_t payload_len = mavlink_message_lengths[msgid]; + unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + _last_write_try_time = hrt_absolute_time(); + + /* check if there is space in the buffer, let it overflow else */ + if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) { + /* no enough space in buffer to send */ + count_txerr(); + count_txerrbytes(packet_len); + pthread_mutex_unlock(&_send_mutex); + return; } -} -int -Mavlink::mavlink_pm_queued_send() -{ - if (_mavlink_param_queue_index < param_count()) { - mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index)); - _mavlink_param_queue_index++; - return 0; + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + + /* header */ + buf[0] = MAVLINK_STX; + buf[1] = payload_len; + /* use mavlink's internal counter for the TX seq */ + buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; + buf[3] = mavlink_system.sysid; + buf[4] = mavlink_system.compid; + buf[5] = msgid; + + /* payload */ + memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len); + + /* checksum */ + uint16_t checksum; + crc_init(&checksum); + crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); + crc_accumulate(mavlink_message_crcs[msgid], &checksum); + + buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); + + /* send message to UART */ + ssize_t ret = write(_uart_fd, buf, packet_len); + + if (ret != (int) packet_len) { + count_txerr(); + count_txerrbytes(packet_len); } else { - return 1; + _last_write_success_time = _last_write_try_time; + count_txbytes(packet_len); } -} -void Mavlink::mavlink_pm_start_queued_send() -{ - _mavlink_param_queue_index = 0; + pthread_mutex_unlock(&_send_mutex); } -int Mavlink::mavlink_pm_send_param_for_index(uint16_t index) +void +Mavlink::resend_message(mavlink_message_t *msg) { - return mavlink_pm_send_param(param_for_index(index)); -} + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; + } -int Mavlink::mavlink_pm_send_param_for_name(const char *name) -{ - return mavlink_pm_send_param(param_find(name)); -} + pthread_mutex_lock(&_send_mutex); -int Mavlink::mavlink_pm_send_param(param_t param) -{ - if (param == PARAM_INVALID) { return 1; } + unsigned buf_free = get_free_tx_buf(); - /* buffers for param transmission */ - char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; - float val_buf; - mavlink_message_t tx_msg; + _last_write_try_time = hrt_absolute_time(); - /* query parameter type */ - param_type_t type = param_type(param); - /* copy parameter name */ - strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - /* - * Map onboard parameter type to MAVLink type, - * endianess matches (both little endian) - */ - uint8_t mavlink_type; + /* check if there is space in the buffer, let it overflow else */ + if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) { + /* no enough space in buffer to send */ + count_txerr(); + count_txerrbytes(packet_len); + pthread_mutex_unlock(&_send_mutex); + return; + } - if (type == PARAM_TYPE_INT32) { - mavlink_type = MAVLINK_TYPE_INT32_T; + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - } else if (type == PARAM_TYPE_FLOAT) { - mavlink_type = MAVLINK_TYPE_FLOAT; + /* header and payload */ + memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len); - } else { - mavlink_type = MAVLINK_TYPE_FLOAT; - } + /* checksum */ + buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8); - /* - * get param value, since MAVLink encodes float and int params in the same - * space during transmission, copy param onto float val_buf - */ + /* send message to UART */ + ssize_t ret = write(_uart_fd, buf, packet_len); - int ret; + if (ret != (int) packet_len) { + count_txerr(); + count_txerrbytes(packet_len); - if ((ret = param_get(param, &val_buf)) != OK) { - return ret; + } else { + _last_write_success_time = _last_write_try_time; + count_txbytes(packet_len); } - mavlink_msg_param_value_pack_chan(mavlink_system.sysid, - mavlink_system.compid, - _channel, - &tx_msg, - name_buf, - val_buf, - mavlink_type, - param_count(), - param_get_index(param)); - send_message(&tx_msg); - return OK; + pthread_mutex_unlock(&_send_mutex); } -void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) +void +Mavlink::handle_message(const mavlink_message_t *msg) { - switch (msg->msgid) { - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { - mavlink_param_request_list_t req; - mavlink_msg_param_request_list_decode(msg, &req); - - if (req.target_system == mavlink_system.sysid && - (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { - /* Start sending parameters */ - mavlink_pm_start_queued_send(); - send_statustext_info("[pm] sending list"); - } - } break; - - case MAVLINK_MSG_ID_PARAM_SET: { - - /* Handle parameter setting */ - - if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { - mavlink_param_set_t mavlink_param_set; - mavlink_msg_param_set_decode(msg, &mavlink_param_set); - - if (mavlink_param_set.target_system == mavlink_system.sysid - && ((mavlink_param_set.target_component == mavlink_system.compid) - || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter, set and send it */ - param_t param = param_find(name); - - if (param == PARAM_INVALID) { - char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; - sprintf(buf, "[pm] unknown: %s", name); - send_statustext_info(buf); - - } else { - /* set and send parameter */ - param_set(param, &(mavlink_param_set.param_value)); - mavlink_pm_send_param(param); - } - } - } - } break; - - case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { - mavlink_param_request_read_t mavlink_param_request_read; - mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); - - if (mavlink_param_request_read.target_system == mavlink_system.sysid - && ((mavlink_param_request_read.target_component == mavlink_system.compid) - || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { - /* when no index is given, loop through string ids and compare them */ - if (mavlink_param_request_read.param_index == -1) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter and send it */ - mavlink_pm_send_param_for_name(name); + /* handle packet with mission manager */ + _mission_manager->handle_message(msg); - } else { - /* when index is >= 0, send this parameter again */ - mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index); - } - } + /* handle packet with parameter component */ + _parameters_manager->handle_message(msg); - } break; + if (get_forwarding_on()) { + /* forward any messages to other mavlink instances */ + Mavlink::forward_message(msg, this); } } -int +void Mavlink::send_statustext_info(const char *string) { - return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string); + send_statustext(MAV_SEVERITY_INFO, string); } -int +void Mavlink::send_statustext_critical(const char *string) { - return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string); + send_statustext(MAV_SEVERITY_CRITICAL, string); } -int +void Mavlink::send_statustext_emergency(const char *string) { - return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string); + send_statustext(MAV_SEVERITY_EMERGENCY, string); } -int -Mavlink::send_statustext(unsigned severity, const char *string) +void +Mavlink::send_statustext(unsigned char severity, const char *string) { - const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; - mavlink_statustext_t statustext; - - int i = 0; - - while (i < len - 1) { - statustext.text[i] = string[i]; - - if (string[i] == '\0') { - break; - } - - i++; - } - - if (i > 1) { - /* Enforce null termination */ - statustext.text[i] = '\0'; - - /* Map severity */ - switch (severity) { - case MAVLINK_IOC_SEND_TEXT_INFO: - statustext.severity = MAV_SEVERITY_INFO; - break; + struct mavlink_logmessage logmsg; + strncpy(logmsg.text, string, sizeof(logmsg.text)); + logmsg.severity = severity; - case MAVLINK_IOC_SEND_TEXT_CRITICAL: - statustext.severity = MAV_SEVERITY_CRITICAL; - break; - - case MAVLINK_IOC_SEND_TEXT_EMERGENCY: - statustext.severity = MAV_SEVERITY_EMERGENCY; - break; - } - - mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text); - return OK; - - } else { - return ERROR; - } + mavlink_logbuffer_write(&_logbuffer, &logmsg); } MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) @@ -1032,11 +924,17 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) return sub_new; } +unsigned int +Mavlink::interval_from_rate(float rate) +{ + return (rate > 0.0f) ? (1000000.0f / rate) : 0; +} + int Mavlink::configure_stream(const char *stream_name, const float rate) { /* calculate interval in us, 0 means disabled stream */ - unsigned int interval = (rate > 0.0f) ? (1000000.0f / rate) : 0; + unsigned int interval = interval_from_rate(rate); /* search if stream exists */ MavlinkStream *stream; @@ -1067,10 +965,8 @@ Mavlink::configure_stream(const char *stream_name, const float rate) if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { /* create new instance */ - stream = streams_list[i]->new_instance(); - stream->set_channel(get_channel()); + stream = streams_list[i]->new_instance(this); stream->set_interval(interval); - stream->subscribe(this); LL_APPEND(_streams, stream); return OK; @@ -1084,6 +980,31 @@ Mavlink::configure_stream(const char *stream_name, const float rate) } void +Mavlink::adjust_stream_rates(const float multiplier) +{ + /* do not allow to push us to zero */ + if (multiplier < 0.01f) { + return; + } + + /* search if stream exists */ + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + /* set new interval */ + unsigned interval = stream->get_interval(); + interval /= multiplier; + + /* allow max ~600 Hz */ + if (interval < 1600) { + interval = 1600; + } + + /* set new interval */ + stream->set_interval(interval * multiplier); + } +} + +void Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) { /* orb subscription must be done from the main thread, @@ -1243,7 +1164,27 @@ Mavlink::pass_message(const mavlink_message_t *msg) float Mavlink::get_rate_mult() { - return _datarate / 1000.0f; + return _rate_mult; +} + +void +Mavlink::update_rate_mult() +{ + float const_rate = 0.0f; + float rate = 0.0f; + + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + if (stream->const_rate()) { + const_rate += stream->get_size() * 1000000.0f / stream->get_interval(); + + } else { + rate += stream->get_size() * 1000000.0f / stream->get_interval(); + } + } + + /* don't scale up rates, only scale down if needed */ + _rate_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate); } int @@ -1297,7 +1238,10 @@ Mavlink::task_main(int argc, char *argv[]) _mode = MAVLINK_MODE_CUSTOM; } else if (strcmp(optarg, "camera") == 0) { - _mode = MAVLINK_MODE_CAMERA; + // left in here for compatibility + _mode = MAVLINK_MODE_ONBOARD; + } else if (strcmp(optarg, "onboard") == 0) { + _mode = MAVLINK_MODE_ONBOARD; } break; @@ -1357,8 +1301,8 @@ Mavlink::task_main(int argc, char *argv[]) warnx("mode: CUSTOM"); break; - case MAVLINK_MODE_CAMERA: - warnx("mode: CAMERA"); + case MAVLINK_MODE_ONBOARD: + warnx("mode: ONBOARD"); break; default: @@ -1381,6 +1325,9 @@ Mavlink::task_main(int argc, char *argv[]) return ERROR; } + /* initialize send mutex */ + pthread_mutex_init(&_send_mutex, NULL); + /* initialize mavlink text message buffering */ mavlink_logbuffer_init(&_logbuffer, 5); @@ -1390,7 +1337,7 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) { + if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { errx(1, "can't allocate message buffer, exiting"); } @@ -1410,12 +1357,6 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ _receive_thread = MavlinkReceiver::receive_start(this); - _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - - /* create mission manager */ - _mission_manager = new MavlinkMissionManager(this); - _mission_manager->set_verbose(_verbose); - _task_running = true; MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); @@ -1426,48 +1367,63 @@ Mavlink::task_main(int argc, char *argv[]) struct vehicle_status_s status; status_sub->update(&status_time, &status); - MavlinkCommandsStream commands_stream(this, _channel); - - /* add default streams depending on mode and intervals depending on datarate */ - float rate_mult = get_rate_mult(); + /* add default streams depending on mode */ + /* HEARTBEAT is constant rate stream, rate never adjusted */ configure_stream("HEARTBEAT", 1.0f); + /* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */ + configure_stream("STATUSTEXT", 20.0f); + + /* COMMAND_LONG stream: use high rate to avoid commands skipping */ + configure_stream("COMMAND_LONG", 100.0f); + + /* PARAM_VALUE stream */ + _parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this); + _parameters_manager->set_interval(interval_from_rate(30.0f)); + LL_APPEND(_streams, _parameters_manager); + + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on + * remote requests rate. Rate specified here controls how much bandwidth we will reserve for + * mission messages. */ + _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); + _mission_manager->set_interval(interval_from_rate(10.0f)); + _mission_manager->set_verbose(_verbose); + LL_APPEND(_streams, _mission_manager); + switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); - configure_stream("HIGHRES_IMU", 1.0f * rate_mult); - configure_stream("ATTITUDE", 10.0f * rate_mult); - configure_stream("VFR_HUD", 8.0f * rate_mult); - configure_stream("GPS_RAW_INT", 1.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); - configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); - configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); - configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); - configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult); - configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult); + configure_stream("HIGHRES_IMU", 1.0f); + configure_stream("ATTITUDE", 10.0f); + configure_stream("VFR_HUD", 8.0f); + configure_stream("GPS_RAW_INT", 1.0f); + configure_stream("GLOBAL_POSITION_INT", 3.0f); + configure_stream("LOCAL_POSITION_NED", 3.0f); + configure_stream("RC_CHANNELS_RAW", 1.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); + configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); + configure_stream("OPTICAL_FLOW_RAD", 5.0f); break; - case MAVLINK_MODE_CAMERA: + case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 15.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult); - configure_stream("CAMERA_CAPTURE", 1.0f); + configure_stream("ATTITUDE", 50.0f); + configure_stream("GLOBAL_POSITION_INT", 50.0f); + configure_stream("CAMERA_CAPTURE", 2.0f); + configure_stream("ATTITUDE_TARGET", 10.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); + configure_stream("VFR_HUD", 10.0f); break; default: break; } - /* don't send parameters on startup without request */ - _mavlink_param_queue_index = param_count(); - - MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); - /* set main loop delay depending on data rate to minimize CPU overhead */ - _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; + _main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate; /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); @@ -1480,6 +1436,8 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); + update_rate_mult(); + if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ mavlink_update_system(); @@ -1490,9 +1448,6 @@ Mavlink::task_main(int argc, char *argv[]) set_hil_enabled(status.hil_state == HIL_STATE_ON); } - /* update commands stream */ - commands_stream.update(t); - /* check for requested subscriptions */ if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { @@ -1518,20 +1473,6 @@ Mavlink::task_main(int argc, char *argv[]) stream->update(t); } - if (fast_rate_limiter.check(t)) { - mavlink_pm_queued_send(); - _mission_manager->eventloop(); - - if (!mavlink_logbuffer_is_empty(&_logbuffer)) { - struct mavlink_logmessage msg; - int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); - - if (lb_ret == OK) { - send_statustext(msg.severity, msg.text); - } - } - } - /* pass messages from other UARTs or FTP worker */ if (_passing_on || _ftp_on) { @@ -1576,7 +1517,7 @@ Mavlink::task_main(int argc, char *argv[]) pthread_mutex_unlock(&_message_buffer_mutex); - _mavlink_resend_uart(_channel, &msg); + resend_message(&msg); } } @@ -1591,14 +1532,13 @@ Mavlink::task_main(int argc, char *argv[]) _bytes_txerr = 0; _bytes_rx = 0; } + _bytes_timestamp = t; } perf_end(_loop_perf); } - delete _mission_manager; - delete _subscribe_to_stream; _subscribe_to_stream = nullptr; @@ -1696,7 +1636,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2700, + 2800, (main_t)&Mavlink::start_helper, (const char **)argv); @@ -1731,6 +1671,8 @@ Mavlink::display_status() printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); } + printf("\tmavlink chan: #%u\n", _channel); + if (_rstatus.timestamp > 0) { printf("\ttype:\t\t"); @@ -1756,10 +1698,12 @@ Mavlink::display_status() } else { printf("\tno telem status.\n"); } + printf("\trates:\n"); printf("\ttx: %.3f kB/s\n", (double)_rate_tx); printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr); printf("\trx: %.3f kB/s\n", (double)_rate_rx); + printf("\trate mult: %.3f\n", (double)_rate_mult); } int diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 70d13acb0..ad5e5001b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -58,7 +58,7 @@ #include "mavlink_stream.h" #include "mavlink_messages.h" #include "mavlink_mission.h" - +#include "mavlink_parameters.h" class Mavlink { @@ -127,7 +127,7 @@ public: enum MAVLINK_MODE { MAVLINK_MODE_NORMAL = 0, MAVLINK_MODE_CUSTOM, - MAVLINK_MODE_CAMERA + MAVLINK_MODE_ONBOARD }; void set_mode(enum MAVLINK_MODE); @@ -137,6 +137,8 @@ public: 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; } @@ -160,7 +162,12 @@ public: */ int set_hil_enabled(bool hil_enabled); - void send_message(const mavlink_message_t *msg); + 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); @@ -188,29 +195,33 @@ public: * * @param string the message to send (will be capped by mavlink max string length) */ - int send_statustext_info(const char *string); + 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) */ - int send_statustext_critical(const char *string); + 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) */ - int send_statustext_emergency(const char *string); + void send_statustext_emergency(const char *string); /** - * Send a status text with loglevel + * 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, one of + * @param severity the log level */ - int send_statustext(unsigned severity, const char *string); + void send_statustext(unsigned char severity, const char *string); + MavlinkStream * get_streams() const { return _streams; } float get_rate_mult(); @@ -223,7 +234,7 @@ public: 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); } @@ -252,6 +263,10 @@ public: */ 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; @@ -264,6 +279,7 @@ private: /* 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. */ @@ -274,9 +290,8 @@ private: MavlinkStream *_streams; MavlinkMissionManager *_mission_manager; + MavlinkParametersManager *_parameters_manager; - orb_advert_t _mission_pub; - int _mission_result_sub; MAVLINK_MODE _mode; mavlink_channel_t _channel; @@ -292,7 +307,9 @@ private: bool _ftp_on; int _uart_fd; int _baudrate; - int _datarate; + 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 @@ -307,6 +324,8 @@ private: 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; @@ -328,62 +347,41 @@ private: 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 */ - /** - * Send one parameter. - * - * @param param The parameter id to send. - * @return zero on success, nonzero on failure. - */ - int mavlink_pm_send_param(param_t param); + void mavlink_update_system(); - /** - * Send one parameter identified by index. - * - * @param index The index of the parameter to send. - * @return zero on success, nonzero else. - */ - int mavlink_pm_send_param_for_index(uint16_t index); + int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); /** - * Send one parameter identified by name. + * Get the free space in the transmit buffer * - * @param name The index of the parameter to send. - * @return zero on success, nonzero else. + * @return free space in the UART TX buffer */ - int mavlink_pm_send_param_for_name(const char *name); + unsigned get_free_tx_buf(); - /** - * Send a queue of parameters, one parameter per function call. - * - * @return zero on success, nonzero on failure - */ - int mavlink_pm_queued_send(void); + static unsigned int interval_from_rate(float rate); + + int configure_stream(const char *stream_name, const float rate); /** - * Start sending the parameter queue. + * Adjust the stream rates based on the current rate * - * This function will not directly send parameters, but instead - * activate the sending of one parameter on each call of - * mavlink_pm_queued_send(). - * @see mavlink_pm_queued_send() + * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease */ - void mavlink_pm_start_queued_send(); - - void mavlink_update_system(); - - int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - - int configure_stream(const char *stream_name, const float rate); + void adjust_stream_rates(const float multiplier); int message_buffer_init(int size); @@ -399,6 +397,11 @@ private: 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); /** diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6885bebde..a8f956ad0 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -40,9 +40,9 @@ */ #include <stdio.h> + #include <commander/px4_custom_mode.h> #include <lib/geo/geo.h> - #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> @@ -72,11 +72,11 @@ #include <drivers/drv_rc_input.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_range_finder.h> - #include <systemlib/err.h> +#include <mavlink/mavlink_log.h> #include "mavlink_messages.h" - +#include "mavlink_main.h" static uint16_t cm_uint16_from_m_float(float m); static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, @@ -162,6 +162,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_AUTO_RTL: + /* fallthrough */ + case NAVIGATION_STATE_AUTO_RCRECOVER: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -170,6 +172,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_LAND: + case NAVIGATION_STATE_AUTO_LANDENGFAIL: + case NAVIGATION_STATE_AUTO_LANDGPSFAIL: /* fallthrough */ case NAVIGATION_STATE_DESCEND: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED @@ -249,61 +253,216 @@ public: return MAVLINK_MSG_ID_HEARTBEAT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamHeartbeat(); + return new MavlinkStreamHeartbeat(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_HEARTBEAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + bool const_rate() { + return true; } private: - MavlinkOrbSubscription *status_sub; - MavlinkOrbSubscription *pos_sp_triplet_sub; + MavlinkOrbSubscription *_status_sub; + MavlinkOrbSubscription *_pos_sp_triplet_sub; /* do not allow top copying this class */ MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &); MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &); protected: - explicit MavlinkStreamHeartbeat() : MavlinkStream(), - status_sub(nullptr), - pos_sp_triplet_sub(nullptr) + explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) {} - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - } - void send(const hrt_abstime t) { struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; /* always send the heartbeat, independent of the update status of the topics */ - if (!status_sub->update(&status)) { + if (!_status_sub->update(&status)) { /* if topic update failed fill it with defaults */ memset(&status, 0, sizeof(status)); } - if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) { + if (!_pos_sp_triplet_sub->update(&pos_sp_triplet)) { /* if topic update failed fill it with defaults */ memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); } - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + mavlink_heartbeat_t msg; - mavlink_msg_heartbeat_send(_channel, - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); + msg.base_mode = 0; + msg.custom_mode = 0; + get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode); + msg.type = _mavlink->get_system_type(); + msg.autopilot = MAV_AUTOPILOT_PX4; + msg.mavlink_version = 3; + + _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg); } }; +class MavlinkStreamStatustext : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamStatustext::get_name_static(); + } + + static const char *get_name_static() + { + return "STATUSTEXT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_STATUSTEXT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamStatustext(mavlink); + } + + unsigned get_size() { + return mavlink_logbuffer_is_empty(_mavlink->get_logbuffer()) ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); + } + +private: + /* do not allow top copying this class */ + MavlinkStreamStatustext(MavlinkStreamStatustext &); + MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &); + FILE *fp = nullptr; + +protected: + explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + ~MavlinkStreamStatustext() { + if (fp) { + fclose(fp); + } + } + + void send(const hrt_abstime t) + { + if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) { + struct mavlink_logmessage logmsg; + int lb_ret = mavlink_logbuffer_read(_mavlink->get_logbuffer(), &logmsg); + + if (lb_ret == OK) { + mavlink_statustext_t msg; + + msg.severity = logmsg.severity; + strncpy(msg.text, logmsg.text, sizeof(msg.text)); + + _mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg); + + /* write log messages in first instance to disk */ + if (_mavlink->get_instance_id() == 0) { + if (fp) { + fputs(msg.text, fp); + fputs("\n", fp); + fsync(fileno(fp)); + } else { + /* string to hold the path to the log */ + char log_file_name[32] = ""; + char log_file_path[64] = ""; + + timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + struct tm tt; + gmtime_r(&gps_time_sec, &tt); + + // XXX we do not want to interfere here with the SD log app + strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt); + snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); + fp = fopen(log_file_path, "ab"); + } + } + } + } + } +}; + +class MavlinkStreamCommandLong : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamCommandLong::get_name_static(); + } + + static const char *get_name_static() + { + return "COMMAND_LONG"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_COMMAND_LONG; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamCommandLong(mavlink); + } + + unsigned get_size() { + return 0; // commands stream is not regular and not predictable + } + +private: + MavlinkOrbSubscription *_cmd_sub; + uint64_t _cmd_time; + + /* do not allow top copying this class */ + MavlinkStreamCommandLong(MavlinkStreamCommandLong &); + MavlinkStreamCommandLong& operator = (const MavlinkStreamCommandLong &); + +protected: + explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink), + _cmd_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_command))), + _cmd_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_command_s cmd; + + if (_cmd_sub->update(&_cmd_time, &cmd)) { + /* only send commands for other systems/components */ + if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { + mavlink_command_long_t msg; + + msg.target_system = cmd.target_system; + msg.target_component = cmd.target_component; + msg.command = cmd.command; + msg.confirmation = cmd.confirmation; + msg.param1 = cmd.param1; + msg.param2 = cmd.param2; + msg.param3 = cmd.param3; + msg.param4 = cmd.param4; + msg.param5 = cmd.param5; + msg.param6 = cmd.param6; + msg.param7 = cmd.param7; + + _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg); + } + } + } +}; class MavlinkStreamSysStatus : public MavlinkStream { @@ -313,7 +472,7 @@ public: return MavlinkStreamSysStatus::get_name_static(); } - static const char *get_name_static () + static const char *get_name_static() { return "SYS_STATUS"; } @@ -323,47 +482,50 @@ public: return MAVLINK_MSG_ID_SYS_STATUS; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamSysStatus(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamSysStatus(); + return MAVLINK_MSG_ID_SYS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *_status_sub; /* do not allow top copying this class */ MavlinkStreamSysStatus(MavlinkStreamSysStatus &); MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &); protected: - explicit MavlinkStreamSysStatus() : MavlinkStream(), - status_sub(nullptr) + explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) {} - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - } - void send(const hrt_abstime t) { struct vehicle_status_s status; - if (status_sub->update(&status)) { - mavlink_msg_sys_status_send(_channel, - status.onboard_control_sensors_present, - status.onboard_control_sensors_enabled, - status.onboard_control_sensors_health, - status.load * 1000.0f, - status.battery_voltage * 1000.0f, - status.battery_current * 100.0f, - status.battery_remaining * 100.0f, - status.drop_rate_comm, - status.errors_comm, - status.errors_count1, - status.errors_count2, - status.errors_count3, - status.errors_count4); + if (_status_sub->update(&status)) { + mavlink_sys_status_t msg; + + msg.onboard_control_sensors_present = status.onboard_control_sensors_present; + msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled; + msg.onboard_control_sensors_health = status.onboard_control_sensors_health; + msg.load = status.load * 1000.0f; + msg.voltage_battery = status.battery_voltage * 1000.0f; + msg.current_battery = status.battery_current * 100.0f; + msg.drop_rate_comm = status.drop_rate_comm; + msg.errors_comm = status.errors_comm; + msg.errors_count1 = status.errors_count1; + msg.errors_count2 = status.errors_count2; + msg.errors_count3 = status.errors_count3; + msg.errors_count4 = status.errors_count4; + msg.battery_remaining = status.battery_remaining * 100.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg); } } }; @@ -387,78 +549,89 @@ public: return MAVLINK_MSG_ID_HIGHRES_IMU; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamHighresIMU(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamHighresIMU(); + return MAVLINK_MSG_ID_HIGHRES_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *sensor_sub; - uint64_t sensor_time; + MavlinkOrbSubscription *_sensor_sub; + uint64_t _sensor_time; - uint64_t accel_timestamp; - uint64_t gyro_timestamp; - uint64_t mag_timestamp; - uint64_t baro_timestamp; + uint64_t _accel_timestamp; + uint64_t _gyro_timestamp; + uint64_t _mag_timestamp; + uint64_t _baro_timestamp; /* do not allow top copying this class */ MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); protected: - explicit MavlinkStreamHighresIMU() : MavlinkStream(), - sensor_sub(nullptr), - sensor_time(0), - accel_timestamp(0), - gyro_timestamp(0), - mag_timestamp(0), - baro_timestamp(0) + explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink), + _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), + _sensor_time(0), + _accel_timestamp(0), + _gyro_timestamp(0), + _mag_timestamp(0), + _baro_timestamp(0) {} - void subscribe(Mavlink *mavlink) - { - sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); - } - void send(const hrt_abstime t) { struct sensor_combined_s sensor; - if (sensor_sub->update(&sensor_time, &sensor)) { + if (_sensor_sub->update(&_sensor_time, &sensor)) { uint16_t fields_updated = 0; - if (accel_timestamp != sensor.accelerometer_timestamp) { + if (_accel_timestamp != sensor.accelerometer_timestamp) { /* mark first three dimensions as changed */ fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_timestamp = sensor.accelerometer_timestamp; + _accel_timestamp = sensor.accelerometer_timestamp; } - if (gyro_timestamp != sensor.timestamp) { + if (_gyro_timestamp != sensor.timestamp) { /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_timestamp = sensor.timestamp; + _gyro_timestamp = sensor.timestamp; } - if (mag_timestamp != sensor.magnetometer_timestamp) { + if (_mag_timestamp != sensor.magnetometer_timestamp) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_timestamp = sensor.magnetometer_timestamp; + _mag_timestamp = sensor.magnetometer_timestamp; } - if (baro_timestamp != sensor.baro_timestamp) { + if (_baro_timestamp != sensor.baro_timestamp) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_timestamp = sensor.baro_timestamp; + _baro_timestamp = sensor.baro_timestamp; } - mavlink_msg_highres_imu_send(_channel, - sensor.timestamp, - sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2], - sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2], - sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2], - sensor.baro_pres_mbar, sensor.differential_pressure_pa, - sensor.baro_alt_meter, sensor.baro_temp_celcius, - fields_updated); + mavlink_highres_imu_t msg; + + msg.time_usec = sensor.timestamp; + msg.xacc = sensor.accelerometer_m_s2[0]; + msg.yacc = sensor.accelerometer_m_s2[1]; + msg.zacc = sensor.accelerometer_m_s2[2]; + msg.xgyro = sensor.gyro_rad_s[0]; + msg.ygyro = sensor.gyro_rad_s[1]; + msg.zgyro = sensor.gyro_rad_s[2]; + msg.xmag = sensor.magnetometer_ga[0]; + msg.ymag = sensor.magnetometer_ga[1]; + msg.zmag = sensor.magnetometer_ga[2]; + msg.abs_pressure = sensor.baro_pres_mbar; + msg.diff_pressure = sensor.differential_pressure_pa; + msg.pressure_alt = sensor.baro_alt_meter; + msg.temperature = sensor.baro_temp_celcius; + msg.fields_updated = fields_updated; + + _mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg); } } }; @@ -482,14 +655,19 @@ public: return MAVLINK_MSG_ID_ATTITUDE; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamAttitude(); + return new MavlinkStreamAttitude(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; /* do not allow top copying this class */ MavlinkStreamAttitude(MavlinkStreamAttitude &); @@ -497,25 +675,27 @@ private: protected: - explicit MavlinkStreamAttitude() : MavlinkStream(), - att_sub(nullptr), - att_time(0) + explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _att_time(0) {} - void subscribe(Mavlink *mavlink) - { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_s att; - if (att_sub->update(&att_time, &att)) { - mavlink_msg_attitude_send(_channel, - att.timestamp / 1000, - att.roll, att.pitch, att.yaw, - att.rollspeed, att.pitchspeed, att.yawspeed); + if (_att_sub->update(&_att_time, &att)) { + mavlink_attitude_t msg; + + msg.time_boot_ms = att.timestamp / 1000; + msg.roll = att.roll; + msg.pitch = att.pitch; + msg.yaw = att.yaw; + msg.rollspeed = att.rollspeed; + msg.pitchspeed = att.pitchspeed; + msg.yawspeed = att.yawspeed; + + _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE, &msg); } } }; @@ -539,44 +719,47 @@ public: return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamAttitudeQuaternion(); + return new MavlinkStreamAttitudeQuaternion(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; /* do not allow top copying this class */ MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); protected: - explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), - att_sub(nullptr), - att_time(0) + explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _att_time(0) {} - void subscribe(Mavlink *mavlink) - { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_s att; - if (att_sub->update(&att_time, &att)) { - mavlink_msg_attitude_quaternion_send(_channel, - att.timestamp / 1000, - att.q[0], - att.q[1], - att.q[2], - att.q[3], - att.rollspeed, - att.pitchspeed, - att.yawspeed); + if (_att_sub->update(&_att_time, &att)) { + mavlink_attitude_quaternion_t msg; + + msg.time_boot_ms = att.timestamp / 1000; + msg.q1 = att.q[0]; + msg.q2 = att.q[1]; + msg.q3 = att.q[2]; + msg.q4 = att.q[3]; + msg.rollspeed = att.rollspeed; + msg.pitchspeed = att.pitchspeed; + msg.yawspeed = att.yawspeed; + + _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_QUATERNION, &msg); } } }; @@ -601,54 +784,55 @@ public: return MAVLINK_MSG_ID_VFR_HUD; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamVFRHUD(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamVFRHUD(); + return MAVLINK_MSG_ID_VFR_HUD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; - MavlinkOrbSubscription *armed_sub; - uint64_t armed_time; + MavlinkOrbSubscription *_armed_sub; + uint64_t _armed_time; - MavlinkOrbSubscription *act_sub; - uint64_t act_time; + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; - MavlinkOrbSubscription *airspeed_sub; - uint64_t airspeed_time; + MavlinkOrbSubscription *_airspeed_sub; + uint64_t _airspeed_time; + + MavlinkOrbSubscription *_sensor_combined_sub; + uint64_t _sensor_combined_time; /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); protected: - explicit MavlinkStreamVFRHUD() : MavlinkStream(), - att_sub(nullptr), - att_time(0), - pos_sub(nullptr), - pos_time(0), - armed_sub(nullptr), - armed_time(0), - act_sub(nullptr), - act_time(0), - airspeed_sub(nullptr), - airspeed_time(0) + explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _att_time(0), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _pos_time(0), + _armed_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_armed))), + _armed_time(0), + _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), + _act_time(0), + _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), + _airspeed_time(0), + _sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), + _sensor_combined_time(0) {} - void subscribe(Mavlink *mavlink) - { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed)); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); - airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_s att; @@ -656,25 +840,26 @@ protected: struct actuator_armed_s armed; struct actuator_controls_s act; struct airspeed_s airspeed; + struct sensor_combined_s sensor_combined; - bool updated = att_sub->update(&att_time, &att); - updated |= pos_sub->update(&pos_time, &pos); - updated |= armed_sub->update(&armed_time, &armed); - updated |= act_sub->update(&act_time, &act); - updated |= airspeed_sub->update(&airspeed_time, &airspeed); + bool updated = _att_sub->update(&_att_time, &att); + updated |= _pos_sub->update(&_pos_time, &pos); + updated |= _armed_sub->update(&_armed_time, &armed); + updated |= _act_sub->update(&_act_time, &act); + updated |= _airspeed_sub->update(&_airspeed_time, &airspeed); + updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined); if (updated) { - float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); - uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; - float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - - mavlink_msg_vfr_hud_send(_channel, - airspeed.true_airspeed_m_s, - groundspeed, - heading, - throttle, - pos.alt, - -pos.vel_d); + mavlink_vfr_hud_t msg; + + msg.airspeed = airspeed.true_airspeed_m_s; + msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); + msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; + msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; + msg.alt = sensor_combined.baro_alt_meter; + msg.climb = -pos.vel_d; + + _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); } } }; @@ -698,46 +883,49 @@ public: return MAVLINK_MSG_ID_GPS_RAW_INT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamGPSRawInt(); + return new MavlinkStreamGPSRawInt(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *gps_sub; - uint64_t gps_time; + MavlinkOrbSubscription *_gps_sub; + uint64_t _gps_time; /* do not allow top copying this class */ MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); protected: - explicit MavlinkStreamGPSRawInt() : MavlinkStream(), - gps_sub(nullptr), - gps_time(0) + explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink), + _gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))), + _gps_time(0) {} - void subscribe(Mavlink *mavlink) - { - gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); - } - void send(const hrt_abstime t) { struct vehicle_gps_position_s gps; - if (gps_sub->update(&gps_time, &gps)) { - mavlink_msg_gps_raw_int_send(_channel, - gps.timestamp_position, - gps.fix_type, - gps.lat, - gps.lon, - gps.alt, - cm_uint16_from_m_float(gps.eph), - cm_uint16_from_m_float(gps.epv), - gps.vel_m_s * 100.0f, - _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps.satellites_used); + if (_gps_sub->update(&_gps_time, &gps)) { + mavlink_gps_raw_int_t msg; + + msg.time_usec = gps.timestamp_position; + msg.fix_type = gps.fix_type; + msg.lat = gps.lat; + msg.lon = gps.lon; + msg.alt = gps.alt; + msg.eph = cm_uint16_from_m_float(gps.eph); + msg.epv = cm_uint16_from_m_float(gps.epv); + msg.vel = cm_uint16_from_m_float(gps.vel_m_s), + msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + msg.satellites_visible = gps.satellites_used; + + _mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg); } } }; @@ -761,55 +949,57 @@ public: return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGlobalPositionInt(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamGlobalPositionInt(); + return MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; - MavlinkOrbSubscription *home_sub; - uint64_t home_time; + MavlinkOrbSubscription *_home_sub; + uint64_t _home_time; /* do not allow top copying this class */ MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); protected: - explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), - pos_sub(nullptr), - pos_time(0), - home_sub(nullptr), - home_time(0) + explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _pos_time(0), + _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), + _home_time(0) {} - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - } - void send(const hrt_abstime t) { struct vehicle_global_position_s pos; struct home_position_s home; - bool updated = pos_sub->update(&pos_time, &pos); - updated |= home_sub->update(&home_time, &home); + bool updated = _pos_sub->update(&_pos_time, &pos); + updated |= _home_sub->update(&_home_time, &home); if (updated) { - mavlink_msg_global_position_int_send(_channel, - pos.timestamp / 1000, - pos.lat * 1e7, - pos.lon * 1e7, - pos.alt * 1000.0f, - (pos.alt - home.alt) * 1000.0f, - pos.vel_n * 100.0f, - pos.vel_e * 100.0f, - pos.vel_d * 100.0f, - _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f); + mavlink_global_position_int_t msg; + + msg.time_boot_ms = pos.timestamp / 1000; + msg.lat = pos.lat * 1e7; + msg.lon = pos.lon * 1e7; + msg.alt = pos.alt * 1000.0f; + msg.relative_alt = (pos.alt - home.alt) * 1000.0f; + msg.vx = pos.vel_n * 100.0f; + msg.vy = pos.vel_e * 100.0f; + msg.vz = pos.vel_d * 100.0f; + msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg); } } }; @@ -833,49 +1023,51 @@ public: return MAVLINK_MSG_ID_LOCAL_POSITION_NED; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamLocalPositionNED(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamLocalPositionNED(); + return MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; /* do not allow top copying this class */ MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); protected: - explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), - pos_sub(nullptr), - pos_time(0) + explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), + _pos_time(0) {} - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); - } - void send(const hrt_abstime t) { struct vehicle_local_position_s pos; - if (pos_sub->update(&pos_time, &pos)) { - mavlink_msg_local_position_ned_send(_channel, - pos.timestamp / 1000, - pos.x, - pos.y, - pos.z, - pos.vx, - pos.vy, - pos.vz); + if (_pos_sub->update(&_pos_time, &pos)) { + mavlink_local_position_ned_t msg; + + msg.time_boot_ms = pos.timestamp / 1000; + msg.x = pos.x; + msg.y = pos.y; + msg.z = pos.z; + msg.vx = pos.vx; + msg.vy = pos.vy; + msg.vz = pos.vz; + + _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg); } } }; - class MavlinkStreamViconPositionEstimate : public MavlinkStream { public: @@ -894,43 +1086,46 @@ public: return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamViconPositionEstimate(); + return new MavlinkStreamViconPositionEstimate(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; /* do not allow top copying this class */ MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); protected: - explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), - pos_sub(nullptr), - pos_time(0) + explicit MavlinkStreamViconPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position))), + _pos_time(0) {} - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); - } - void send(const hrt_abstime t) { struct vehicle_vicon_position_s pos; - if (pos_sub->update(&pos_time, &pos)) { - mavlink_msg_vicon_position_estimate_send(_channel, - pos.timestamp / 1000, - pos.x, - pos.y, - pos.z, - pos.roll, - pos.pitch, - pos.yaw); + if (_pos_sub->update(&_pos_time, &pos)) { + mavlink_vicon_position_estimate_t msg; + + msg.usec = pos.timestamp; + msg.x = pos.x; + msg.y = pos.y; + msg.z = pos.z; + msg.roll = pos.roll; + msg.pitch = pos.pitch; + msg.yaw = pos.yaw; + + _mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg); } } }; @@ -954,45 +1149,49 @@ public: return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGPSGlobalOrigin(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamGPSGlobalOrigin(); + return _home_sub->is_published() ? (MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *home_sub; + MavlinkOrbSubscription *_home_sub; /* do not allow top copying this class */ MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); protected: - explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(), - home_sub(nullptr) + explicit MavlinkStreamGPSGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink), + _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))) {} - void subscribe(Mavlink *mavlink) - { - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - } - void send(const hrt_abstime t) { /* we're sending the GPS home periodically to ensure the * the GCS does pick it up at one point */ - if (home_sub->is_published()) { + if (_home_sub->is_published()) { struct home_position_s home; - if (home_sub->update(&home)) { - mavlink_msg_gps_global_origin_send(_channel, - (int32_t)(home.lat * 1e7), - (int32_t)(home.lon * 1e7), - (int32_t)(home.alt) * 1000.0f); + if (_home_sub->update(&home)) { + mavlink_gps_global_origin_t msg; + + msg.latitude = home.lat * 1e7; + msg.longitude = home.lon * 1e7; + msg.altitude = home.alt * 1e3f; + + _mavlink->send_message(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, &msg); } } } }; + template <int N> class MavlinkStreamServoOutputRaw : public MavlinkStream { @@ -1024,26 +1223,28 @@ public: } } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamServoOutputRaw<N>(); + return new MavlinkStreamServoOutputRaw<N>(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *act_sub; - uint64_t act_time; + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; /* do not allow top copying this class */ MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); protected: - explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), - act_sub(nullptr), - act_time(0) - {} - - void subscribe(Mavlink *mavlink) + explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink), + _act_sub(nullptr), + _act_time(0) { orb_id_t act_topics[] = { ORB_ID(actuator_outputs_0), @@ -1052,25 +1253,28 @@ protected: ORB_ID(actuator_outputs_3) }; - act_sub = mavlink->add_orb_subscription(act_topics[N]); + _act_sub = _mavlink->add_orb_subscription(act_topics[N]); } void send(const hrt_abstime t) { struct actuator_outputs_s act; - if (act_sub->update(&act_time, &act)) { - mavlink_msg_servo_output_raw_send(_channel, - act.timestamp / 1000, - N, - act.output[0], - act.output[1], - act.output[2], - act.output[3], - act.output[4], - act.output[5], - act.output[6], - act.output[7]); + if (_act_sub->update(&_act_time, &act)) { + mavlink_servo_output_raw_t msg; + + msg.time_usec = act.timestamp; + msg.port = N; + msg.servo1_raw = act.output[0]; + msg.servo2_raw = act.output[1]; + msg.servo3_raw = act.output[2]; + msg.servo4_raw = act.output[3]; + msg.servo5_raw = act.output[4]; + msg.servo6_raw = act.output[5]; + msg.servo7_raw = act.output[6]; + msg.servo8_raw = act.output[7]; + + _mavlink->send_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg); } } }; @@ -1094,51 +1298,49 @@ public: return MAVLINK_MSG_ID_HIL_CONTROLS; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamHILControls(); + return new MavlinkStreamHILControls(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_HIL_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *status_sub; - uint64_t status_time; + MavlinkOrbSubscription *_status_sub; + uint64_t _status_time; - MavlinkOrbSubscription *pos_sp_triplet_sub; - uint64_t pos_sp_triplet_time; + MavlinkOrbSubscription *_pos_sp_triplet_sub; + uint64_t _pos_sp_triplet_time; - MavlinkOrbSubscription *act_sub; - uint64_t act_time; + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; /* do not allow top copying this class */ MavlinkStreamHILControls(MavlinkStreamHILControls &); MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); protected: - explicit MavlinkStreamHILControls() : MavlinkStream(), - status_sub(nullptr), - status_time(0), - pos_sp_triplet_sub(nullptr), - pos_sp_triplet_time(0), - act_sub(nullptr), - act_time(0) + explicit MavlinkStreamHILControls(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _status_time(0), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), + _pos_sp_triplet_time(0), + _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0))), + _act_time(0) {} - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); - } - void send(const hrt_abstime t) { struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; struct actuator_outputs_s act; - bool updated = act_sub->update(&act_time, &act); - updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet); - updated |= status_sub->update(&status_time, &status); + bool updated = _act_sub->update(&_act_time, &act); + updated |= _pos_sp_triplet_sub->update(&_pos_sp_triplet_time, &pos_sp_triplet); + updated |= _status_sub->update(&_status_time, &status); if (updated && (status.arming_state == ARMING_STATE_ARMED)) { /* translate the current syste state to mavlink state and mode */ @@ -1151,15 +1353,17 @@ protected: const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + unsigned system_type = _mavlink->get_system_type(); + /* scale outputs depending on system type */ - if (mavlink_system.type == MAV_TYPE_QUADROTOR || - mavlink_system.type == MAV_TYPE_HEXAROTOR || - mavlink_system.type == MAV_TYPE_OCTOROTOR) { + if (system_type == MAV_TYPE_QUADROTOR || + system_type == MAV_TYPE_HEXAROTOR || + system_type == MAV_TYPE_OCTOROTOR) { /* multirotors: set number of rotor outputs depending on type */ unsigned n; - switch (mavlink_system.type) { + switch (system_type) { case MAV_TYPE_QUADROTOR: n = 4; break; @@ -1174,7 +1378,7 @@ protected: } for (unsigned i = 0; i < 8; i++) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + if (act.output[i] > PWM_LOWEST_MIN / 2) { if (i < n) { /* scale PWM out 900..2100 us to 0..1 for rotors */ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); @@ -1185,7 +1389,7 @@ protected: } } else { - /* send 0 when disarmed */ + /* send 0 when disarmed and for disabled channels */ out[i] = 0.0f; } } @@ -1194,79 +1398,96 @@ protected: /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ for (unsigned i = 0; i < 8; i++) { - if (i != 3) { - /* scale PWM out 900..2100 us to -1..1 for normal channels */ - out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + if (act.output[i] > PWM_LOWEST_MIN / 2) { + if (i != 3) { + /* scale PWM out 900..2100 us to -1..1 for normal channels */ + out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + + } else { + /* scale PWM out 900..2100 us to 0..1 for throttle */ + out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + } } else { - /* scale PWM out 900..2100 us to 0..1 for throttle */ - out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + /* set 0 for disabled channels */ + out[i] = 0.0f; } - } } - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + mavlink_hil_controls_t msg; + + msg.time_usec = hrt_absolute_time(); + msg.roll_ailerons = out[0]; + msg.pitch_elevator = out[1]; + msg.yaw_rudder = out[2]; + msg.throttle = out[3]; + msg.aux1 = out[4]; + msg.aux2 = out[5]; + msg.aux3 = out[6]; + msg.aux4 = out[7]; + msg.mode = mavlink_base_mode; + msg.nav_mode = 0; + + _mavlink->send_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg); } } }; -class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); + return MavlinkStreamPositionTargetGlobalInt::get_name_static(); } static const char *get_name_static() { - return "GLOBAL_POSITION_SETPOINT_INT"; + return "POSITION_TARGET_GLOBAL_INT"; } uint8_t get_id() { - return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; + return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamGlobalPositionSetpointInt(); + return new MavlinkStreamPositionTargetGlobalInt(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sp_triplet_sub; + MavlinkOrbSubscription *_pos_sp_triplet_sub; /* do not allow top copying this class */ - MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); - MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); + MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &); + MavlinkStreamPositionTargetGlobalInt& operator = (const MavlinkStreamPositionTargetGlobalInt &); protected: - explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(), - pos_sp_triplet_sub(nullptr) + explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) {} - void subscribe(Mavlink *mavlink) - { - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - } - void send(const hrt_abstime t) { struct position_setpoint_triplet_s pos_sp_triplet; - if (pos_sp_triplet_sub->update(&pos_sp_triplet)) { - mavlink_msg_global_position_setpoint_int_send(_channel, - MAV_FRAME_GLOBAL, - (int32_t)(pos_sp_triplet.current.lat * 1e7), - (int32_t)(pos_sp_triplet.current.lon * 1e7), - (int32_t)(pos_sp_triplet.current.alt * 1000), - (int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f)); + if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { + mavlink_position_target_global_int_t msg{}; + + msg.coordinate_frame = MAV_FRAME_GLOBAL; + msg.lat_int = pos_sp_triplet.current.lat * 1e7; + msg.lon_int = pos_sp_triplet.current.lon * 1e7; + msg.alt = pos_sp_triplet.current.alt; + + _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, &msg); } } }; @@ -1282,165 +1503,124 @@ public: static const char *get_name_static() { - return "LOCAL_POSITION_SETPOINT"; + return "POSITION_TARGET_LOCAL_NED"; } uint8_t get_id() { - return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamLocalPositionSetpoint(); + return new MavlinkStreamLocalPositionSetpoint(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sp_sub; - uint64_t pos_sp_time; + MavlinkOrbSubscription *_pos_sp_sub; + uint64_t _pos_sp_time; /* do not allow top copying this class */ MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); protected: - explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), - pos_sp_sub(nullptr), - pos_sp_time(0) + explicit MavlinkStreamLocalPositionSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint))), + _pos_sp_time(0) {} - void subscribe(Mavlink *mavlink) - { - pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); - } - void send(const hrt_abstime t) { struct vehicle_local_position_setpoint_s pos_sp; - if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) { - mavlink_msg_local_position_setpoint_send(_channel, - MAV_FRAME_LOCAL_NED, - pos_sp.x, - pos_sp.y, - pos_sp.z, - pos_sp.yaw); + if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) { + mavlink_position_target_local_ned_t msg{}; + + msg.time_boot_ms = pos_sp.timestamp / 1000; + msg.coordinate_frame = MAV_FRAME_LOCAL_NED; + msg.x = pos_sp.x; + msg.y = pos_sp.y; + msg.z = pos_sp.z; + + _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg); } } }; -class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +class MavlinkStreamAttitudeTarget : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static(); + return MavlinkStreamAttitudeTarget::get_name_static(); } static const char *get_name_static() { - return "ROLL_PITCH_YAW_THRUST_SETPOINT"; + return "ATTITUDE_TARGET"; } uint8_t get_id() { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + return MAVLINK_MSG_ID_ATTITUDE_TARGET; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamAttitudeTarget(mavlink); } - static MavlinkStream *new_instance() + unsigned get_size() { - return new MavlinkStreamRollPitchYawThrustSetpoint(); + return MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *att_sp_sub; - uint64_t att_sp_time; + MavlinkOrbSubscription *_att_sp_sub; + MavlinkOrbSubscription *_att_rates_sp_sub; + uint64_t _att_sp_time; + uint64_t _att_rates_sp_time; /* do not allow top copying this class */ - MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); - MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); + MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &); + MavlinkStreamAttitudeTarget& operator = (const MavlinkStreamAttitudeTarget &); protected: - explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), - att_sp_sub(nullptr), - att_sp_time(0) + explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))), + _att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))), + _att_sp_time(0), + _att_rates_sp_time(0) {} - void subscribe(Mavlink *mavlink) - { - att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_setpoint_s att_sp; - if (att_sp_sub->update(&att_sp_time, &att_sp)) { - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, - att_sp.timestamp / 1000, - att_sp.roll_body, - att_sp.pitch_body, - att_sp.yaw_body, - att_sp.thrust); - } - } -}; - - -class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static(); - } - - static const char *get_name_static() - { - return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; - } + if (_att_sp_sub->update(&_att_sp_time, &att_sp)) { - uint8_t get_id() - { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; - } + struct vehicle_rates_setpoint_s att_rates_sp; + (void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp); - static MavlinkStream *new_instance() - { - return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); - } + mavlink_attitude_target_t msg{}; -private: - MavlinkOrbSubscription *att_rates_sp_sub; - uint64_t att_rates_sp_time; + msg.time_boot_ms = att_sp.timestamp / 1000; + mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q); - /* do not allow top copying this class */ - MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); - MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); + msg.body_roll_rate = att_rates_sp.roll; + msg.body_pitch_rate = att_rates_sp.pitch; + msg.body_yaw_rate = att_rates_sp.yaw; -protected: - explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), - att_rates_sp_sub(nullptr), - att_rates_sp_time(0) - {} + msg.thrust = att_sp.thrust; - void subscribe(Mavlink *mavlink) - { - att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); - } - - void send(const hrt_abstime t) - { - struct vehicle_rates_setpoint_s att_rates_sp; - - if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) { - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, - att_rates_sp.timestamp / 1000, - att_rates_sp.roll, - att_rates_sp.pitch, - att_rates_sp.yaw, - att_rates_sp.thrust); + _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_TARGET, &msg); } } }; @@ -1464,77 +1644,130 @@ public: return MAVLINK_MSG_ID_RC_CHANNELS_RAW; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRCChannelsRaw(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamRCChannelsRaw(); + return _rc_sub->is_published() ? ((RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) + + MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *rc_sub; - uint64_t rc_time; + MavlinkOrbSubscription *_rc_sub; + uint64_t _rc_time; /* do not allow top copying this class */ MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); protected: - explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), - rc_sub(nullptr), - rc_time(0) + explicit MavlinkStreamRCChannelsRaw(Mavlink *mavlink) : MavlinkStream(mavlink), + _rc_sub(_mavlink->add_orb_subscription(ORB_ID(input_rc))), + _rc_time(0) {} - void subscribe(Mavlink *mavlink) - { - rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); - } - void send(const hrt_abstime t) { struct rc_input_values rc; - if (rc_sub->update(&rc_time, &rc)) { + if (_rc_sub->update(&_rc_time, &rc)) { const unsigned port_width = 8; - // Deprecated message (but still needed for compatibility!) + /* deprecated message (but still needed for compatibility!) */ for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) { - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(_channel, - rc.timestamp_publication / 1000, - i, - (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX, - rc.rssi); + /* channels are sent in MAVLink main loop at a fixed interval */ + mavlink_rc_channels_raw_t msg; + + msg.time_boot_ms = rc.timestamp_publication / 1000; + msg.port = i; + msg.chan1_raw = (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX; + msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX; + msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX; + msg.chan5_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX; + msg.chan6_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX; + msg.chan7_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX; + msg.chan8_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX; + msg.rssi = rc.rssi; + + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg); } - // New message - mavlink_msg_rc_channels_send(_channel, - rc.timestamp_publication / 1000, - rc.channel_count, - ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX), - ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX), - ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX), - ((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX), - ((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX), - ((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX), - ((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX), - ((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX), - ((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX), - ((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX), - ((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX), - ((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX), - ((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX), - ((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX), - ((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX), - ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX), - ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX), - ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX), - rc.rssi); + /* new message */ + mavlink_rc_channels_t msg; + + msg.time_boot_ms = rc.timestamp_publication / 1000; + msg.chancount = rc.channel_count; + msg.chan1_raw = (rc.channel_count > 0) ? rc.values[0] : UINT16_MAX; + msg.chan2_raw = (rc.channel_count > 1) ? rc.values[1] : UINT16_MAX; + msg.chan3_raw = (rc.channel_count > 2) ? rc.values[2] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > 3) ? rc.values[3] : UINT16_MAX; + msg.chan5_raw = (rc.channel_count > 4) ? rc.values[4] : UINT16_MAX; + msg.chan6_raw = (rc.channel_count > 5) ? rc.values[5] : UINT16_MAX; + msg.chan7_raw = (rc.channel_count > 6) ? rc.values[6] : UINT16_MAX; + msg.chan8_raw = (rc.channel_count > 7) ? rc.values[7] : UINT16_MAX; + msg.chan9_raw = (rc.channel_count > 8) ? rc.values[8] : UINT16_MAX; + msg.chan10_raw = (rc.channel_count > 9) ? rc.values[9] : UINT16_MAX; + msg.chan11_raw = (rc.channel_count > 10) ? rc.values[10] : UINT16_MAX; + msg.chan12_raw = (rc.channel_count > 11) ? rc.values[11] : UINT16_MAX; + msg.chan13_raw = (rc.channel_count > 12) ? rc.values[12] : UINT16_MAX; + msg.chan14_raw = (rc.channel_count > 13) ? rc.values[13] : UINT16_MAX; + msg.chan15_raw = (rc.channel_count > 14) ? rc.values[14] : UINT16_MAX; + msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX; + msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX; + msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX; + + /* RSSI has a max value of 100, and when Spektrum or S.BUS are + * available, the RSSI field is invalid, as they do not provide + * an RSSI measurement. Use an out of band magic value to signal + * these digital ports. XXX revise MAVLink spec to address this. + * One option would be to use the top bit to toggle between RSSI + * and input source mode. + * + * Full RSSI field: 0b 1 111 1111 + * + * ^ If bit is set, RSSI encodes type + RSSI + * + * ^ These three bits encode a total of 8 + * digital RC input types. + * 0: PPM, 1: SBUS, 2: Spektrum, 2: ST24 + * ^ These four bits encode a total of + * 16 RSSI levels. 15 = full, 0 = no signal + * + */ + + /* Initialize RSSI with the special mode level flag */ + msg.rssi = (1 << 7); + + /* Set RSSI */ + msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15; + + switch (rc.input_source) { + case RC_INPUT_SOURCE_PX4FMU_PPM: + /* fallthrough */ + case RC_INPUT_SOURCE_PX4IO_PPM: + msg.rssi |= (0 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_SPEKTRUM: + msg.rssi |= (1 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_SBUS: + msg.rssi |= (2 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_ST24: + msg.rssi |= (3 << 4); + break; + } + + if (rc.rc_lost) { + /* RSSI is by definition zero */ + msg.rssi = 0; + } + + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); } } }; @@ -1558,101 +1791,113 @@ public: return MAVLINK_MSG_ID_MANUAL_CONTROL; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamManualControl(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamManualControl(); + return _manual_sub->is_published() ? (MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *manual_sub; - uint64_t manual_time; + MavlinkOrbSubscription *_manual_sub; + uint64_t _manual_time; /* do not allow top copying this class */ MavlinkStreamManualControl(MavlinkStreamManualControl &); MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); protected: - explicit MavlinkStreamManualControl() : MavlinkStream(), - manual_sub(nullptr), - manual_time(0) + explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink), + _manual_sub(_mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint))), + _manual_time(0) {} - void subscribe(Mavlink *mavlink) - { - manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); - } - void send(const hrt_abstime t) { struct manual_control_setpoint_s manual; - if (manual_sub->update(&manual_time, &manual)) { - mavlink_msg_manual_control_send(_channel, - mavlink_system.sysid, - manual.x * 1000, - manual.y * 1000, - manual.z * 1000, - manual.r * 1000, - 0); + if (_manual_sub->update(&_manual_time, &manual)) { + mavlink_manual_control_t msg; + + msg.target = mavlink_system.sysid; + msg.x = manual.x * 1000; + msg.y = manual.y * 1000; + msg.z = manual.z * 1000; + msg.r = manual.r * 1000; + msg.buttons = 0; + + _mavlink->send_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg); } } }; - -class MavlinkStreamOpticalFlow : public MavlinkStream +class MavlinkStreamOpticalFlowRad : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamOpticalFlow::get_name_static(); + return MavlinkStreamOpticalFlowRad::get_name_static(); } static const char *get_name_static() { - return "OPTICAL_FLOW"; + return "OPTICAL_FLOW_RAD"; } uint8_t get_id() { - return MAVLINK_MSG_ID_OPTICAL_FLOW; + return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamOpticalFlow(); + return new MavlinkStreamOpticalFlowRad(mavlink); + } + + unsigned get_size() + { + return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *flow_sub; - uint64_t flow_time; + MavlinkOrbSubscription *_flow_sub; + uint64_t _flow_time; /* do not allow top copying this class */ - MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); - MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); + MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &); + MavlinkStreamOpticalFlowRad& operator = (const MavlinkStreamOpticalFlowRad &); protected: - explicit MavlinkStreamOpticalFlow() : MavlinkStream(), - flow_sub(nullptr), - flow_time(0) + explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink), + _flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))), + _flow_time(0) {} - void subscribe(Mavlink *mavlink) - { - flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); - } - void send(const hrt_abstime t) { struct optical_flow_s flow; - if (flow_sub->update(&flow_time, &flow)) { - mavlink_msg_optical_flow_send(_channel, - flow.timestamp, - flow.sensor_id, - flow.flow_raw_x, flow.flow_raw_y, - flow.flow_comp_x_m, flow.flow_comp_y_m, - flow.quality, - flow.ground_distance_m); + if (_flow_sub->update(&_flow_time, &flow)) { + mavlink_optical_flow_rad_t msg; + + msg.time_usec = flow.timestamp; + msg.sensor_id = flow.sensor_id; + msg.integrated_x = flow.pixel_flow_x_integral; + msg.integrated_y = flow.pixel_flow_y_integral; + msg.integrated_xgyro = flow.gyro_x_rate_integral; + msg.integrated_ygyro = flow.gyro_y_rate_integral; + msg.integrated_zgyro = flow.gyro_z_rate_integral; + msg.distance = flow.ground_distance_m; + msg.quality = flow.quality; + msg.integration_time_us = flow.integration_timespan; + msg.sensor_id = flow.sensor_id; + msg.time_delta_distance_us = flow.time_since_last_sonar_update; + msg.temperature = flow.gyro_temperature; + + _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &msg); } } }; @@ -1675,56 +1920,64 @@ public: return 0; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamAttitudeControls(); + return new MavlinkStreamAttitudeControls(mavlink); + } + + unsigned get_size() + { + return 4 * (MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); } private: - MavlinkOrbSubscription *att_ctrl_sub; - uint64_t att_ctrl_time; + MavlinkOrbSubscription *_att_ctrl_sub; + uint64_t _att_ctrl_time; /* do not allow top copying this class */ MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); protected: - explicit MavlinkStreamAttitudeControls() : MavlinkStream(), - att_ctrl_sub(nullptr), - att_ctrl_time(0) + explicit MavlinkStreamAttitudeControls(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_ctrl_sub(_mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS)), + _att_ctrl_time(0) {} - void subscribe(Mavlink *mavlink) - { - att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - } - void send(const hrt_abstime t) { struct actuator_controls_s att_ctrl; - if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) { + if (_att_ctrl_sub->update(&_att_ctrl_time, &att_ctrl)) { /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "rll ctrl ", - att_ctrl.control[0]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "ptch ctrl ", - att_ctrl.control[1]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "yaw ctrl ", - att_ctrl.control[2]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "thr ctrl ", - att_ctrl.control[3]); + mavlink_named_value_float_t msg; + + msg.time_boot_ms = att_ctrl.timestamp / 1000; + + snprintf(msg.name, sizeof(msg.name), "rll ctrl"); + msg.value = att_ctrl.control[0]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "ptch ctrl"); + msg.value = att_ctrl.control[1]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "yaw ctrl"); + msg.value = att_ctrl.control[2]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "thr ctrl"); + msg.value = att_ctrl.control[3]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); } } }; + class MavlinkStreamNamedValueFloat : public MavlinkStream { public: @@ -1743,46 +1996,49 @@ public: return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamNamedValueFloat(); + return new MavlinkStreamNamedValueFloat(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *debug_sub; - uint64_t debug_time; + MavlinkOrbSubscription *_debug_sub; + uint64_t _debug_time; /* do not allow top copying this class */ MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); protected: - explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), - debug_sub(nullptr), - debug_time(0) + explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink), + _debug_sub(_mavlink->add_orb_subscription(ORB_ID(debug_key_value))), + _debug_time(0) {} - void subscribe(Mavlink *mavlink) - { - debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); - } - void send(const hrt_abstime t) { struct debug_key_value_s debug; - if (debug_sub->update(&debug_time, &debug)) { + if (_debug_sub->update(&_debug_time, &debug)) { + mavlink_named_value_float_t msg; + + msg.time_boot_ms = debug.timestamp_ms; + memcpy(msg.name, debug.key, sizeof(msg.name)); /* enforce null termination */ - debug.key[sizeof(debug.key) - 1] = '\0'; + msg.name[sizeof(msg.name) - 1] = '\0'; + msg.value = debug.value; - mavlink_msg_named_value_float_send(_channel, - debug.timestamp_ms, - debug.key, - debug.value); + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); } } }; + class MavlinkStreamCameraCapture : public MavlinkStream { public: @@ -1801,46 +2057,53 @@ public: return 0; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamCameraCapture(); + return new MavlinkStreamCameraCapture(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *_status_sub; /* do not allow top copying this class */ MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); protected: - explicit MavlinkStreamCameraCapture() : MavlinkStream(), - status_sub(nullptr) + explicit MavlinkStreamCameraCapture(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) {} - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - } - void send(const hrt_abstime t) { struct vehicle_status_s status; - (void)status_sub->update(&status); + (void)_status_sub->update(&status); - if (status.arming_state == ARMING_STATE_ARMED - || status.arming_state == ARMING_STATE_ARMED_ERROR) { + mavlink_command_long_t msg; - /* send camera capture on */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + msg.target_system = mavlink_system.sysid; + msg.target_component = MAV_COMP_ID_ALL; + msg.command = MAV_CMD_DO_CONTROL_VIDEO; + msg.confirmation = 0; + msg.param1 = 0; + msg.param2 = 0; + msg.param3 = 0; + /* set camera capture ON/OFF depending on arming state */ + msg.param4 = (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) ? 1 : 0; + msg.param5 = 0; + msg.param6 = 0; + msg.param7 = 0; - } else { - /* send camera capture off */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); - } + _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg); } }; + class MavlinkStreamDistanceSensor : public MavlinkStream { public: @@ -1859,50 +2122,58 @@ public: return MAVLINK_MSG_ID_DISTANCE_SENSOR; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamDistanceSensor(); + return new MavlinkStreamDistanceSensor(mavlink); + } + + unsigned get_size() + { + return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *range_sub; - uint64_t range_time; + MavlinkOrbSubscription *_range_sub; + uint64_t _range_time; /* do not allow top copying this class */ MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); protected: - explicit MavlinkStreamDistanceSensor() : MavlinkStream(), - range_sub(nullptr), - range_time(0) + explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink), + _range_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_range_finder))), + _range_time(0) {} - void subscribe(Mavlink *mavlink) - { - range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); - } - void send(const hrt_abstime t) { struct range_finder_report range; - if (range_sub->update(&range_time, &range)) { + if (_range_sub->update(&_range_time, &range)) { + + mavlink_distance_sensor_t msg; - uint8_t type; + msg.time_boot_ms = range.timestamp / 1000; switch (range.type) { - case RANGE_FINDER_TYPE_LASER: - type = MAV_DISTANCE_SENSOR_LASER; + case RANGE_FINDER_TYPE_LASER: + msg.type = MAV_DISTANCE_SENSOR_LASER; + break; + + default: + msg.type = MAV_DISTANCE_SENSOR_LASER; break; } - uint8_t id = 0; - uint8_t orientation = 0; - uint8_t covariance = 20; + msg.id = 0; + msg.orientation = 0; + msg.min_distance = range.minimum_distance * 100; + msg.max_distance = range.minimum_distance * 100; + msg.current_distance = range.distance * 100; + msg.covariance = 20; - mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation, - range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance); + _mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg); } } }; @@ -1910,6 +2181,8 @@ protected: StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), + new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static), + new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static), new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), @@ -1918,23 +2191,22 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), + new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static), new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), - new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), + new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), + new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static), new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), - new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), + new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static), new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), - new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), nullptr }; diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index ee64d0e42..7e4416609 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -46,10 +46,10 @@ class StreamListItem { public: - MavlinkStream* (*new_instance)(); + MavlinkStream* (*new_instance)(Mavlink *mavlink); const char* (*get_name)(); - StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) : + StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)()) : new_instance(inst), get_name(name) {}; diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 068774c47..a3c127cdc 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -58,9 +58,12 @@ #endif static const int ERROR = -1; +#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \ + ((_msg.target_component == mavlink_system.compid) || \ + (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \ + (_msg.target_component == MAV_COMP_ID_ALL))) -MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : - _mavlink(mavlink), +MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink), _state(MAVLINK_WPM_STATE_IDLE), _time_last_recv(0), _time_last_sent(0), @@ -79,10 +82,8 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _offboard_mission_sub(-1), _mission_result_sub(-1), _offboard_mission_pub(-1), - _slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()), - _verbose(false), - _channel(mavlink->get_channel()), - _comp_id(MAV_COMP_ID_MISSIONPLANNER) + _slow_rate_limiter(_interval / 10.0f), + _verbose(false) { _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); @@ -96,6 +97,20 @@ MavlinkMissionManager::~MavlinkMissionManager() close(_mission_result_sub); } +unsigned +MavlinkMissionManager::get_size() +{ + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + return MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST) { + return MAVLINK_MSG_ID_MISSION_REQUEST + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else { + return 0; + } +} + void MavlinkMissionManager::init_offboard_mission() { @@ -157,15 +172,13 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int void MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type) { - mavlink_message_t msg; mavlink_mission_ack_t wpa; wpa.target_system = sysid; wpa.target_component = compid; wpa.type = type; - mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ACK, &wpa); if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } } @@ -175,13 +188,11 @@ void MavlinkMissionManager::send_mission_current(uint16_t seq) { if (seq < _count) { - mavlink_message_t msg; mavlink_mission_current_t wpc; wpc.seq = seq; - mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_CURRENT, &wpc); } else if (seq == 0 && _count == 0) { /* don't broadcast if no WPs */ @@ -199,15 +210,13 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_ { _time_last_sent = hrt_absolute_time(); - mavlink_message_t msg; mavlink_mission_count_t wpc; wpc.target_system = sysid; wpc.target_component = compid; wpc.count = _count; - mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_COUNT, &wpc); if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); } } @@ -226,13 +235,12 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t mavlink_mission_item_t wp; format_mavlink_mission_item(&mission_item, &wp); - mavlink_message_t msg; wp.target_system = sysid; wp.target_component = compid; wp.seq = seq; wp.current = (_current_seq == seq) ? 1 : 0; - mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp); - _mavlink->send_message(&msg); + + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM, &wp); if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); } @@ -251,13 +259,12 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 if (seq < _max_count) { _time_last_sent = hrt_absolute_time(); - mavlink_message_t msg; mavlink_mission_request_t wpr; wpr.target_system = sysid; wpr.target_component = compid; wpr.seq = seq; - mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr); - _mavlink->send_message(&msg); + + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_REQUEST, &wpr); if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } @@ -272,22 +279,21 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 void MavlinkMissionManager::send_mission_item_reached(uint16_t seq) { - mavlink_message_t msg; mavlink_mission_item_reached_t wp_reached; wp_reached.seq = seq; - mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &wp_reached); if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); } } void -MavlinkMissionManager::eventloop() +MavlinkMissionManager::send(const hrt_abstime now) { - hrt_abstime now = hrt_absolute_time(); + /* update interval for slow rate limiter */ + _slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult()); bool updated = false; orb_check(_mission_result_sub, &updated); @@ -381,7 +387,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(msg, &wpa); - if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID_MISSION(wpa)) { if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -413,7 +419,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) mavlink_mission_set_current_t wpc; mavlink_msg_mission_set_current_decode(msg, &wpc); - if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID_MISSION(wpc)) { if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); @@ -448,7 +454,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) mavlink_mission_request_list_t wprl; mavlink_msg_mission_request_list_decode(msg, &wprl); - if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID_MISSION(wprl)) { if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -484,7 +490,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(msg, &wpr); - if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID_MISSION(wpr)) { if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -555,7 +561,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) mavlink_mission_count_t wpc; mavlink_msg_mission_count_decode(msg, &wpc); - if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID_MISSION(wpc)) { if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); @@ -621,7 +627,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); - if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) { + if (CHECK_SYSID_COMPID_MISSION(wp)) { if (_state == MAVLINK_WPM_STATE_GETLIST) { _time_last_recv = hrt_absolute_time(); @@ -707,7 +713,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) mavlink_mission_clear_all_t wpca; mavlink_msg_mission_clear_all_decode(msg, &wpca); - if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { + if (CHECK_SYSID_COMPID_MISSION(wpca)) { if (_state == MAVLINK_WPM_STATE_IDLE) { /* don't touch mission items storage itself, but only items count in mission state */ @@ -792,10 +798,10 @@ int MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) { if (mission_item->altitude_is_relative) { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL; + mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; } else { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + mavlink_mission_item->frame = MAV_FRAME_GLOBAL; } switch (mission_item->nav_cmd) { diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index b792b9aaf..a7bb94c22 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -42,11 +42,12 @@ #pragma once +#include <uORB/uORB.h> + #include "mavlink_bridge_header.h" #include "mavlink_rate_limiter.h" -#include <uORB/uORB.h> +#include "mavlink_stream.h" -// FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { MAVLINK_WPM_STATE_IDLE = 0, MAVLINK_WPM_STATE_SENDLIST, @@ -66,20 +67,73 @@ enum MAVLINK_WPM_CODES { #define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds #define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds -class Mavlink; - -class MavlinkMissionManager { +class MavlinkMissionManager : public MavlinkStream { public: - MavlinkMissionManager(Mavlink *parent); - ~MavlinkMissionManager(); + const char *get_name() const + { + return MavlinkMissionManager::get_name_static(); + } + + static const char *get_name_static() + { + return "MISSION_ITEM"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_MISSION_ITEM; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkMissionManager(mavlink); + } + + unsigned get_size(); + + void handle_message(const mavlink_message_t *msg); + + void set_verbose(bool v) { _verbose = v; } + +private: + enum MAVLINK_WPM_STATES _state; ///< Current state + + uint64_t _time_last_recv; + uint64_t _time_last_sent; + + uint32_t _action_timeout; + uint32_t _retry_timeout; + unsigned _max_count; ///< Maximum number of mission items + + int _dataman_id; ///< Dataman storage ID for active mission + unsigned _count; ///< Count of items in active mission + int _current_seq; ///< Current item sequence in active mission + + int _transfer_dataman_id; ///< Dataman storage ID for current transmission + unsigned _transfer_count; ///< Items count in current transmission + unsigned _transfer_seq; ///< Item sequence in current transmission + unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) + unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission + unsigned _transfer_partner_compid; ///< Partner component ID for current transmission + + int _offboard_mission_sub; + int _mission_result_sub; + orb_advert_t _offboard_mission_pub; + + MavlinkRateLimiter _slow_rate_limiter; + + bool _verbose; + + /* do not allow top copying this class */ + MavlinkMissionManager(MavlinkMissionManager &); + MavlinkMissionManager& operator = (const MavlinkMissionManager &); + void init_offboard_mission(); int update_active_mission(int dataman_id, unsigned count, int seq); - void send_message(mavlink_message_t *msg); - /** * @brief Sends an waypoint ack message */ @@ -111,10 +165,6 @@ public: */ void send_mission_item_reached(uint16_t seq); - void eventloop(); - - void handle_message(const mavlink_message_t *msg); - void handle_mission_ack(const mavlink_message_t *msg); void handle_mission_set_current(const mavlink_message_t *msg); @@ -139,43 +189,8 @@ public: */ int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - void set_verbose(bool v) { _verbose = v; } - -private: - Mavlink* _mavlink; - - enum MAVLINK_WPM_STATES _state; ///< Current state - - uint64_t _time_last_recv; - uint64_t _time_last_sent; - - uint32_t _action_timeout; - uint32_t _retry_timeout; - unsigned _max_count; ///< Maximum number of mission items - - int _dataman_id; ///< Dataman storage ID for active mission - unsigned _count; ///< Count of items in active mission - int _current_seq; ///< Current item sequence in active mission - - int _transfer_dataman_id; ///< Dataman storage ID for current transmission - unsigned _transfer_count; ///< Items count in current transmission - unsigned _transfer_seq; ///< Item sequence in current transmission - unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) - unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission - unsigned _transfer_partner_compid; ///< Partner component ID for current transmission - - int _offboard_mission_sub; - int _mission_result_sub; - orb_advert_t _offboard_mission_pub; - - MavlinkRateLimiter _slow_rate_limiter; - - bool _verbose; - - mavlink_channel_t _channel; - uint8_t _comp_id; +protected: + explicit MavlinkMissionManager(Mavlink *mavlink); - /* do not allow copying this class */ - MavlinkMissionManager(const MavlinkMissionManager&); - MavlinkMissionManager operator=(const MavlinkMissionManager&); + void send(const hrt_abstime t); }; diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp new file mode 100644 index 000000000..cd5f53d88 --- /dev/null +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -0,0 +1,197 @@ +/**************************************************************************** + * + * 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_parameters.cpp + * Mavlink parameters manager implementation. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <stdio.h> + +#include "mavlink_parameters.h" +#include "mavlink_main.h" + +MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), + _send_all_index(-1) +{ +} + +unsigned +MavlinkParametersManager::get_size() +{ + if (_send_all_index >= 0) { + return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else { + return 0; + } +} + +void +MavlinkParametersManager::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + /* request all parameters */ + mavlink_param_request_list_t req_list; + mavlink_msg_param_request_list_decode(msg, &req_list); + + if (req_list.target_system == mavlink_system.sysid && + (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { + + _send_all_index = 0; + _mavlink->send_statustext_info("[pm] sending list"); + } + break; + } + + case MAVLINK_MSG_ID_PARAM_SET: { + /* set parameter */ + if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { + mavlink_param_set_t set; + mavlink_msg_param_set_decode(msg, &set); + + if (set.target_system == mavlink_system.sysid && + (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { + + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter, set and send it */ + param_t param = param_find(name); + + if (param == PARAM_INVALID) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] unknown param: %s", name); + _mavlink->send_statustext_info(buf); + + } else { + /* set and send parameter */ + param_set(param, &(set.param_value)); + send_param(param); + } + } + } + break; + } + + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { + /* request one parameter */ + mavlink_param_request_read_t req_read; + mavlink_msg_param_request_read_decode(msg, &req_read); + + if (req_read.target_system == mavlink_system.sysid && + (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { + + /* when no index is given, loop through string ids and compare them */ + if (req_read.param_index < 0) { + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter and send it */ + send_param(param_find(name)); + + } else { + /* when index is >= 0, send this parameter again */ + send_param(param_for_index(req_read.param_index)); + } + } + break; + } + + default: + break; + } +} + +void +MavlinkParametersManager::send(const hrt_abstime t) +{ + /* send all parameters if requested */ + if (_send_all_index >= 0) { + send_param(param_for_index(_send_all_index)); + _send_all_index++; + if (_send_all_index >= (int) param_count()) { + _send_all_index = -1; + } + } +} + +void +MavlinkParametersManager::send_param(param_t param) +{ + if (param == PARAM_INVALID) { + return; + } + + mavlink_param_value_t msg; + + /* + * get param value, since MAVLink encodes float and int params in the same + * space during transmission, copy param onto float val_buf + */ + if (param_get(param, &msg.param_value) != OK) { + return; + } + + msg.param_count = param_count(); + msg.param_index = param_get_index(param); + + /* copy parameter name */ + strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + + /* query parameter type */ + param_type_t type = param_type(param); + + /* + * Map onboard parameter type to MAVLink type, + * endianess matches (both little endian) + */ + if (type == PARAM_TYPE_INT32) { + msg.param_type = MAVLINK_TYPE_INT32_T; + + } else if (type == PARAM_TYPE_FLOAT) { + msg.param_type = MAVLINK_TYPE_FLOAT; + + } else { + msg.param_type = MAVLINK_TYPE_FLOAT; + } + + _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg); +} diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_parameters.h index b502c3c86..5576e6b84 100644 --- a/src/modules/mavlink/mavlink_commands.cpp +++ b/src/modules/mavlink/mavlink_parameters.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * 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 @@ -32,42 +32,84 @@ ****************************************************************************/ /** - * @file mavlink_commands.cpp - * Mavlink commands stream implementation. + * @file mavlink_parameters.h + * Mavlink parameters manager definition. * * @author Anton Babushkin <anton.babushkin@me.com> */ -#include "mavlink_commands.h" +#pragma once -MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : - _cmd_sub(mavlink->add_orb_subscription(ORB_ID(vehicle_command))), - _cmd{}, - _channel(channel), - _cmd_time(0) -{ -} +#include <systemlib/param/param.h> + +#include "mavlink_bridge_header.h" +#include "mavlink_stream.h" -void -MavlinkCommandsStream::update(const hrt_abstime t) +class MavlinkParametersManager : public MavlinkStream { - struct vehicle_command_s cmd; - - if (_cmd_sub->update(&_cmd_time, &cmd)) { - /* only send commands for other systems/components */ - if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { - mavlink_msg_command_long_send(_channel, - cmd.target_system, - cmd.target_component, - cmd.command, - cmd.confirmation, - cmd.param1, - cmd.param2, - cmd.param3, - cmd.param4, - cmd.param5, - cmd.param6, - cmd.param7); - } +public: + const char *get_name() const + { + return MavlinkParametersManager::get_name_static(); + } + + static const char *get_name_static() + { + return "PARAM_VALUE"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_PARAM_VALUE; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkParametersManager(mavlink); } -} + + unsigned get_size(); + + void handle_message(const mavlink_message_t *msg); + + /** + * Send one parameter identified by index. + * + * @param index The index of the parameter to send. + * @return zero on success, nonzero else. + */ + void start_send_one(int index); + + + /** + * Send one parameter identified by name. + * + * @param name The index of the parameter to send. + * @return zero on success, nonzero else. + */ + int start_send_for_name(const char *name); + + /** + * Start sending the parameter queue. + * + * This function will not directly send parameters, but instead + * activate the sending of one parameter on each call of + * mavlink_pm_queued_send(). + * @see mavlink_pm_queued_send() + */ + void start_send_all(); + +private: + int _send_all_index; + + /* do not allow top copying this class */ + MavlinkParametersManager(MavlinkParametersManager &); + MavlinkParametersManager& operator = (const MavlinkParametersManager &); + +protected: + explicit MavlinkParametersManager(Mavlink *mavlink); + + void send(const hrt_abstime t); + + void send_param(param_t param); +}; diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp index 9cdda8b17..d3b3e1cde 100644 --- a/src/modules/mavlink/mavlink_rate_limiter.cpp +++ b/src/modules/mavlink/mavlink_rate_limiter.cpp @@ -64,7 +64,7 @@ MavlinkRateLimiter::check(hrt_abstime t) uint64_t dt = t - _last_sent; if (dt > 0 && dt >= _interval) { - _last_sent = (t / _interval) * _interval; + _last_sent = t; return true; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 54c412ce7..e98d72afe 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -37,6 +37,7 @@ * * @author Lorenz Meier <lm@inf.ethz.ch> * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> */ /* XXX trim includes */ @@ -54,6 +55,7 @@ #include <drivers/drv_gyro.h> #include <drivers/drv_mag.h> #include <drivers/drv_baro.h> +#include <drivers/drv_range_finder.h> #include <time.h> #include <float.h> #include <unistd.h> @@ -102,16 +104,18 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _battery_pub(-1), _cmd_pub(-1), _flow_pub(-1), + _range_pub(-1), _offboard_control_sp_pub(-1), - _local_pos_sp_pub(-1), _global_vel_sp_pub(-1), _att_sp_pub(-1), _rates_sp_pub(-1), + _force_sp_pub(-1), + _pos_sp_triplet_pub(-1), _vicon_position_pub(-1), + _vision_position_pub(-1), _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), - _radio_status_available(false), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -121,7 +125,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : { // make sure the FTP server is started - (void)MavlinkFTP::getServer(); + (void)MavlinkFTP::get_server(); } MavlinkReceiver::~MavlinkReceiver() @@ -136,8 +140,12 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_command_long(msg); break; - case MAVLINK_MSG_ID_OPTICAL_FLOW: - handle_message_optical_flow(msg); + case MAVLINK_MSG_ID_COMMAND_INT: + handle_message_command_int(msg); + break; + + case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD: + handle_message_optical_flow_rad(msg); break; case MAVLINK_MSG_ID_SET_MODE: @@ -148,8 +156,16 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_vicon_position_estimate(msg); break; - case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST: - handle_message_quad_swarm_roll_pitch_yaw_thrust(msg); + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + handle_message_set_position_target_local_ned(msg); + break; + + case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: + handle_message_set_attitude_target(msg); + break; + + case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: + handle_message_vision_position_estimate(msg); break; case MAVLINK_MSG_ID_RADIO_STATUS: @@ -168,8 +184,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_request_data_stream(msg); break; - case MAVLINK_MSG_ID_ENCAPSULATED_DATA: - MavlinkFTP::getServer()->handle_message(_mavlink, msg); + case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: + MavlinkFTP::get_server()->handle_message(_mavlink, msg); break; default: @@ -196,6 +212,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_hil_state_quaternion(msg); break; + case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW: + handle_message_hil_optical_flow(msg); + break; + default: break; } @@ -241,7 +261,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { - warnx("ignoring CMD spoofed with same SYS/COMP ID"); + warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + mavlink_system.sysid, mavlink_system.compid); return; } @@ -275,24 +296,83 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } void -MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) +MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) +{ + /* command */ + mavlink_command_int_t cmd_mavlink; + mavlink_msg_command_int_decode(msg, &cmd_mavlink); + + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + //check for MAVLINK terminate command + if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { + /* This is the link shutdown command, terminate mavlink */ + warnx("terminated by remote command"); + fflush(stdout); + usleep(50000); + + /* terminate other threads and this thread */ + _mavlink->_task_should_exit = true; + + } else { + + if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { + warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + mavlink_system.sysid, mavlink_system.compid); + return; + } + + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); + + /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + /* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */ + vcmd.param5 = ((double)cmd_mavlink.x) / 1e7; + vcmd.param6 = ((double)cmd_mavlink.y) / 1e7; + vcmd.param7 = cmd_mavlink.z; + // XXX do proper translation + vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + + if (_cmd_pub < 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + + } else { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); + } + } + } +} + +void +MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) { /* optical flow */ - mavlink_optical_flow_t flow; - mavlink_msg_optical_flow_decode(msg, &flow); + mavlink_optical_flow_rad_t flow; + mavlink_msg_optical_flow_rad_decode(msg, &flow); struct optical_flow_s f; memset(&f, 0, sizeof(f)); - f.timestamp = hrt_absolute_time(); - f.flow_timestamp = flow.time_usec; - f.flow_raw_x = flow.flow_x; - f.flow_raw_y = flow.flow_y; - f.flow_comp_x_m = flow.flow_comp_m_x; - f.flow_comp_y_m = flow.flow_comp_m_y; - f.ground_distance_m = flow.ground_distance; + f.timestamp = flow.time_usec; + f.integration_timespan = flow.integration_time_us; + f.pixel_flow_x_integral = flow.integrated_x; + f.pixel_flow_y_integral = flow.integrated_y; + f.gyro_x_rate_integral = flow.integrated_xgyro; + f.gyro_y_rate_integral = flow.integrated_ygyro; + f.gyro_z_rate_integral = flow.integrated_zgyro; + f.time_since_last_sonar_update = flow.time_delta_distance_us; + f.ground_distance_m = flow.distance; f.quality = flow.quality; f.sensor_id = flow.sensor_id; + f.gyro_temperature = flow.temperature; if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); @@ -303,6 +383,55 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) } void +MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) +{ + /* optical flow */ + mavlink_hil_optical_flow_t flow; + mavlink_msg_hil_optical_flow_decode(msg, &flow); + + struct optical_flow_s f; + memset(&f, 0, sizeof(f)); + + f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec; + f.integration_timespan = flow.integration_time_us; + f.pixel_flow_x_integral = flow.integrated_x; + f.pixel_flow_y_integral = flow.integrated_y; + f.gyro_x_rate_integral = flow.integrated_xgyro; + f.gyro_y_rate_integral = flow.integrated_ygyro; + f.gyro_z_rate_integral = flow.integrated_zgyro; + f.time_since_last_sonar_update = flow.time_delta_distance_us; + f.ground_distance_m = flow.distance; + f.quality = flow.quality; + f.sensor_id = flow.sensor_id; + f.gyro_temperature = flow.temperature; + + if (_flow_pub < 0) { + _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + + } else { + orb_publish(ORB_ID(optical_flow), _flow_pub, &f); + } + + /* Use distance value for range finder report */ + struct range_finder_report r; + memset(&r, 0, sizeof(f)); + + r.timestamp = hrt_absolute_time(); + r.error_count = 0; + r.type = RANGE_FINDER_TYPE_LASER; + r.distance = flow.distance; + r.minimum_distance = 0.0f; + r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable + r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); + + if (_range_pub < 0) { + _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); + } else { + orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); + } +} + +void MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) { mavlink_set_mode_t new_mode; @@ -362,23 +491,71 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) } void -MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg) +MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg) { - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control); + mavlink_set_position_target_local_ned_t set_position_target_local_ned; + mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned); + + struct offboard_control_setpoint_s offboard_control_sp; + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints + + /* Only accept messages which are intended for this system */ + if ((mavlink_system.sysid == set_position_target_local_ned.target_system || + set_position_target_local_ned.target_system == 0) && + (mavlink_system.compid == set_position_target_local_ned.target_component || + set_position_target_local_ned.target_component == 0)) { + + /* convert mavlink type (local, NED) to uORB offboard control struct */ + switch (set_position_target_local_ned.coordinate_frame) { + case MAV_FRAME_LOCAL_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED; + break; + case MAV_FRAME_LOCAL_OFFSET_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED; + break; + case MAV_FRAME_BODY_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED; + break; + case MAV_FRAME_BODY_OFFSET_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED; + break; + default: + /* invalid setpoint, avoid publishing */ + return; + } + offboard_control_sp.position[0] = set_position_target_local_ned.x; + offboard_control_sp.position[1] = set_position_target_local_ned.y; + offboard_control_sp.position[2] = set_position_target_local_ned.z; + offboard_control_sp.velocity[0] = set_position_target_local_ned.vx; + offboard_control_sp.velocity[1] = set_position_target_local_ned.vy; + offboard_control_sp.velocity[2] = set_position_target_local_ned.vz; + offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx; + offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy; + offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz; + offboard_control_sp.yaw = set_position_target_local_ned.yaw; + offboard_control_sp.yaw_rate = set_position_target_local_ned.yaw_rate; + offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); + + /* If we are in force control mode, for now set offboard mode to force control */ + if (offboard_control_sp.isForceSetpoint) { + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE; + } - /* Only accept system IDs from 1 to 4 */ - if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) { - struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); + /* set ignore flags */ + for (int i = 0; i < 9; i++) { + offboard_control_sp.ignore &= ~(1 << i); + offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); + } - /* Convert values * 1000 back */ - offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f; + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW); + if (set_position_target_local_ned.type_mask & (1 << 10)) { + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAW); + } - offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode; + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); + if (set_position_target_local_ned.type_mask & (1 << 11)) { + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE); + } offboard_control_sp.timestamp = hrt_absolute_time(); @@ -388,6 +565,251 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message } else { orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); } + + /* If we are in offboard control mode and offboard control loop through is enabled + * also publish the setpoint topic which is read by the controller */ + if (_mavlink->get_forward_externalsp()) { + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } + if (_control_mode.flag_control_offboard_enabled) { + if (offboard_control_sp.isForceSetpoint && + offboard_control_sp_ignore_position_all(offboard_control_sp) && + offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { + /* The offboard setpoint is a force setpoint only, directly writing to the force + * setpoint topic and not publishing the setpoint triplet topic */ + struct vehicle_force_setpoint_s force_sp; + force_sp.x = offboard_control_sp.acceleration[0]; + force_sp.y = offboard_control_sp.acceleration[1]; + force_sp.z = offboard_control_sp.acceleration[2]; + //XXX: yaw + if (_force_sp_pub < 0) { + _force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp); + } else { + orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); + } + } else { + /* It's not a pure force setpoint: publish to setpoint triplet topic */ + struct position_setpoint_triplet_s pos_sp_triplet; + pos_sp_triplet.previous.valid = false; + pos_sp_triplet.next.valid = false; + pos_sp_triplet.current.valid = true; + pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + + /* set the local pos values if the setpoint type is 'local pos' and none + * of the local pos fields is set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_position_some(offboard_control_sp)) { + pos_sp_triplet.current.position_valid = true; + pos_sp_triplet.current.x = offboard_control_sp.position[0]; + pos_sp_triplet.current.y = offboard_control_sp.position[1]; + pos_sp_triplet.current.z = offboard_control_sp.position[2]; + } else { + pos_sp_triplet.current.position_valid = false; + } + + /* set the local vel values if the setpoint type is 'local pos' and none + * of the local vel fields is set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_velocity_some(offboard_control_sp)) { + pos_sp_triplet.current.velocity_valid = true; + pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; + pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; + pos_sp_triplet.current.vz = offboard_control_sp.velocity[2]; + } else { + pos_sp_triplet.current.velocity_valid = false; + } + + /* set the local acceleration values if the setpoint type is 'local pos' and none + * of the accelerations fields is set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) { + pos_sp_triplet.current.acceleration_valid = true; + pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0]; + pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1]; + pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2]; + pos_sp_triplet.current.acceleration_is_force = + offboard_control_sp.isForceSetpoint; + + } else { + pos_sp_triplet.current.acceleration_valid = false; + } + + /* set the yaw sp value if the setpoint type is 'local pos' and the yaw + * field is not set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_yaw(offboard_control_sp)) { + pos_sp_triplet.current.yaw_valid = true; + pos_sp_triplet.current.yaw = offboard_control_sp.yaw; + + } else { + pos_sp_triplet.current.yaw_valid = false; + } + + /* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate + * field is not set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_yawrate(offboard_control_sp)) { + pos_sp_triplet.current.yawspeed_valid = true; + pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate; + + } else { + pos_sp_triplet.current.yawspeed_valid = false; + } + //XXX handle global pos setpoints (different MAV frames) + + + if (_pos_sp_triplet_pub < 0) { + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), + &pos_sp_triplet); + } else { + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, + &pos_sp_triplet); + } + + } + + } + + } + } +} + +void +MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) +{ + mavlink_vision_position_estimate_t pos; + mavlink_msg_vision_position_estimate_decode(msg, &pos); + + struct vision_position_estimate vision_position; + memset(&vision_position, 0, sizeof(vision_position)); + + // Use the component ID to identify the vision sensor + vision_position.id = msg->compid; + + vision_position.timestamp_boot = hrt_absolute_time(); + vision_position.timestamp_computer = pos.usec; + vision_position.x = pos.x; + vision_position.y = pos.y; + vision_position.z = pos.z; + + // XXX fix this + vision_position.vx = 0.0f; + vision_position.vy = 0.0f; + vision_position.vz = 0.0f; + + math::Quaternion q; + q.from_euler(pos.roll, pos.pitch, pos.yaw); + + vision_position.q[0] = q(0); + vision_position.q[1] = q(1); + vision_position.q[2] = q(2); + vision_position.q[3] = q(3); + + if (_vision_position_pub < 0) { + _vision_position_pub = orb_advertise(ORB_ID(vision_position_estimate), &vision_position); + + } else { + orb_publish(ORB_ID(vision_position_estimate), _vision_position_pub, &vision_position); + } +} + +void +MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) +{ + mavlink_set_attitude_target_t set_attitude_target; + mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target); + + struct offboard_control_setpoint_s offboard_control_sp; + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints + + /* Only accept messages which are intended for this system */ + if ((mavlink_system.sysid == set_attitude_target.target_system || + set_attitude_target.target_system == 0) && + (mavlink_system.compid == set_attitude_target.target_component || + set_attitude_target.target_component == 0)) { + for (int i = 0; i < 4; i++) { + offboard_control_sp.attitude[i] = set_attitude_target.q[i]; + } + offboard_control_sp.attitude_rate[0] = set_attitude_target.body_roll_rate; + offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate; + offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate; + + /* set correct ignore flags for body rate fields: copy from mavlink message */ + for (int i = 0; i < 3; i++) { + offboard_control_sp.ignore &= ~(1 << (i + OFB_IGN_BIT_BODYRATE_X)); + offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X; + } + /* set correct ignore flags for thrust field: copy from mavlink message */ + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST); + offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST); + /* set correct ignore flags for attitude field: copy from mavlink message */ + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_ATT); + offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << OFB_IGN_BIT_ATT); + + + offboard_control_sp.timestamp = hrt_absolute_time(); + offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode + + if (_offboard_control_sp_pub < 0) { + _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + + } else { + orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + } + + /* If we are in offboard control mode and offboard control loop through is enabled + * also publish the setpoint topic which is read by the controller */ + if (_mavlink->get_forward_externalsp()) { + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } + + if (_control_mode.flag_control_offboard_enabled) { + + /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ + if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) || + offboard_control_sp_ignore_thrust(offboard_control_sp))) { + struct vehicle_attitude_setpoint_s att_sp; + att_sp.timestamp = hrt_absolute_time(); + mavlink_quaternion_to_euler(set_attitude_target.q, + &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body); + att_sp.R_valid = true; + att_sp.thrust = set_attitude_target.thrust; + memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); + att_sp.q_d_valid = true; + if (_att_sp_pub < 0) { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + } else { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); + } + } + + /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ + ///XXX add support for ignoring individual axes + if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) || + offboard_control_sp_ignore_thrust(offboard_control_sp))) { + struct vehicle_rates_setpoint_s rates_sp; + rates_sp.timestamp = hrt_absolute_time(); + rates_sp.roll = set_attitude_target.body_roll_rate; + rates_sp.pitch = set_attitude_target.body_pitch_rate; + rates_sp.yaw = set_attitude_target.body_yaw_rate; + rates_sp.thrust = set_attitude_target.thrust; + + if (_att_sp_pub < 0) { + _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } else { + orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); + } + } + } + + } } } @@ -412,6 +834,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) tstatus.remote_noise = rstatus.remnoise; tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; + tstatus.system_id = msg->sysid; + tstatus.component_id = msg->compid; if (_telemetry_status_pub < 0) { _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); @@ -419,9 +843,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) } else { orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); } - - /* this means that heartbeats alone won't be published to the radio status no more */ - _radio_status_available = true; } } @@ -463,25 +884,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); - hrt_abstime tnow = hrt_absolute_time(); - - /* always set heartbeat, publish only if telemetry link not up */ - tstatus.heartbeat_time = tnow; - - /* if no radio status messages arrive, lets at least publish that heartbeats were received */ - if (!_radio_status_available) { + /* set heartbeat time and topic time and publish - + * the telem status also gets updated on telemetry events + */ + tstatus.timestamp = hrt_absolute_time(); + tstatus.heartbeat_time = tstatus.timestamp; - tstatus.timestamp = tnow; - /* telem_time indicates the timestamp of a telemetry status packet and we got none */ - tstatus.telem_time = 0; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - - } else { - orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); - } + } else { + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); } } } @@ -995,7 +1408,7 @@ MavlinkReceiver::receive_start(Mavlink *parent) struct sched_param param; (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); - param.sched_priority = SCHED_PRIORITY_MAX - 40; + param.sched_priority = SCHED_PRIORITY_MAX - 80; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); pthread_attr_setstacksize(&receiveloop_attr, 2900); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index f4d0c52d8..a057074a7 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -58,6 +58,7 @@ #include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_vicon_position.h> +#include <uORB/topics/vision_position_estimate.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/optical_flow.h> @@ -70,6 +71,7 @@ #include <uORB/topics/debug_key_value.h> #include <uORB/topics/airspeed.h> #include <uORB/topics/battery_status.h> +#include <uORB/topics/vehicle_force_setpoint.h> #include "mavlink_ftp.h" @@ -109,10 +111,15 @@ private: void handle_message(mavlink_message_t *msg); void handle_message_command_long(mavlink_message_t *msg); - void handle_message_optical_flow(mavlink_message_t *msg); + void handle_message_command_int(mavlink_message_t *msg); + void handle_message_optical_flow_rad(mavlink_message_t *msg); + void handle_message_hil_optical_flow(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); + void handle_message_vision_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); + void handle_message_set_position_target_local_ned(mavlink_message_t *msg); + void handle_message_set_attitude_target(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); @@ -139,16 +146,18 @@ private: orb_advert_t _battery_pub; orb_advert_t _cmd_pub; orb_advert_t _flow_pub; + orb_advert_t _range_pub; orb_advert_t _offboard_control_sp_pub; - orb_advert_t _local_pos_sp_pub; orb_advert_t _global_vel_sp_pub; orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; + orb_advert_t _force_sp_pub; + orb_advert_t _pos_sp_triplet_pub; orb_advert_t _vicon_position_pub; + orb_advert_t _vision_position_pub; orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; - bool _radio_status_available; int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 7279206db..5b9e45d3e 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -43,9 +43,9 @@ #include "mavlink_stream.h" #include "mavlink_main.h" -MavlinkStream::MavlinkStream() : +MavlinkStream::MavlinkStream(Mavlink *mavlink) : next(nullptr), - _channel(MAVLINK_COMM_0), + _mavlink(mavlink), _interval(1000000), _last_sent(0) { @@ -65,26 +65,27 @@ MavlinkStream::set_interval(const unsigned int interval) } /** - * Set mavlink channel - */ -void -MavlinkStream::set_channel(mavlink_channel_t channel) -{ - _channel = channel; -} - -/** * Update subscriptions and send message if necessary */ int MavlinkStream::update(const hrt_abstime t) { uint64_t dt = t - _last_sent; + unsigned int interval = _interval; + + if (!const_rate()) { + interval /= _mavlink->get_rate_mult(); + } - if (dt > 0 && dt >= _interval) { + if (dt > 0 && dt >= interval) { /* interval expired, send message */ send(t); - _last_sent = (t / _interval) * _interval; + if (const_rate()) { + _last_sent = (t / _interval) * _interval; + + } else { + _last_sent = t; + } return 0; } diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 20e1c7c44..ef22dc443 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -46,31 +46,50 @@ class Mavlink; class MavlinkStream; -#include "mavlink_main.h" - class MavlinkStream { public: MavlinkStream *next; - MavlinkStream(); + MavlinkStream(Mavlink *mavlink); virtual ~MavlinkStream(); + + /** + * Get the interval + * + * @param interval the inveral in microseconds (us) between messages + */ void set_interval(const unsigned int interval); - void set_channel(mavlink_channel_t channel); + + /** + * Get the interval + * + * @return the inveral in microseconds (us) between messages + */ + unsigned get_interval() { return _interval; } /** * @return 0 if updated / sent, -1 if unchanged */ int update(const hrt_abstime t); - static MavlinkStream *new_instance(); + static MavlinkStream *new_instance(const Mavlink *mavlink); static const char *get_name_static(); - virtual void subscribe(Mavlink *mavlink) = 0; virtual const char *get_name() const = 0; virtual uint8_t get_id() = 0; + /** + * @return true if steam rate shouldn't be adjusted + */ + virtual bool const_rate() { return false; } + + /** + * Get maximal total messages size on update + */ + virtual unsigned get_size() = 0; + protected: - mavlink_channel_t _channel; + Mavlink * _mavlink; unsigned int _interval; virtual void send(const hrt_abstime t) = 0; diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp new file mode 100644 index 000000000..4caa820b6 --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -0,0 +1,761 @@ +/**************************************************************************** + * + * 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_test.cpp +/// @author Don Gagne <don@thegagnes.com> + +#include <sys/stat.h> +#include <crc32.h> +#include <stdio.h> +#include <fcntl.h> + +#include "mavlink_ftp_test.h" +#include "../mavlink_ftp.h" + +/// @brief Test case file name for Read command. File are generated using mavlink_ftp_test_data.py +const MavlinkFtpTest::ReadTestCase MavlinkFtpTest::_rgReadTestCases[] = { + { "/etc/unit_test_data/mavlink_tests/test_238.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1}, // Read takes less than single packet + { "/etc/unit_test_data/mavlink_tests/test_239.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) }, // Read completely fills single packet + { "/etc/unit_test_data/mavlink_tests/test_240.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1 }, // Read take two packets +}; + +const char MavlinkFtpTest::_unittest_microsd_dir[] = "/fs/microsd/ftp_unit_test_dir"; +const char MavlinkFtpTest::_unittest_microsd_file[] = "/fs/microsd/ftp_unit_test_dir/file"; + +MavlinkFtpTest::MavlinkFtpTest() : + _ftp_server{}, + _reply_msg{}, + _lastOutgoingSeqNumber{} +{ +} + +MavlinkFtpTest::~MavlinkFtpTest() +{ + +} + +/// @brief Called before every test to initialize the FTP Server. +void MavlinkFtpTest::_init(void) +{ + _ftp_server = new MavlinkFTP;; + _ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this); + + _cleanup_microsd(); +} + +/// @brief Called after every test to take down the FTP Server. +void MavlinkFtpTest::_cleanup(void) +{ + delete _ftp_server; + + _cleanup_microsd(); +} + +/// @brief Tests for correct behavior of an Ack response. +bool MavlinkFtpTest::_ack_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + payload.opcode = MavlinkFTP::kCmdNone; + + bool success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + + return true; +} + +/// @brief Tests for correct response to an invalid opcpde. +bool MavlinkFtpTest::_bad_opcode_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + payload.opcode = 0xFF; // bogus opcode + + bool success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrUnknownCommand); + + return true; +} + +/// @brief Tests for correct reponse to a payload which an invalid data size field. +bool MavlinkFtpTest::_bad_datasize_test(void) +{ + mavlink_message_t msg; + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + payload.opcode = MavlinkFTP::kCmdListDirectory; + + _setup_ftp_msg(&payload, 0, nullptr, &msg); + + // Set the data size to be one larger than is legal + ((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->size = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1; + + _ftp_server->handle_message(nullptr /* mavlink */, &msg); + + if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidDataSize); + + return true; +} + +bool MavlinkFtpTest::_list_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + char response1[] = "Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240"; + char response2[] = "Ddev|Detc|Dfs|Dobj"; + + struct _testCase { + const char *dir; ///< Directory to run List command on + char *response; ///< Expected response entries from List command + int response_count; ///< Number of directories that should be returned + bool success; ///< true: List command should succeed, false: List command should fail + }; + struct _testCase rgTestCases[] = { + { "/bogus", nullptr, 0, false }, + { "/etc/unit_test_data/mavlink_tests", response1, 4, true }, + { "/", response2, 4, true }, + }; + + for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) { + const struct _testCase *test = &rgTestCases[i]; + + payload.opcode = MavlinkFTP::kCmdListDirectory; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->dir)+1, // size in bytes of data + (uint8_t*)test->dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, strlen(test->response) + 1); + + // The return order of directories from the List command is not repeatable. So we can't do a direct comparison + // to a hardcoded return result string. + + // Convert null terminators to seperator char so we can use strok to parse returned data + for (uint8_t j=0; j<reply->size-1; j++) { + if (reply->data[j] == 0) { + reply->data[j] = '|'; + } + } + + // Loop over returned directory entries trying to find then in the response list + char *dir; + int response_count = 0; + dir = strtok((char *)&reply->data[0], "|"); + while (dir != nullptr) { + ut_assert("Returned directory not found in expected response", strstr(test->response, dir)); + response_count++; + dir = strtok(nullptr, "|"); + } + + ut_compare("Incorrect number of directory entires returned", test->response_count, response_count); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +/// @brief Tests for correct reponse to a List command on a valid directory, but with an offset that +/// is beyond the last directory entry. +bool MavlinkFtpTest::_list_eof_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + const char *dir = "/"; + + payload.opcode = MavlinkFTP::kCmdListDirectory; + payload.offset = 4; // offset past top level dirs + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(dir)+1, // size in bytes of data + (uint8_t*)dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrEOF); + + return true; +} + +/// @brief Tests for correct reponse to an Open command on a file which does not exist. +bool MavlinkFtpTest::_open_badfile_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + const char *dir = "/foo"; // non-existent file + + payload.opcode = MavlinkFTP::kCmdOpenFileRO; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(dir)+1, // size in bytes of data + (uint8_t*)dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + + return true; +} + +/// @brief Tests for correct reponse to an Open command on a file, followed by Terminate +bool MavlinkFtpTest::_open_terminate_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) { + struct stat st; + const ReadTestCase *test = &_rgReadTestCases[i]; + + payload.opcode = MavlinkFTP::kCmdOpenFileRO; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->file)+1, // size in bytes of data + (uint8_t*)test->file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("stat failed", stat(test->file, &st), 0); + + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, sizeof(uint32_t)); + ut_compare("File size incorrect", *((uint32_t*)&reply->data[0]), (uint32_t)st.st_size); + + payload.opcode = MavlinkFTP::kCmdTerminateSession; + payload.session = reply->session; + payload.size = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } + + return true; +} + +/// @brief Tests for correct reponse to a Terminate command on an invalid session. +bool MavlinkFtpTest::_terminate_badsession_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + const char *file = _rgReadTestCases[0].file; + + payload.opcode = MavlinkFTP::kCmdOpenFileRO; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(file)+1, // size in bytes of data + (uint8_t*)file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + payload.opcode = MavlinkFTP::kCmdTerminateSession; + payload.session = reply->session + 1; + payload.size = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession); + + return true; +} + +/// @brief Tests for correct reponse to a Read command on an open session. +bool MavlinkFtpTest::_read_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) { + struct stat st; + const ReadTestCase *test = &_rgReadTestCases[i]; + + // Read in the file so we can compare it to what we get back + ut_compare("stat failed", stat(test->file, &st), 0); + uint8_t *bytes = new uint8_t[st.st_size]; + ut_assert("new failed", bytes != nullptr); + int fd = ::open(test->file, O_RDONLY); + ut_assert("open failed", fd != -1); + int bytes_read = ::read(fd, bytes, st.st_size); + ut_compare("read failed", bytes_read, st.st_size); + ::close(fd); + + // Test case data files are created for specific boundary conditions + ut_compare("Test case data files are out of date", test->length, st.st_size); + + payload.opcode = MavlinkFTP::kCmdOpenFileRO; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->file)+1, // size in bytes of data + (uint8_t*)test->file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + payload.opcode = MavlinkFTP::kCmdReadFile; + payload.session = reply->session; + payload.offset = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Offset incorrect", reply->offset, 0); + + if (test->length <= MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader)) { + ut_compare("Payload size incorrect", reply->size, (uint32_t)st.st_size); + ut_compare("File contents differ", memcmp(reply->data, bytes, st.st_size), 0); + } + + payload.opcode = MavlinkFTP::kCmdTerminateSession; + payload.session = reply->session; + payload.size = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } + + return true; +} + +/// @brief Tests for correct reponse to a Read command on an invalid session. +bool MavlinkFtpTest::_read_badsession_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + const char *file = _rgReadTestCases[0].file; + + payload.opcode = MavlinkFTP::kCmdOpenFileRO; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(file)+1, // size in bytes of data + (uint8_t*)file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + payload.opcode = MavlinkFTP::kCmdReadFile; + payload.session = reply->session + 1; // Invalid session + payload.offset = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession); + + return true; +} + +bool MavlinkFtpTest::_removedirectory_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + int fd; + + struct _testCase { + const char *dir; + bool success; + bool deleteFile; + }; + static const struct _testCase rgTestCases[] = { + { "/bogus", false, false }, + { "/etc/unit_test_data/mavlink_tests/empty_dir", false, false }, + { _unittest_microsd_dir, false, false }, + { _unittest_microsd_file, false, false }, + { _unittest_microsd_dir, true, true }, + }; + + ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0); + ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1); + ::close(fd); + + for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) { + const struct _testCase *test = &rgTestCases[i]; + + if (test->deleteFile) { + ut_compare("unlink failed", ::unlink(_unittest_microsd_file), 0); + } + + payload.opcode = MavlinkFTP::kCmdRemoveDirectory; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->dir)+1, // size in bytes of data + (uint8_t*)test->dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +bool MavlinkFtpTest::_createdirectory_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + struct _testCase { + const char *dir; + bool success; + }; + static const struct _testCase rgTestCases[] = { + { "/etc/bogus", false }, + { _unittest_microsd_dir, true }, + { _unittest_microsd_dir, false }, + { "/fs/microsd/bogus/bogus", false }, + }; + + for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) { + const struct _testCase *test = &rgTestCases[i]; + + payload.opcode = MavlinkFTP::kCmdCreateDirectory; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->dir)+1, // size in bytes of data + (uint8_t*)test->dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +bool MavlinkFtpTest::_removefile_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + int fd; + + struct _testCase { + const char *file; + bool success; + }; + static const struct _testCase rgTestCases[] = { + { "/bogus", false }, + { _rgReadTestCases[0].file, false }, + { _unittest_microsd_dir, false }, + { _unittest_microsd_file, true }, + { _unittest_microsd_file, false }, + }; + + ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0); + ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1); + ::close(fd); + + for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) { + const struct _testCase *test = &rgTestCases[i]; + + payload.opcode = MavlinkFTP::kCmdRemoveFile; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->file)+1, // size in bytes of data + (uint8_t*)test->file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +/// @brief Static method used as callback from MavlinkFTP. This method will be called by MavlinkFTP when +/// it needs to send a message out on Mavlink. +void MavlinkFtpTest::receive_message(const mavlink_message_t *msg, MavlinkFtpTest *ftp_test) +{ + ftp_test->_receive_message(msg); +} + +/// @brief Non-Static version of receive_message +void MavlinkFtpTest::_receive_message(const mavlink_message_t *msg) +{ + // Move the message into our own member variable + memcpy(&_reply_msg, msg, sizeof(mavlink_message_t)); +} + +/// @brief Decode and validate the incoming message +bool MavlinkFtpTest::_decode_message(const mavlink_message_t *msg, ///< Mavlink message to decode + mavlink_file_transfer_protocol_t *ftp_msg, ///< Decoded FTP message + MavlinkFTP::PayloadHeader **payload) ///< Payload inside FTP message response +{ + mavlink_msg_file_transfer_protocol_decode(msg, ftp_msg); + + // Make sure the targets are correct + ut_compare("Target network non-zero", ftp_msg->target_network, 0); + ut_compare("Target system id mismatch", ftp_msg->target_system, clientSystemId); + ut_compare("Target component id mismatch", ftp_msg->target_component, clientComponentId); + + *payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(ftp_msg->payload); + + // Make sure we have a good sequence number + ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1); + + // Bump sequence number for next outgoing message + _lastOutgoingSeqNumber++; + + return true; +} + +/// @brief Initializes an FTP message into a mavlink message +void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header + uint8_t size, ///< size in bytes of data + const uint8_t *data, ///< Data to start into FTP message payload + mavlink_message_t *msg) ///< Returned mavlink message +{ + uint8_t payload_bytes[MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN]; + MavlinkFTP::PayloadHeader *payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(payload_bytes); + + memcpy(payload, payload_header, sizeof(MavlinkFTP::PayloadHeader)); + + payload->seqNumber = _lastOutgoingSeqNumber; + payload->size = size; + if (size != 0) { + memcpy(payload->data, data, size); + } + + payload->padding[0] = 0; + payload->padding[1] = 0; + + msg->checksum = 0; + mavlink_msg_file_transfer_protocol_pack(clientSystemId, // Sender system id + clientComponentId, // Sender component id + msg, // Message to pack payload into + 0, // Target network + serverSystemId, // Target system id + serverComponentId, // Target component id + payload_bytes); // Payload to pack into message +} + +/// @brief Sends the specified FTP message to the server and returns response +bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header + uint8_t size, ///< size in bytes of data + const uint8_t *data, ///< Data to start into FTP message payload + mavlink_file_transfer_protocol_t *ftp_msg_reply, ///< Response from server + MavlinkFTP::PayloadHeader **payload_reply) ///< Payload inside FTP message response +{ + mavlink_message_t msg; + + _setup_ftp_msg(payload_header, size, data, &msg); + _ftp_server->handle_message(nullptr /* mavlink */, &msg); + return _decode_message(&_reply_msg, ftp_msg_reply, payload_reply); +} + +/// @brief Cleans up an files created on microsd during testing +void MavlinkFtpTest::_cleanup_microsd(void) +{ + ::unlink(_unittest_microsd_file); + ::rmdir(_unittest_microsd_dir); +} + +/// @brief Runs all the unit tests +bool MavlinkFtpTest::run_tests(void) +{ + ut_run_test(_ack_test); + ut_run_test(_bad_opcode_test); + ut_run_test(_bad_datasize_test); + ut_run_test(_list_test); + ut_run_test(_list_eof_test); + ut_run_test(_open_badfile_test); + ut_run_test(_open_terminate_test); + ut_run_test(_terminate_badsession_test); + ut_run_test(_read_test); + ut_run_test(_read_badsession_test); + ut_run_test(_removedirectory_test); + ut_run_test(_createdirectory_test); + ut_run_test(_removefile_test); + + return (_tests_failed == 0); + +} + +ut_declare_test(mavlink_ftp_test, MavlinkFtpTest) diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h new file mode 100644 index 000000000..2696192cc --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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_test.h +/// @author Don Gagne <don@thegagnes.com> + +#pragma once + +#include <unit_test/unit_test.h> +#include "../mavlink_bridge_header.h" +#include "../mavlink_ftp.h" + +class MavlinkFtpTest : public UnitTest +{ +public: + MavlinkFtpTest(); + virtual ~MavlinkFtpTest(); + + virtual bool run_tests(void); + + static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest); + + static const uint8_t serverSystemId = 50; ///< System ID for server + static const uint8_t serverComponentId = 1; ///< Component ID for server + static const uint8_t serverChannel = 0; ///< Channel to send to + + static const uint8_t clientSystemId = 1; ///< System ID for client + static const uint8_t clientComponentId = 0; ///< Component ID for client + + // We don't want any of these + MavlinkFtpTest(const MavlinkFtpTest&); + MavlinkFtpTest& operator=(const MavlinkFtpTest&); + +private: + virtual void _init(void); + virtual void _cleanup(void); + + bool _ack_test(void); + bool _bad_opcode_test(void); + bool _bad_datasize_test(void); + bool _list_test(void); + bool _list_eof_test(void); + bool _open_badfile_test(void); + bool _open_terminate_test(void); + bool _terminate_badsession_test(void); + bool _read_test(void); + bool _read_badsession_test(void); + bool _removedirectory_test(void); + bool _createdirectory_test(void); + bool _removefile_test(void); + + void _receive_message(const mavlink_message_t *msg); + void _setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg); + bool _decode_message(const mavlink_message_t *msg, mavlink_file_transfer_protocol_t *ftp_msg, MavlinkFTP::PayloadHeader **payload); + bool _send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, + uint8_t size, + const uint8_t *data, + mavlink_file_transfer_protocol_t *ftp_msg_reply, + MavlinkFTP::PayloadHeader **payload_reply); + void _cleanup_microsd(void); + + MavlinkFTP *_ftp_server; + + mavlink_message_t _reply_msg; + + uint16_t _lastOutgoingSeqNumber; + + struct ReadTestCase { + const char *file; + const uint16_t length; + }; + static const ReadTestCase _rgReadTestCases[]; + + static const char _unittest_microsd_dir[]; + static const char _unittest_microsd_file[]; +}; + +bool mavlink_ftp_test(void); diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py new file mode 100644 index 000000000..a7e68f2a3 --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py @@ -0,0 +1,7 @@ +import sys +print 'Arguments: file - ' + sys.argv[1] + ', length - ' + sys.argv[2] +file = open(sys.argv[1], 'w') +for i in range(int(sys.argv[2])): + b = bytearray([i & 0xFF]) + file.write(b) +file.close()
\ No newline at end of file diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp index abdba34b9..10cbcb0ec 100644 --- a/src/modules/mavlink/mavlink_commands.h +++ b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * 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 @@ -32,34 +32,16 @@ ****************************************************************************/ /** - * @file mavlink_commands.h - * Mavlink commands stream definition. - * - * @author Anton Babushkin <anton.babushkin@me.com> + * @file mavlink_ftp_tests.cpp */ -#ifndef MAVLINK_COMMANDS_H_ -#define MAVLINK_COMMANDS_H_ - -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_command.h> +#include <systemlib/err.h> -class Mavlink; -class MavlinkCommansStream; +#include "mavlink_ftp_test.h" -#include "mavlink_main.h" +extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]); -class MavlinkCommandsStream +int mavlink_tests_main(int argc, char *argv[]) { -private: - MavlinkOrbSubscription *_cmd_sub; - struct vehicle_command_s *_cmd; - mavlink_channel_t _channel; - uint64_t _cmd_time; - -public: - MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel); - void update(const hrt_abstime t); -}; - -#endif /* MAVLINK_COMMANDS_H_ */ + return mavlink_ftp_test() ? 0 : -1; +} diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk new file mode 100644 index 000000000..1cc28cce1 --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/module.mk @@ -0,0 +1,50 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# System state machine tests. +# + +MODULE_COMMAND = mavlink_tests +SRCS = mavlink_tests.cpp \ + mavlink_ftp_test.cpp \ + ../mavlink_ftp.cpp \ + ../mavlink.c + +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink + +MODULE_STACKSIZE = 5000 + +MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 1986ae3c8..91fdd6154 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -40,11 +40,11 @@ SRCS += mavlink_main.cpp \ 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_commands.cpp \ mavlink_ftp.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink |