fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 00:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

MAVLink: Code style

Browse Source
This commit is contained in:
Lorenz Meier
2016-08-26 23:43:33 +02:00
parent e67a6bdfc2
commit 720c445f21
17 changed files with 922 additions and 627 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_ftp.cpp
+131 -79
View File
@@ -51,7 +51,7 @@
int buf_size_1 = 0; int buf_size_1 = 0;
int buf_size_2 = 0; int buf_size_2 = 0;
MavlinkFTP::MavlinkFTP(Mavlink* mavlink) : MavlinkFTP::MavlinkFTP(Mavlink *mavlink) :
MavlinkStream(mavlink), MavlinkStream(mavlink),
_session_info{}, _session_info{},
_utRcvMsgFunc{}, _utRcvMsgFunc{},
@@ -63,10 +63,10 @@ MavlinkFTP::MavlinkFTP(Mavlink* mavlink) :
MavlinkFTP::~MavlinkFTP() MavlinkFTP::~MavlinkFTP()
{ {
} }
const char* const char *
MavlinkFTP::get_name(void) const MavlinkFTP::get_name(void) const
{ {
return "MAVLINK_FTP"; return "MAVLINK_FTP";
@@ -83,13 +83,13 @@ MavlinkFTP::get_size(void)
{ {
if (_session_info.stream_download) { if (_session_info.stream_download) {
return MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; return MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
} else { } else {
return 0; return 0;
} }
} }
MavlinkStream* MavlinkStream *
MavlinkFTP::new_instance(Mavlink *mavlink) MavlinkFTP::new_instance(Mavlink *mavlink)
{ {
return new MavlinkFTP(mavlink); return new MavlinkFTP(mavlink);
@@ -144,15 +144,15 @@ void
MavlinkFTP::handle_message(const mavlink_message_t *msg) MavlinkFTP::handle_message(const mavlink_message_t *msg)
{ {
//warnx("MavlinkFTP::handle_message %d %d", buf_size_1, buf_size_2); //warnx("MavlinkFTP::handle_message %d %d", buf_size_1, buf_size_2);
if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
mavlink_file_transfer_protocol_t ftp_request; mavlink_file_transfer_protocol_t ftp_request;
mavlink_msg_file_transfer_protocol_decode(msg, &ftp_request); mavlink_msg_file_transfer_protocol_decode(msg, &ftp_request);
#ifdef MAVLINK_FTP_DEBUG #ifdef MAVLINK_FTP_DEBUG
warnx("FTP: received ftp protocol message target_system: %d", ftp_request.target_system); warnx("FTP: received ftp protocol message target_system: %d", ftp_request.target_system);
#endif #endif
if (ftp_request.target_system == _getServerSystemId()) { if (ftp_request.target_system == _getServerSystemId()) {
_process_request(&ftp_request, msg->sysid); _process_request(&ftp_request, msg->sysid);
return; return;
@@ -162,7 +162,7 @@ MavlinkFTP::handle_message(const mavlink_message_t *msg)
/// @brief Processes an FTP message /// @brief Processes an FTP message
void void
MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t* ftp_req, uint8_t target_system_id) MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t *ftp_req, uint8_t target_system_id)
{ {
bool stream_send = false; bool stream_send = false;
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]); PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]);
@@ -176,7 +176,8 @@ MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t* ftp_req, uint8_t
} }
#ifdef MAVLINK_FTP_DEBUG #ifdef MAVLINK_FTP_DEBUG
printf("ftp: channel %u opc %u size %u offset %u\n", _getServerChannel(), payload->opcode, payload->size, payload->offset); printf("ftp: channel %u opc %u size %u offset %u\n", _getServerChannel(), payload->opcode, payload->size,
payload->offset);
#endif #endif
switch (payload->opcode) { switch (payload->opcode) {
@@ -215,7 +216,7 @@ MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t* ftp_req, uint8_t
errorCode = _workBurst(payload, target_system_id); errorCode = _workBurst(payload, target_system_id);
stream_send = true; stream_send = true;
break; break;
case kCmdWriteFile: case kCmdWriteFile:
errorCode = _workWrite(payload); errorCode = _workWrite(payload);
break; break;
@@ -235,33 +236,35 @@ MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t* ftp_req, uint8_t
case kCmdCreateDirectory: case kCmdCreateDirectory:
errorCode = _workCreateDirectory(payload); errorCode = _workCreateDirectory(payload);
break; break;
case kCmdRemoveDirectory: case kCmdRemoveDirectory:
errorCode = _workRemoveDirectory(payload); errorCode = _workRemoveDirectory(payload);
break; break;
case kCmdCalcFileCRC32: case kCmdCalcFileCRC32:
errorCode = _workCalcFileCRC32(payload); errorCode = _workCalcFileCRC32(payload);
break; break;
default: default:
errorCode = kErrUnknownCommand; errorCode = kErrUnknownCommand;
break; break;
} }
out: out:
payload->seq_number++; payload->seq_number++;
// handle success vs. error // handle success vs. error
if (errorCode == kErrNone) { if (errorCode == kErrNone) {
payload->req_opcode = payload->opcode; payload->req_opcode = payload->opcode;
payload->opcode = kRspAck; payload->opcode = kRspAck;
} else { } else {
int r_errno = errno; int r_errno = errno;
payload->req_opcode = payload->opcode; payload->req_opcode = payload->opcode;
payload->opcode = kRspNak; payload->opcode = kRspNak;
payload->size = 1; payload->size = 1;
payload->data[0] = errorCode; payload->data[0] = errorCode;
if (errorCode == kErrFailErrno) { if (errorCode == kErrFailErrno) {
payload->size = 2; payload->size = 2;
payload->data[1] = r_errno; payload->data[1] = r_errno;
@@ -278,14 +281,14 @@ out:
/// @brief Sends the specified FTP response message out through mavlink /// @brief Sends the specified FTP response message out through mavlink
void void
MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req) MavlinkFTP::_reply(mavlink_file_transfer_protocol_t *ftp_req)
{ {
#ifdef MAVLINK_FTP_DEBUG #ifdef MAVLINK_FTP_DEBUG
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]); PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]);
warnx("FTP: %s seq_number: %d", payload->opcode == kRspAck ? "Ack" : "Nak", payload->seq_number); warnx("FTP: %s seq_number: %d", payload->opcode == kRspAck ? "Ack" : "Nak", payload->seq_number);
#endif #endif
ftp_req->target_network = 0; ftp_req->target_network = 0;
ftp_req->target_component = 0; ftp_req->target_component = 0;
#ifdef MAVLINK_FTP_UNIT_TEST #ifdef MAVLINK_FTP_UNIT_TEST
@@ -299,7 +302,7 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req)
/// @brief Responds to a List command /// @brief Responds to a List command
MavlinkFTP::ErrorCode MavlinkFTP::ErrorCode
MavlinkFTP::_workList(PayloadHeader* payload, bool list_hidden) MavlinkFTP::_workList(PayloadHeader *payload, bool list_hidden)
{ {
char dirPath[kMaxDataLength]; char dirPath[kMaxDataLength];
strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength); strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength);
@@ -333,7 +336,7 @@ MavlinkFTP::_workList(PayloadHeader* payload, bool list_hidden)
// read the directory entry // read the directory entry
if (readdir_r(dp, &entry, &result)) { if (readdir_r(dp, &entry, &result)) {
#ifdef MAVLINK_FTP_UNIT_TEST #ifdef MAVLINK_FTP_UNIT_TEST
warnx("readdir_r failed"); warnx("readdir_r failed");
#else #else
_mavlink->send_statustext_critical("FTP: list readdir_r failure"); _mavlink->send_statustext_critical("FTP: list readdir_r failure");
_mavlink->send_statustext_critical(dirPath); _mavlink->send_statustext_critical(dirPath);
@@ -355,6 +358,7 @@ MavlinkFTP::_workList(PayloadHeader* payload, bool list_hidden)
// to seek past EOF. // to seek past EOF.
errorCode = kErrEOF; errorCode = kErrEOF;
} }
// Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that // Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that
break; break;
} }
@@ -366,6 +370,7 @@ MavlinkFTP::_workList(PayloadHeader* payload, bool list_hidden)
// Determine the directory entry type // Determine the directory entry type
switch (entry.d_type) { switch (entry.d_type) {
#ifdef __PX4_NUTTX #ifdef __PX4_NUTTX
case DTYPE_FILE: case DTYPE_FILE:
#else #else
case DT_REG: case DT_REG:
@@ -374,51 +379,60 @@ MavlinkFTP::_workList(PayloadHeader* payload, bool list_hidden)
direntType = kDirentFile; direntType = kDirentFile;
snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name); snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name);
struct stat st; struct stat st;
if (stat(buf, &st) == 0) { if (stat(buf, &st) == 0) {
fileSize = st.st_size; fileSize = st.st_size;
} }
break; break;
#ifdef __PX4_NUTTX #ifdef __PX4_NUTTX
case DTYPE_DIRECTORY: case DTYPE_DIRECTORY:
#else #else
case DT_DIR: case DT_DIR:
#endif #endif
if ((!list_hidden && (strncmp(entry.d_name, ".", 1) == 0)) || if ((!list_hidden && (strncmp(entry.d_name, ".", 1) == 0)) ||
strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
// Don't bother sending these back // Don't bother sending these back
direntType = kDirentSkip; direntType = kDirentSkip;
} else { } else {
direntType = kDirentDir; direntType = kDirentDir;
} }
break; break;
default: default:
// We only send back file and diretory entries, skip everything else // We only send back file and diretory entries, skip everything else
direntType = kDirentSkip; direntType = kDirentSkip;
} }
if (direntType == kDirentSkip) { if (direntType == kDirentSkip) {
// Skip send only dirent identifier // Skip send only dirent identifier
buf[0] = '\0'; buf[0] = '\0';
} else if (direntType == kDirentFile) { } else if (direntType == kDirentFile) {
// Files send filename and file length // Files send filename and file length
snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize); snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize);
} else { } else {
// Everything else just sends name // Everything else just sends name
strncpy(buf, entry.d_name, sizeof(buf)); strncpy(buf, entry.d_name, sizeof(buf));
buf[sizeof(buf)-1] = 0; buf[sizeof(buf) - 1] = 0;
} }
size_t nameLen = strlen(buf); size_t nameLen = strlen(buf);
// Do we have room for the name, the one char directory identifier and the null terminator? // Do we have room for the name, the one char directory identifier and the null terminator?
if ((offset + nameLen + 2) > kMaxDataLength) { if ((offset + nameLen + 2) > kMaxDataLength) {
break; break;
} }
// Move the data into the buffer // Move the data into the buffer
payload->data[offset++] = direntType; payload->data[offset++] = direntType;
strcpy((char *)&payload->data[offset], buf); strcpy((char *)&payload->data[offset], buf);
#ifdef MAVLINK_FTP_DEBUG #ifdef MAVLINK_FTP_DEBUG
printf("FTP: list %s %s\n", dirPath, (char *)&payload->data[offset-1]); printf("FTP: list %s %s\n", dirPath, (char *)&payload->data[offset - 1]);
#endif #endif
offset += nameLen + 1; offset += nameLen + 1;
} }
@@ -431,7 +445,7 @@ MavlinkFTP::_workList(PayloadHeader* payload, bool list_hidden)
/// @brief Responds to an Open command /// @brief Responds to an Open command
MavlinkFTP::ErrorCode MavlinkFTP::ErrorCode
MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag) MavlinkFTP::_workOpen(PayloadHeader *payload, int oflag)
{ {
if (_session_info.fd >= 0) { if (_session_info.fd >= 0) {
warnx("FTP: Open failed - out of sessions\n"); warnx("FTP: Open failed - out of sessions\n");
@@ -439,41 +453,47 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
} }
char *filename = _data_as_cstring(payload); char *filename = _data_as_cstring(payload);
#ifdef MAVLINK_FTP_DEBUG #ifdef MAVLINK_FTP_DEBUG
warnx("FTP: open '%s'", filename); warnx("FTP: open '%s'", filename);
#endif #endif
uint32_t fileSize = 0; uint32_t fileSize = 0;
struct stat st; struct stat st;
if (stat(filename, &st) != 0) { if (stat(filename, &st) != 0) {
// fail only if requested open for read // fail only if requested open for read
if (oflag & O_RDONLY) if (oflag & O_RDONLY) {
return kErrFailErrno; return kErrFailErrno;
else
} else {
st.st_size = 0; st.st_size = 0;
}
} }
fileSize = st.st_size; fileSize = st.st_size;
// Set mode to 666 incase oflag has O_CREAT // Set mode to 666 incase oflag has O_CREAT
int fd = ::open(filename, oflag, PX4_O_MODE_666); int fd = ::open(filename, oflag, PX4_O_MODE_666);
if (fd < 0) { if (fd < 0) {
return kErrFailErrno; return kErrFailErrno;
} }
_session_info.fd = fd; _session_info.fd = fd;
_session_info.file_size = fileSize; _session_info.file_size = fileSize;
_session_info.stream_download = false; _session_info.stream_download = false;
payload->session = 0; payload->session = 0;
payload->size = sizeof(uint32_t); payload->size = sizeof(uint32_t);
*((uint32_t*)payload->data) = fileSize; *((uint32_t *)payload->data) = fileSize;
return kErrNone; return kErrNone;
} }
/// @brief Responds to a Read command /// @brief Responds to a Read command
MavlinkFTP::ErrorCode MavlinkFTP::ErrorCode
MavlinkFTP::_workRead(PayloadHeader* payload) MavlinkFTP::_workRead(PayloadHeader *payload)
{ {
if (payload->session != 0 || _session_info.fd < 0) { if (payload->session != 0 || _session_info.fd < 0) {
return kErrInvalidSession; return kErrInvalidSession;
@@ -482,18 +502,20 @@ MavlinkFTP::_workRead(PayloadHeader* payload)
#ifdef MAVLINK_FTP_DEBUG #ifdef MAVLINK_FTP_DEBUG
warnx("FTP: read offset:%d", payload->offset); warnx("FTP: read offset:%d", payload->offset);
#endif #endif
// We have to test seek past EOF ourselves, lseek will allow seek past EOF // We have to test seek past EOF ourselves, lseek will allow seek past EOF
if (payload->offset >= _session_info.file_size) { if (payload->offset >= _session_info.file_size) {
warnx("request past EOF"); warnx("request past EOF");
return kErrEOF; return kErrEOF;
} }
if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) { if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) {
warnx("seek fail"); warnx("seek fail");
return kErrFailErrno; return kErrFailErrno;
} }
int bytes_read = ::read(_session_info.fd, &payload->data[0], kMaxDataLength); int bytes_read = ::read(_session_info.fd, &payload->data[0], kMaxDataLength);
if (bytes_read < 0) { if (bytes_read < 0) {
// Negative return indicates error other than eof // Negative return indicates error other than eof
warnx("read fail %d", bytes_read); warnx("read fail %d", bytes_read);
@@ -507,12 +529,12 @@ MavlinkFTP::_workRead(PayloadHeader* payload)
/// @brief Responds to a Stream command /// @brief Responds to a Stream command
MavlinkFTP::ErrorCode MavlinkFTP::ErrorCode
MavlinkFTP::_workBurst(PayloadHeader* payload, uint8_t target_system_id) MavlinkFTP::_workBurst(PayloadHeader *payload, uint8_t target_system_id)
{ {
if (payload->session != 0 && _session_info.fd < 0) { if (payload->session != 0 && _session_info.fd < 0) {
return kErrInvalidSession; return kErrInvalidSession;
} }
#ifdef MAVLINK_FTP_DEBUG #ifdef MAVLINK_FTP_DEBUG
warnx("FTP: burst offset:%d", payload->offset); warnx("FTP: burst offset:%d", payload->offset);
#endif #endif
@@ -528,7 +550,7 @@ MavlinkFTP::_workBurst(PayloadHeader* payload, uint8_t target_system_id)
/// @brief Responds to a Write command /// @brief Responds to a Write command
MavlinkFTP::ErrorCode MavlinkFTP::ErrorCode
MavlinkFTP::_workWrite(PayloadHeader* payload) MavlinkFTP::_workWrite(PayloadHeader *payload)
{ {
if (payload->session != 0 && _session_info.fd < 0) { if (payload->session != 0 && _session_info.fd < 0) {
return kErrInvalidSession; return kErrInvalidSession;
@@ -541,6 +563,7 @@ MavlinkFTP::_workWrite(PayloadHeader* payload)
} }
int bytes_written = ::write(_session_info.fd, &payload->data[0], payload->size); int bytes_written = ::write(_session_info.fd, &payload->data[0], payload->size);
if (bytes_written < 0) { if (bytes_written < 0) {
// Negative return indicates error other than eof // Negative return indicates error other than eof
warnx("write fail %d", bytes_written); warnx("write fail %d", bytes_written);
@@ -548,21 +571,22 @@ MavlinkFTP::_workWrite(PayloadHeader* payload)
} }
payload->size = sizeof(uint32_t); payload->size = sizeof(uint32_t);
*((uint32_t*)payload->data) = bytes_written; *((uint32_t *)payload->data) = bytes_written;
return kErrNone; return kErrNone;
} }
/// @brief Responds to a RemoveFile command /// @brief Responds to a RemoveFile command
MavlinkFTP::ErrorCode MavlinkFTP::ErrorCode
MavlinkFTP::_workRemoveFile(PayloadHeader* payload) MavlinkFTP::_workRemoveFile(PayloadHeader *payload)
{ {
char file[kMaxDataLength]; char file[kMaxDataLength];
strncpy(file, _data_as_cstring(payload), kMaxDataLength); strncpy(file, _data_as_cstring(payload), kMaxDataLength);
if (unlink(file) == 0) { if (unlink(file) == 0) {
payload->size = 0; payload->size = 0;
return kErrNone; return kErrNone;
} else { } else {
return kErrFailErrno; return kErrFailErrno;
} }
@@ -570,7 +594,7 @@ MavlinkFTP::_workRemoveFile(PayloadHeader* payload)
/// @brief Responds to a TruncateFile command /// @brief Responds to a TruncateFile command
MavlinkFTP::ErrorCode MavlinkFTP::ErrorCode
MavlinkFTP::_workTruncateFile(PayloadHeader* payload) MavlinkFTP::_workTruncateFile(PayloadHeader *payload)
{ {
char file[kMaxDataLength]; char file[kMaxDataLength];
const char temp_file[] = PX4_ROOTFSDIR"/fs/microsd/.trunc.tmp"; const char temp_file[] = PX4_ROOTFSDIR"/fs/microsd/.trunc.tmp";
@@ -581,6 +605,7 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
// copying to temp and overwrite with O_TRUNC flag. // copying to temp and overwrite with O_TRUNC flag.
struct stat st; struct stat st;
if (stat(file, &st) != 0) { if (stat(file, &st) != 0) {
return kErrFailErrno; return kErrFailErrno;
} }
@@ -599,20 +624,22 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
if (payload->offset == (unsigned)st.st_size) { if (payload->offset == (unsigned)st.st_size) {
// nothing to do // nothing to do
return kErrNone; return kErrNone;
}
else if (payload->offset == 0) { } else if (payload->offset == 0) {
// 1: truncate all data // 1: truncate all data
int fd = ::open(file, O_TRUNC | O_WRONLY); int fd = ::open(file, O_TRUNC | O_WRONLY);
if (fd < 0) { if (fd < 0) {
return kErrFailErrno; return kErrFailErrno;
} }
::close(fd); ::close(fd);
return kErrNone; return kErrNone;
}
else if (payload->offset > (unsigned)st.st_size) { } else if (payload->offset > (unsigned)st.st_size) {
// 2: extend file // 2: extend file
int fd = ::open(file, O_WRONLY); int fd = ::open(file, O_WRONLY);
if (fd < 0) { if (fd < 0) {
return kErrFailErrno; return kErrFailErrno;
} }
@@ -625,16 +652,18 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
bool ok = 1 == ::write(fd, "", 1); bool ok = 1 == ::write(fd, "", 1);
::close(fd); ::close(fd);
return (ok)? kErrNone : kErrFailErrno; return (ok) ? kErrNone : kErrFailErrno;
}
else { } else {
// 3: truncate // 3: truncate
if (_copy_file(file, temp_file, payload->offset) != 0) { if (_copy_file(file, temp_file, payload->offset) != 0) {
return kErrFailErrno; return kErrFailErrno;
} }
if (_copy_file(temp_file, file, payload->offset) != 0) { if (_copy_file(temp_file, file, payload->offset) != 0) {
return kErrFailErrno; return kErrFailErrno;
} }
if (::unlink(temp_file) != 0) { if (::unlink(temp_file) != 0) {
return kErrFailErrno; return kErrFailErrno;
} }
@@ -645,16 +674,16 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
/// @brief Responds to a Terminate command /// @brief Responds to a Terminate command
MavlinkFTP::ErrorCode MavlinkFTP::ErrorCode
MavlinkFTP::_workTerminate(PayloadHeader* payload) MavlinkFTP::_workTerminate(PayloadHeader *payload)
{ {
if (payload->session != 0 || _session_info.fd < 0) { if (payload->session != 0 || _session_info.fd < 0) {
return kErrInvalidSession; return kErrInvalidSession;
} }
::close(_session_info.fd); ::close(_session_info.fd);
_session_info.fd = -1; _session_info.fd = -1;
_session_info.stream_download = false; _session_info.stream_download = false;
payload->size = 0; payload->size = 0;
return kErrNone; return kErrNone;
@@ -662,7 +691,7 @@ MavlinkFTP::_workTerminate(PayloadHeader* payload)
/// @brief Responds to a Reset command /// @brief Responds to a Reset command
MavlinkFTP::ErrorCode MavlinkFTP::ErrorCode
MavlinkFTP::_workReset(PayloadHeader* payload) MavlinkFTP::_workReset(PayloadHeader *payload)
{ {
if (_session_info.fd != -1) { if (_session_info.fd != -1) {
::close(_session_info.fd); ::close(_session_info.fd);
@@ -671,13 +700,13 @@ MavlinkFTP::_workReset(PayloadHeader* payload)
} }
payload->size = 0; payload->size = 0;
return kErrNone; return kErrNone;
} }
/// @brief Responds to a Rename command /// @brief Responds to a Rename command
MavlinkFTP::ErrorCode MavlinkFTP::ErrorCode
MavlinkFTP::_workRename(PayloadHeader* payload) MavlinkFTP::_workRename(PayloadHeader *payload)
{ {
char oldpath[kMaxDataLength]; char oldpath[kMaxDataLength];
char newpath[kMaxDataLength]; char newpath[kMaxDataLength];
@@ -697,6 +726,7 @@ MavlinkFTP::_workRename(PayloadHeader* payload)
if (rename(oldpath, newpath) == 0) { if (rename(oldpath, newpath) == 0) {
payload->size = 0; payload->size = 0;
return kErrNone; return kErrNone;
} else { } else {
return kErrFailErrno; return kErrFailErrno;
} }
@@ -704,14 +734,15 @@ MavlinkFTP::_workRename(PayloadHeader* payload)
/// @brief Responds to a RemoveDirectory command /// @brief Responds to a RemoveDirectory command
MavlinkFTP::ErrorCode MavlinkFTP::ErrorCode
MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload) MavlinkFTP::_workRemoveDirectory(PayloadHeader *payload)
{ {
char dir[kMaxDataLength]; char dir[kMaxDataLength];
strncpy(dir, _data_as_cstring(payload), kMaxDataLength); strncpy(dir, _data_as_cstring(payload), kMaxDataLength);
if (rmdir(dir) == 0) { if (rmdir(dir) == 0) {
payload->size = 0; payload->size = 0;
return kErrNone; return kErrNone;
} else { } else {
return kErrFailErrno; return kErrFailErrno;
} }
@@ -719,14 +750,15 @@ MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload)
/// @brief Responds to a CreateDirectory command /// @brief Responds to a CreateDirectory command
MavlinkFTP::ErrorCode MavlinkFTP::ErrorCode
MavlinkFTP::_workCreateDirectory(PayloadHeader* payload) MavlinkFTP::_workCreateDirectory(PayloadHeader *payload)
{ {
char dir[kMaxDataLength]; char dir[kMaxDataLength];
strncpy(dir, _data_as_cstring(payload), kMaxDataLength); strncpy(dir, _data_as_cstring(payload), kMaxDataLength);
if (mkdir(dir, S_IRWXU | S_IRWXG | S_IRWXO) == 0) { if (mkdir(dir, S_IRWXU | S_IRWXG | S_IRWXO) == 0) {
payload->size = 0; payload->size = 0;
return kErrNone; return kErrNone;
} else { } else {
return kErrFailErrno; return kErrFailErrno;
} }
@@ -734,7 +766,7 @@ MavlinkFTP::_workCreateDirectory(PayloadHeader* payload)
/// @brief Responds to a CalcFileCRC32 command /// @brief Responds to a CalcFileCRC32 command
MavlinkFTP::ErrorCode MavlinkFTP::ErrorCode
MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload) MavlinkFTP::_workCalcFileCRC32(PayloadHeader *payload)
{ {
char file_buf[256]; char file_buf[256];
uint32_t checksum = 0; uint32_t checksum = 0;
@@ -742,12 +774,14 @@ MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload)
strncpy(file_buf, _data_as_cstring(payload), kMaxDataLength); strncpy(file_buf, _data_as_cstring(payload), kMaxDataLength);
int fd = ::open(file_buf, O_RDONLY); int fd = ::open(file_buf, O_RDONLY);
if (fd < 0) { if (fd < 0) {
return kErrFailErrno; return kErrFailErrno;
} }
do { do {
bytes_read = ::read(fd, file_buf, sizeof(file_buf)); bytes_read = ::read(fd, file_buf, sizeof(file_buf));
if (bytes_read < 0) { if (bytes_read < 0) {
int r_errno = errno; int r_errno = errno;
::close(fd); ::close(fd);
@@ -755,30 +789,31 @@ MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload)
return kErrFailErrno; return kErrFailErrno;
} }
checksum = crc32part((uint8_t*)file_buf, bytes_read, checksum); checksum = crc32part((uint8_t *)file_buf, bytes_read, checksum);
} while (bytes_read == sizeof(file_buf)); } while (bytes_read == sizeof(file_buf));
::close(fd); ::close(fd);
payload->size = sizeof(uint32_t); payload->size = sizeof(uint32_t);
*((uint32_t*)payload->data) = checksum; *((uint32_t *)payload->data) = checksum;
return kErrNone; return kErrNone;
} }
/// @brief Guarantees that the payload data is null terminated. /// @brief Guarantees that the payload data is null terminated.
/// @return Returns a pointer to the payload data as a char * /// @return Returns a pointer to the payload data as a char *
char * char *
MavlinkFTP::_data_as_cstring(PayloadHeader* payload) MavlinkFTP::_data_as_cstring(PayloadHeader *payload)
{ {
// guarantee nul termination // guarantee nul termination
if (payload->size < kMaxDataLength) { if (payload->size < kMaxDataLength) {
payload->data[payload->size] = '\0'; payload->data[payload->size] = '\0';
} else { } else {
payload->data[kMaxDataLength - 1] = '\0'; payload->data[kMaxDataLength - 1] = '\0';
} }
// and return data // and return data
return (char *)&(payload->data[0]); return (char *) & (payload->data[0]);
} }
/// @brief Copy file (with limited space) /// @brief Copy file (with limited space)
@@ -790,6 +825,7 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length
int op_errno = 0; int op_errno = 0;
src_fd = ::open(src_path, O_RDONLY); src_fd = ::open(src_path, O_RDONLY);
if (src_fd < 0) { if (src_fd < 0) {
return -1; return -1;
} }
@@ -799,7 +835,8 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length
#ifdef __PX4_POSIX #ifdef __PX4_POSIX
, 0666 , 0666
#endif #endif
); );
if (dst_fd < 0) { if (dst_fd < 0) {
op_errno = errno; op_errno = errno;
::close(src_fd); ::close(src_fd);
@@ -809,20 +846,22 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length
while (length > 0) { while (length > 0) {
ssize_t bytes_read, bytes_written; ssize_t bytes_read, bytes_written;
size_t blen = (length > sizeof(buff))? sizeof(buff) : length; size_t blen = (length > sizeof(buff)) ? sizeof(buff) : length;
bytes_read = ::read(src_fd, buff, blen); bytes_read = ::read(src_fd, buff, blen);
if (bytes_read == 0) { if (bytes_read == 0) {
// EOF // EOF
break; break;
}
else if (bytes_read < 0) { } else if (bytes_read < 0) {
warnx("cp: read"); warnx("cp: read");
op_errno = errno; op_errno = errno;
break; break;
} }
bytes_written = ::write(dst_fd, buff, bytes_read); bytes_written = ::write(dst_fd, buff, bytes_read);
if (bytes_written != bytes_read) { if (bytes_written != bytes_read) {
warnx("cp: short write"); warnx("cp: short write");
op_errno = errno; op_errno = errno;
@@ -836,7 +875,7 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length
::close(dst_fd); ::close(dst_fd);
errno = op_errno; errno = op_errno;
return (length > 0)? -1 : 0; return (length > 0) ? -1 : 0;
} }
void MavlinkFTP::send(const hrt_abstime t) void MavlinkFTP::send(const hrt_abstime t)
@@ -845,29 +884,32 @@ void MavlinkFTP::send(const hrt_abstime t)
if (!_session_info.stream_download) { if (!_session_info.stream_download) {
return; return;
} }
#ifndef MAVLINK_FTP_UNIT_TEST #ifndef MAVLINK_FTP_UNIT_TEST
// Skip send if not enough room // Skip send if not enough room
unsigned max_bytes_to_send = _mavlink->get_free_tx_buf(); unsigned max_bytes_to_send = _mavlink->get_free_tx_buf();
#ifdef MAVLINK_FTP_DEBUG #ifdef MAVLINK_FTP_DEBUG
warnx("MavlinkFTP::send max_bytes_to_send(%d) get_free_tx_buf(%d)", max_bytes_to_send, _mavlink->get_free_tx_buf()); warnx("MavlinkFTP::send max_bytes_to_send(%d) get_free_tx_buf(%d)", max_bytes_to_send, _mavlink->get_free_tx_buf());
#endif #endif
if (max_bytes_to_send < get_size()) { if (max_bytes_to_send < get_size()) {
return; return;
} }
#endif #endif
// Send stream packets until buffer is full // Send stream packets until buffer is full
bool more_data; bool more_data;
do { do {
more_data = false; more_data = false;
ErrorCode error_code = kErrNone; ErrorCode error_code = kErrNone;
mavlink_file_transfer_protocol_t ftp_msg; mavlink_file_transfer_protocol_t ftp_msg;
PayloadHeader* payload = reinterpret_cast<PayloadHeader *>(&ftp_msg.payload[0]); PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_msg.payload[0]);
payload->seq_number = _session_info.stream_seq_number; payload->seq_number = _session_info.stream_seq_number;
payload->session = 0; payload->session = 0;
payload->opcode = kRspAck; payload->opcode = kRspAck;
@@ -878,6 +920,7 @@ void MavlinkFTP::send(const hrt_abstime t)
#ifdef MAVLINK_FTP_DEBUG #ifdef MAVLINK_FTP_DEBUG
warnx("stream send: offset %d", _session_info.stream_offset); warnx("stream send: offset %d", _session_info.stream_offset);
#endif #endif
// We have to test seek past EOF ourselves, lseek will allow seek past EOF // We have to test seek past EOF ourselves, lseek will allow seek past EOF
if (_session_info.stream_offset >= _session_info.file_size) { if (_session_info.stream_offset >= _session_info.file_size) {
error_code = kErrEOF; error_code = kErrEOF;
@@ -885,7 +928,7 @@ void MavlinkFTP::send(const hrt_abstime t)
warnx("stream download: sending Nak EOF"); warnx("stream download: sending Nak EOF");
#endif #endif
} }
if (error_code == kErrNone) { if (error_code == kErrNone) {
if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) { if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) {
error_code = kErrFailErrno; error_code = kErrFailErrno;
@@ -894,43 +937,51 @@ void MavlinkFTP::send(const hrt_abstime t)
#endif #endif
} }
} }
if (error_code == kErrNone) { if (error_code == kErrNone) {
int bytes_read = ::read(_session_info.fd, &payload->data[0], kMaxDataLength); int bytes_read = ::read(_session_info.fd, &payload->data[0], kMaxDataLength);
if (bytes_read < 0) { if (bytes_read < 0) {
// Negative return indicates error other than eof // Negative return indicates error other than eof
error_code = kErrFailErrno; error_code = kErrFailErrno;
#ifdef MAVLINK_FTP_DEBUG #ifdef MAVLINK_FTP_DEBUG
warnx("stream download: read fail"); warnx("stream download: read fail");
#endif #endif
} else { } else {
payload->size = bytes_read; payload->size = bytes_read;
_session_info.stream_offset += bytes_read; _session_info.stream_offset += bytes_read;
_session_info.stream_chunk_transmitted += bytes_read; _session_info.stream_chunk_transmitted += bytes_read;
} }
} }
if (error_code != kErrNone) { if (error_code != kErrNone) {
payload->opcode = kRspNak; payload->opcode = kRspNak;
payload->size = 1; payload->size = 1;
uint8_t* pData = &payload->data[0]; uint8_t *pData = &payload->data[0];
*pData = error_code; // Straight reference to data[0] is causing bogus gcc array subscript error *pData = error_code; // Straight reference to data[0] is causing bogus gcc array subscript error
if (error_code == kErrFailErrno) { if (error_code == kErrFailErrno) {
int r_errno = errno; int r_errno = errno;
payload->size = 2; payload->size = 2;
payload->data[1] = r_errno; payload->data[1] = r_errno;
} }
_session_info.stream_download = false; _session_info.stream_download = false;
} else { } else {
#ifndef MAVLINK_FTP_UNIT_TEST #ifndef MAVLINK_FTP_UNIT_TEST
if (max_bytes_to_send < (get_size()*2)) {
if (max_bytes_to_send < (get_size() * 2)) {
more_data = false; more_data = false;
/* perform transfers in 35K chunks - this is determined empirical */ /* perform transfers in 35K chunks - this is determined empirical */
if (_session_info.stream_chunk_transmitted > 35000) { if (_session_info.stream_chunk_transmitted > 35000) {
payload->burst_complete = true; payload->burst_complete = true;
_session_info.stream_download = false; _session_info.stream_download = false;
_session_info.stream_chunk_transmitted = 0; _session_info.stream_chunk_transmitted = 0;
} }
} else { } else {
#endif #endif
more_data = true; more_data = true;
@@ -938,9 +989,10 @@ void MavlinkFTP::send(const hrt_abstime t)
#ifndef MAVLINK_FTP_UNIT_TEST #ifndef MAVLINK_FTP_UNIT_TEST
max_bytes_to_send -= get_size(); max_bytes_to_send -= get_size();
} }
#endif #endif
} }
ftp_msg.target_system = _session_info.stream_target_system_id; ftp_msg.target_system = _session_info.stream_target_system_id;
_reply(&ftp_msg); _reply(&ftp_msg);
} while (more_data); } while (more_data);
src/modules/mavlink/mavlink_ftp.h
+30 -33
View File
@@ -35,7 +35,7 @@
/// @file mavlink_ftp.h /// @file mavlink_ftp.h
/// @author px4dev, Don Gagne <don@thegagnes.com> /// @author px4dev, Don Gagne <don@thegagnes.com>
#include <dirent.h> #include <dirent.h>
#include <queue.h> #include <queue.h>
@@ -55,12 +55,12 @@ public:
~MavlinkFTP(); ~MavlinkFTP();
static MavlinkStream *new_instance(Mavlink *mavlink); static MavlinkStream *new_instance(Mavlink *mavlink);
/// Handle possible FTP message /// Handle possible FTP message
void handle_message(const mavlink_message_t *msg); void handle_message(const mavlink_message_t *msg);
typedef void (*ReceiveMessageFunc_t)(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data); typedef void (*ReceiveMessageFunc_t)(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data);
/// @brief Sets up the server to run in unit test mode. /// @brief Sets up the server to run in unit test mode.
/// @param rcvmsgFunc Function which will be called to handle outgoing mavlink messages. /// @param rcvmsgFunc Function which will be called to handle outgoing mavlink messages.
/// @param worker_data Data to pass to worker /// @param worker_data Data to pass to worker
@@ -68,8 +68,7 @@ public:
/// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. We pad the structure ourselves to /// @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. /// 32 bit alignment to avoid usage of any pack pragmas.
struct PayloadHeader struct PayloadHeader {
{
uint16_t seq_number; ///< sequence number for message uint16_t seq_number; ///< sequence number for message
uint8_t session; ///< Session id for read and write commands uint8_t session; ///< Session id for read and write commands
uint8_t opcode; ///< Command opcode uint8_t opcode; ///< Command opcode
@@ -79,11 +78,10 @@ public:
uint8_t padding; ///< 32 bit aligment padding uint8_t padding; ///< 32 bit aligment padding
uint32_t offset; ///< Offsets for List and Read commands uint32_t offset; ///< Offsets for List and Read commands
uint8_t data[]; ///< command data, varies by Opcode uint8_t data[]; ///< command data, varies by Opcode
}; };
/// @brief Command opcodes /// @brief Command opcodes
enum Opcode : uint8_t enum Opcode : uint8_t {
{
kCmdNone, ///< ignored, always acked kCmdNone, ///< ignored, always acked
kCmdTerminateSession, ///< Terminates open Read session kCmdTerminateSession, ///< Terminates open Read session
kCmdResetSessions, ///< Terminates all open Read sessions kCmdResetSessions, ///< Terminates all open Read sessions
@@ -100,14 +98,13 @@ public:
kCmdRename, ///< Rename <path1> to <path2> kCmdRename, ///< Rename <path1> to <path2>
kCmdCalcFileCRC32, ///< Calculate CRC32 for file at <path> kCmdCalcFileCRC32, ///< Calculate CRC32 for file at <path>
kCmdBurstReadFile, ///< Burst download session file kCmdBurstReadFile, ///< Burst download session file
kRspAck = 128, ///< Ack response kRspAck = 128, ///< Ack response
kRspNak ///< Nak response kRspNak ///< Nak response
}; };
/// @brief Error codes returned in Nak response PayloadHeader.data[0]. /// @brief Error codes returned in Nak response PayloadHeader.data[0].
enum ErrorCode : uint8_t enum ErrorCode : uint8_t {
{
kErrNone, kErrNone,
kErrFail, ///< Unknown failure kErrFail, ///< Unknown failure
kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1] kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1]
@@ -116,48 +113,48 @@ public:
kErrNoSessionsAvailable, ///< All available Sessions in use kErrNoSessionsAvailable, ///< All available Sessions in use
kErrEOF, ///< Offset past end of file for List and Read commands kErrEOF, ///< Offset past end of file for List and Read commands
kErrUnknownCommand ///< Unknown command opcode kErrUnknownCommand ///< Unknown command opcode
}; };
// MavlinkStream overrides // MavlinkStream overrides
virtual const char *get_name(void) const; virtual const char *get_name(void) const;
virtual uint8_t get_id(void); virtual uint8_t get_id(void);
virtual unsigned get_size(void); virtual unsigned get_size(void);
private: private:
char *_data_as_cstring(PayloadHeader* payload); char *_data_as_cstring(PayloadHeader *payload);
void _process_request(mavlink_file_transfer_protocol_t* ftp_req, uint8_t target_system_id); void _process_request(mavlink_file_transfer_protocol_t *ftp_req, uint8_t target_system_id);
void _reply(mavlink_file_transfer_protocol_t* ftp_req); void _reply(mavlink_file_transfer_protocol_t *ftp_req);
int _copy_file(const char *src_path, const char *dst_path, size_t length); int _copy_file(const char *src_path, const char *dst_path, size_t length);
ErrorCode _workList(PayloadHeader *payload, bool list_hidden = false); ErrorCode _workList(PayloadHeader *payload, bool list_hidden = false);
ErrorCode _workOpen(PayloadHeader *payload, int oflag); ErrorCode _workOpen(PayloadHeader *payload, int oflag);
ErrorCode _workRead(PayloadHeader *payload); ErrorCode _workRead(PayloadHeader *payload);
ErrorCode _workBurst(PayloadHeader* payload, uint8_t target_system_id); ErrorCode _workBurst(PayloadHeader *payload, uint8_t target_system_id);
ErrorCode _workWrite(PayloadHeader *payload); ErrorCode _workWrite(PayloadHeader *payload);
ErrorCode _workTerminate(PayloadHeader *payload); ErrorCode _workTerminate(PayloadHeader *payload);
ErrorCode _workReset(PayloadHeader* payload); ErrorCode _workReset(PayloadHeader *payload);
ErrorCode _workRemoveDirectory(PayloadHeader *payload); ErrorCode _workRemoveDirectory(PayloadHeader *payload);
ErrorCode _workCreateDirectory(PayloadHeader *payload); ErrorCode _workCreateDirectory(PayloadHeader *payload);
ErrorCode _workRemoveFile(PayloadHeader *payload); ErrorCode _workRemoveFile(PayloadHeader *payload);
ErrorCode _workTruncateFile(PayloadHeader *payload); ErrorCode _workTruncateFile(PayloadHeader *payload);
ErrorCode _workRename(PayloadHeader *payload); ErrorCode _workRename(PayloadHeader *payload);
ErrorCode _workCalcFileCRC32(PayloadHeader *payload); ErrorCode _workCalcFileCRC32(PayloadHeader *payload);
uint8_t _getServerSystemId(void); uint8_t _getServerSystemId(void);
uint8_t _getServerComponentId(void); uint8_t _getServerComponentId(void);
uint8_t _getServerChannel(void); uint8_t _getServerChannel(void);
// Overrides from MavlinkStream // Overrides from MavlinkStream
virtual void send(const hrt_abstime t); virtual void send(const hrt_abstime t);
static const char kDirentFile = 'F'; ///< Identifies File returned from List command 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 kDirentDir = 'D'; ///< Identifies Directory returned from List command
static const char kDirentSkip = 'S'; ///< Identifies Skipped entry from List command static const char kDirentSkip = 'S'; ///< Identifies Skipped entry from List command
/// @brief Maximum data size in RequestHeader::data /// @brief Maximum data size in RequestHeader::data
static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader); static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader);
struct SessionInfo { struct SessionInfo {
int fd; int fd;
uint32_t file_size; uint32_t file_size;
@@ -168,15 +165,15 @@ private:
unsigned stream_chunk_transmitted; unsigned stream_chunk_transmitted;
}; };
struct SessionInfo _session_info; ///< Session info, fd=-1 for no active session struct SessionInfo _session_info; ///< Session info, fd=-1 for no active session
ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending
void *_worker_data; ///< Additional parameter to _utRcvMsgFunc; void *_worker_data; ///< Additional parameter to _utRcvMsgFunc;
/* do not allow copying this class */ /* do not allow copying this class */
MavlinkFTP(const MavlinkFTP&); MavlinkFTP(const MavlinkFTP &);
MavlinkFTP operator=(const MavlinkFTP&); MavlinkFTP operator=(const MavlinkFTP &);
// Mavlink test needs to be able to call send // Mavlink test needs to be able to call send
friend class MavlinkFtpTest; friend class MavlinkFtpTest;
}; };
src/modules/mavlink/mavlink_log_handler.cpp
+144 -57
View File
@@ -64,13 +64,18 @@ static const char *kTmpData = MOUNTPOINT "/$log$.txt";
//------------------------------------------------------------------- //-------------------------------------------------------------------
static bool static bool
stat_file(const char* file, time_t* date = 0, uint32_t* size = 0) { stat_file(const char *file, time_t *date = 0, uint32_t *size = 0)
{
struct stat st; struct stat st;
if (stat(file, &st) == 0) { if (stat(file, &st) == 0) {
if (date) *date = st.st_mtime; if (date) { *date = st.st_mtime; }
if (size) *size = st.st_size;
if (size) { *size = st.st_size; }
return true; return true;
} }
return false; return false;
} }
@@ -97,12 +102,15 @@ MavlinkLogHandler::handle_message(const mavlink_message_t *msg)
case MAVLINK_MSG_ID_LOG_REQUEST_LIST: case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
_log_request_list(msg); _log_request_list(msg);
break; break;
case MAVLINK_MSG_ID_LOG_REQUEST_DATA: case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
_log_request_data(msg); _log_request_data(msg);
break; break;
case MAVLINK_MSG_ID_LOG_ERASE: case MAVLINK_MSG_ID_LOG_ERASE:
_log_request_erase(msg); _log_request_erase(msg);
break; break;
case MAVLINK_MSG_ID_LOG_REQUEST_END: case MAVLINK_MSG_ID_LOG_REQUEST_END:
_log_request_end(msg); _log_request_end(msg);
break; break;
@@ -110,7 +118,7 @@ MavlinkLogHandler::handle_message(const mavlink_message_t *msg)
} }
//------------------------------------------------------------------- //-------------------------------------------------------------------
const char* const char *
MavlinkLogHandler::get_name(void) const MavlinkLogHandler::get_name(void) const
{ {
return "MAVLINK_LOG_HANDLER"; return "MAVLINK_LOG_HANDLER";
@@ -130,10 +138,12 @@ MavlinkLogHandler::get_size(void)
//-- Sending Log Entries //-- Sending Log Entries
if (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_LISTING) { if (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_LISTING) {
return MAVLINK_MSG_ID_LOG_ENTRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; return MAVLINK_MSG_ID_LOG_ENTRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
//-- Sending Log Data (contents of one of the log files) //-- Sending Log Data (contents of one of the log files)
} else if (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_SENDING_DATA) { } else if (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_SENDING_DATA) {
return MAVLINK_MSG_ID_LOG_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; return MAVLINK_MSG_ID_LOG_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
//-- Idle //-- Idle
} else { } else {
return 0; return 0;
} }
@@ -144,14 +154,18 @@ void
MavlinkLogHandler::send(const hrt_abstime /*t*/) MavlinkLogHandler::send(const hrt_abstime /*t*/)
{ {
//-- An arbitrary count of max bytes in one go (one of the two below but never both) //-- An arbitrary count of max bytes in one go (one of the two below but never both)
#define MAX_BYTES_SEND 64 * 1024 #define MAX_BYTES_SEND 64 * 1024
size_t count = 0; size_t count = 0;
//-- Log Entries //-- Log Entries
while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_LISTING && _mavlink->get_free_tx_buf() > get_size() && count < MAX_BYTES_SEND) { while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_LISTING
&& _mavlink->get_free_tx_buf() > get_size() && count < MAX_BYTES_SEND) {
count += _log_send_listing(); count += _log_send_listing();
} }
//-- Log Data //-- Log Data
while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_SENDING_DATA && _mavlink->get_free_tx_buf() > get_size() && count < MAX_BYTES_SEND) { while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_SENDING_DATA
&& _mavlink->get_free_tx_buf() > get_size() && count < MAX_BYTES_SEND) {
count += _log_send_data(); count += _log_send_data();
} }
} }
@@ -162,29 +176,35 @@ MavlinkLogHandler::_log_request_list(const mavlink_message_t *msg)
{ {
mavlink_log_request_list_t request; mavlink_log_request_list_t request;
mavlink_msg_log_request_list_decode(msg, &request); mavlink_msg_log_request_list_decode(msg, &request);
//-- Check for re-requests (data loss) or new request //-- Check for re-requests (data loss) or new request
if(_pLogHandlerHelper) { if (_pLogHandlerHelper) {
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE; _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
//-- Is this a new request? //-- Is this a new request?
if((request.end - request.start) > _pLogHandlerHelper->log_count) { if ((request.end - request.start) > _pLogHandlerHelper->log_count) {
delete _pLogHandlerHelper; delete _pLogHandlerHelper;
_pLogHandlerHelper = NULL; _pLogHandlerHelper = NULL;
} }
} }
if(!_pLogHandlerHelper) {
if (!_pLogHandlerHelper) {
//-- Prepare new request //-- Prepare new request
_pLogHandlerHelper = new LogListHelper; _pLogHandlerHelper = new LogListHelper;
} }
if (_pLogHandlerHelper->log_count)
{ if (_pLogHandlerHelper->log_count) {
//-- Define (and clamp) range //-- Define (and clamp) range
_pLogHandlerHelper->next_entry = request.start < _pLogHandlerHelper->log_count ? request.start : _pLogHandlerHelper->log_count - 1; _pLogHandlerHelper->next_entry = request.start < _pLogHandlerHelper->log_count ? request.start :
_pLogHandlerHelper->last_entry = request.end < _pLogHandlerHelper->log_count ? request.end : _pLogHandlerHelper->log_count - 1; _pLogHandlerHelper->log_count - 1;
_pLogHandlerHelper->last_entry = request.end < _pLogHandlerHelper->log_count ? request.end :
_pLogHandlerHelper->log_count - 1;
} }
PX4LOG_WARN("\nMavlinkLogHandler::_log_request_list: start: %u last: %u count: %u\n", PX4LOG_WARN("\nMavlinkLogHandler::_log_request_list: start: %u last: %u count: %u\n",
_pLogHandlerHelper->next_entry, _pLogHandlerHelper->next_entry,
_pLogHandlerHelper->last_entry, _pLogHandlerHelper->last_entry,
_pLogHandlerHelper->log_count); _pLogHandlerHelper->log_count);
//-- Enable streaming //-- Enable streaming
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_LISTING; _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_LISTING;
} }
@@ -194,44 +214,54 @@ void
MavlinkLogHandler::_log_request_data(const mavlink_message_t *msg) MavlinkLogHandler::_log_request_data(const mavlink_message_t *msg)
{ {
//-- If we haven't listed, we can't do much //-- If we haven't listed, we can't do much
if(!_pLogHandlerHelper) { if (!_pLogHandlerHelper) {
PX4LOG_WARN("MavlinkLogHandler::_log_request_data Log request with no list requested.\n"); PX4LOG_WARN("MavlinkLogHandler::_log_request_data Log request with no list requested.\n");
return; return;
} }
mavlink_log_request_data_t request; mavlink_log_request_data_t request;
mavlink_msg_log_request_data_decode(msg, &request); mavlink_msg_log_request_data_decode(msg, &request);
//-- Does the requested log exist? //-- Does the requested log exist?
if (request.id >= _pLogHandlerHelper->log_count) { if (request.id >= _pLogHandlerHelper->log_count) {
PX4LOG_WARN("MavlinkLogHandler::_log_request_data Requested log %u but we only have %u.\n", request.id, _pLogHandlerHelper->log_count); PX4LOG_WARN("MavlinkLogHandler::_log_request_data Requested log %u but we only have %u.\n", request.id,
_pLogHandlerHelper->log_count);
return; return;
} }
//-- If we were sending log entries, stop it //-- If we were sending log entries, stop it
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE; _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
if (_pLogHandlerHelper->current_log_index != request.id) { if (_pLogHandlerHelper->current_log_index != request.id) {
//-- Init send log dataset //-- Init send log dataset
_pLogHandlerHelper->current_log_filename[0] = 0; _pLogHandlerHelper->current_log_filename[0] = 0;
_pLogHandlerHelper->current_log_index = request.id; _pLogHandlerHelper->current_log_index = request.id;
uint32_t time_utc = 0; uint32_t time_utc = 0;
_pLogHandlerHelper->get_entry(_pLogHandlerHelper->current_log_index, _pLogHandlerHelper->current_log_size, time_utc, _pLogHandlerHelper->current_log_filename); _pLogHandlerHelper->get_entry(_pLogHandlerHelper->current_log_index, _pLogHandlerHelper->current_log_size, time_utc,
_pLogHandlerHelper->current_log_filename);
_pLogHandlerHelper->open_for_transmit(); _pLogHandlerHelper->open_for_transmit();
} }
_pLogHandlerHelper->current_log_data_offset = request.ofs; _pLogHandlerHelper->current_log_data_offset = request.ofs;
if (_pLogHandlerHelper->current_log_data_offset >= _pLogHandlerHelper->current_log_size) { if (_pLogHandlerHelper->current_log_data_offset >= _pLogHandlerHelper->current_log_size) {
_pLogHandlerHelper->current_log_data_remaining = 0; _pLogHandlerHelper->current_log_data_remaining = 0;
} else { } else {
_pLogHandlerHelper->current_log_data_remaining = _pLogHandlerHelper->current_log_size - request.ofs; _pLogHandlerHelper->current_log_data_remaining = _pLogHandlerHelper->current_log_size - request.ofs;
} }
if (_pLogHandlerHelper->current_log_data_remaining > request.count) { if (_pLogHandlerHelper->current_log_data_remaining > request.count) {
_pLogHandlerHelper->current_log_data_remaining = request.count; _pLogHandlerHelper->current_log_data_remaining = request.count;
} }
//-- Enable streaming //-- Enable streaming
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_SENDING_DATA; _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_SENDING_DATA;
} }
//------------------------------------------------------------------- //-------------------------------------------------------------------
void void
MavlinkLogHandler::_log_request_erase(const mavlink_message_t* /*msg*/) MavlinkLogHandler::_log_request_erase(const mavlink_message_t * /*msg*/)
{ {
/* /*
mavlink_log_erase_t request; mavlink_log_erase_t request;
@@ -241,36 +271,43 @@ MavlinkLogHandler::_log_request_erase(const mavlink_message_t* /*msg*/)
delete _pLogHandlerHelper; delete _pLogHandlerHelper;
_pLogHandlerHelper = 0; _pLogHandlerHelper = 0;
} }
//-- Delete all logs //-- Delete all logs
LogListHelper::delete_all(kLogRoot); LogListHelper::delete_all(kLogRoot);
//-- Now delete all "msgs_*" from root //-- Now delete all "msgs_*" from root
DIR* dp = opendir(kSDRoot); DIR *dp = opendir(kSDRoot);
if (dp) { if (dp) {
struct dirent entry, *result = nullptr; struct dirent entry, *result = nullptr;
while (readdir_r(dp, &entry, &result) == 0) { while (readdir_r(dp, &entry, &result) == 0) {
// no more entries? // no more entries?
if (!result) { if (!result) {
break; break;
} }
if (entry.d_type == PX4LOG_REGULAR_FILE) { if (entry.d_type == PX4LOG_REGULAR_FILE) {
if(!memcmp(entry.d_name, "msgs_", 5)) { if (!memcmp(entry.d_name, "msgs_", 5)) {
char msg_path[128]; char msg_path[128];
snprintf(msg_path, sizeof(msg_path), "%s%s", kSDRoot, entry.d_name); snprintf(msg_path, sizeof(msg_path), "%s%s", kSDRoot, entry.d_name);
if(unlink(msg_path)) {
if (unlink(msg_path)) {
PX4LOG_WARN("MavlinkLogHandler::_log_request_erase Error deleting %s\n", msg_path); PX4LOG_WARN("MavlinkLogHandler::_log_request_erase Error deleting %s\n", msg_path);
} }
} }
} }
} }
closedir(dp); closedir(dp);
} }
} }
//------------------------------------------------------------------- //-------------------------------------------------------------------
void void
MavlinkLogHandler::_log_request_end(const mavlink_message_t* /*msg*/) MavlinkLogHandler::_log_request_end(const mavlink_message_t * /*msg*/)
{ {
PX4LOG_WARN("MavlinkLogHandler::_log_request_end\n"); PX4LOG_WARN("MavlinkLogHandler::_log_request_end\n");
if (_pLogHandlerHelper) { if (_pLogHandlerHelper) {
delete _pLogHandlerHelper; delete _pLogHandlerHelper;
_pLogHandlerHelper = 0; _pLogHandlerHelper = 0;
@@ -290,19 +327,22 @@ MavlinkLogHandler::_log_send_listing()
response.num_logs = _pLogHandlerHelper->log_count; response.num_logs = _pLogHandlerHelper->log_count;
response.last_log_num = _pLogHandlerHelper->last_entry; response.last_log_num = _pLogHandlerHelper->last_entry;
mavlink_msg_log_entry_send_struct(_mavlink->get_channel(), &response); mavlink_msg_log_entry_send_struct(_mavlink->get_channel(), &response);
//-- If we're done listing, flag it. //-- If we're done listing, flag it.
if (_pLogHandlerHelper->next_entry == _pLogHandlerHelper->last_entry) { if (_pLogHandlerHelper->next_entry == _pLogHandlerHelper->last_entry) {
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE; _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
} else { } else {
_pLogHandlerHelper->next_entry++; _pLogHandlerHelper->next_entry++;
} }
PX4LOG_WARN("MavlinkLogHandler::_log_send_listing id: %u count: %u last: %u size: %u date: %u status: %d\n", PX4LOG_WARN("MavlinkLogHandler::_log_send_listing id: %u count: %u last: %u size: %u date: %u status: %d\n",
response.id, response.id,
response.num_logs, response.num_logs,
response.last_log_num, response.last_log_num,
response.size, response.size,
response.time_utc, response.time_utc,
_pLogHandlerHelper->current_status); _pLogHandlerHelper->current_status);
return sizeof(response); return sizeof(response);
} }
@@ -313,9 +353,11 @@ MavlinkLogHandler::_log_send_data()
mavlink_log_data_t response; mavlink_log_data_t response;
memset(&response, 0, sizeof(response)); memset(&response, 0, sizeof(response));
uint32_t len = _pLogHandlerHelper->current_log_data_remaining; uint32_t len = _pLogHandlerHelper->current_log_data_remaining;
if (len > sizeof(response.data)) { if (len > sizeof(response.data)) {
len = sizeof(response.data); len = sizeof(response.data);
} }
size_t read_size = _pLogHandlerHelper->get_log_data(len, response.data); size_t read_size = _pLogHandlerHelper->get_log_data(len, response.data);
response.ofs = _pLogHandlerHelper->current_log_data_offset; response.ofs = _pLogHandlerHelper->current_log_data_offset;
response.id = _pLogHandlerHelper->current_log_index; response.id = _pLogHandlerHelper->current_log_index;
@@ -323,9 +365,11 @@ MavlinkLogHandler::_log_send_data()
mavlink_msg_log_data_send_struct(_mavlink->get_channel(), &response); mavlink_msg_log_data_send_struct(_mavlink->get_channel(), &response);
_pLogHandlerHelper->current_log_data_offset += read_size; _pLogHandlerHelper->current_log_data_offset += read_size;
_pLogHandlerHelper->current_log_data_remaining -= read_size; _pLogHandlerHelper->current_log_data_remaining -= read_size;
if (read_size < sizeof(response.data) || _pLogHandlerHelper->current_log_data_remaining == 0) { if (read_size < sizeof(response.data) || _pLogHandlerHelper->current_log_data_remaining == 0) {
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE; _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
} }
return sizeof(response); return sizeof(response);
} }
@@ -354,33 +398,39 @@ LogListHelper::~LogListHelper()
//------------------------------------------------------------------- //-------------------------------------------------------------------
bool bool
LogListHelper::get_entry(int idx, uint32_t& size, uint32_t& date, char* filename) LogListHelper::get_entry(int idx, uint32_t &size, uint32_t &date, char *filename)
{ {
//-- Find log file in log list file created during init() //-- Find log file in log list file created during init()
size = 0; size = 0;
date = 0; date = 0;
bool result = false; bool result = false;
//-- Open list of log files //-- Open list of log files
FILE* f = ::fopen(kLogData, "r"); FILE *f = ::fopen(kLogData, "r");
if (f) { if (f) {
//--- Find requested entry //--- Find requested entry
char line[160]; char line[160];
int count = 0; int count = 0;
while (fgets(line, sizeof(line), f)) { while (fgets(line, sizeof(line), f)) {
//-- Found our "index" //-- Found our "index"
if(count++ == idx) { if (count++ == idx) {
char file[128]; char file[128];
if(sscanf(line, "%u %u %s", &date, &size, file) == 3) {
if(filename) { if (sscanf(line, "%u %u %s", &date, &size, file) == 3) {
if (filename) {
strcpy(filename, file); strcpy(filename, file);
} }
result = true; result = true;
break; break;
} }
} }
} }
fclose(f); fclose(f);
} }
return result; return result;
} }
@@ -392,30 +442,38 @@ LogListHelper::open_for_transmit()
::fclose(current_log_filep); ::fclose(current_log_filep);
current_log_filep = 0; current_log_filep = 0;
} }
current_log_filep = ::fopen(current_log_filename, "rb"); current_log_filep = ::fopen(current_log_filename, "rb");
if (!current_log_filep) { if (!current_log_filep) {
PX4LOG_WARN("MavlinkLogHandler::open_for_transmit Could not open %s\n", current_log_filename); PX4LOG_WARN("MavlinkLogHandler::open_for_transmit Could not open %s\n", current_log_filename);
return false; return false;
} }
return true; return true;
} }
//------------------------------------------------------------------- //-------------------------------------------------------------------
size_t size_t
LogListHelper::get_log_data(uint8_t len, uint8_t* buffer) LogListHelper::get_log_data(uint8_t len, uint8_t *buffer)
{ {
if(!current_log_filename[0]) if (!current_log_filename[0]) {
return 0; return 0;
}
if (!current_log_filep) { if (!current_log_filep) {
PX4LOG_WARN("MavlinkLogHandler::get_log_data file not open %s\n", current_log_filename); PX4LOG_WARN("MavlinkLogHandler::get_log_data file not open %s\n", current_log_filename);
return 0; return 0;
} }
long int offset = current_log_data_offset - ftell(current_log_filep); long int offset = current_log_data_offset - ftell(current_log_filep);
if(offset && fseek(current_log_filep, offset, SEEK_CUR)) {
if (offset && fseek(current_log_filep, offset, SEEK_CUR)) {
fclose(current_log_filep); fclose(current_log_filep);
PX4LOG_WARN("MavlinkLogHandler::get_log_data Seek error in %s\n", current_log_filename); PX4LOG_WARN("MavlinkLogHandler::get_log_data Seek error in %s\n", current_log_filename);
return 0; return 0;
} }
size_t result = fread(buffer, 1, len, current_log_filep); size_t result = fread(buffer, 1, len, current_log_filep);
return result; return result;
} }
@@ -436,36 +494,44 @@ LogListHelper::_init()
unlink(kLogData); unlink(kLogData);
// Open log directory // Open log directory
DIR *dp = opendir(kLogRoot); DIR *dp = opendir(kLogRoot);
if (dp == nullptr) { if (dp == nullptr) {
// No log directory. Nothing to do. // No log directory. Nothing to do.
return; return;
} }
// Create work file // Create work file
FILE* f = ::fopen(kTmpData, "w"); FILE *f = ::fopen(kTmpData, "w");
if (!f) { if (!f) {
PX4LOG_WARN("MavlinkLogHandler::init Error creating %s\n", kTmpData); PX4LOG_WARN("MavlinkLogHandler::init Error creating %s\n", kTmpData);
closedir(dp); closedir(dp);
return; return;
} }
// Scan directory and collect log files // Scan directory and collect log files
struct dirent entry, *result = nullptr; struct dirent entry, *result = nullptr;
while (readdir_r(dp, &entry, &result) == 0) { while (readdir_r(dp, &entry, &result) == 0) {
// no more entries? // no more entries?
if (result == nullptr) { if (result == nullptr) {
break; break;
} }
if (entry.d_type == PX4LOG_DIRECTORY)
{ if (entry.d_type == PX4LOG_DIRECTORY) {
time_t tt = 0; time_t tt = 0;
char log_path[128]; char log_path[128];
snprintf(log_path, sizeof(log_path), "%s/%s", kLogRoot, entry.d_name); snprintf(log_path, sizeof(log_path), "%s/%s", kLogRoot, entry.d_name);
if (_get_session_date(log_path, entry.d_name, tt)) { if (_get_session_date(log_path, entry.d_name, tt)) {
_scan_logs(f, log_path, tt); _scan_logs(f, log_path, tt);
} }
} }
} }
closedir(dp); closedir(dp);
fclose(f); fclose(f);
// Rename temp file to data file // Rename temp file to data file
if (rename(kTmpData, kLogData)) { if (rename(kTmpData, kLogData)) {
PX4LOG_WARN("MavlinkLogHandler::init Error renaming %s\n", kTmpData); PX4LOG_WARN("MavlinkLogHandler::init Error renaming %s\n", kTmpData);
@@ -475,20 +541,23 @@ LogListHelper::_init()
//------------------------------------------------------------------- //-------------------------------------------------------------------
bool bool
LogListHelper::_get_session_date(const char* path, const char* dir, time_t& date) LogListHelper::_get_session_date(const char *path, const char *dir, time_t &date)
{ {
if(strlen(dir) > 4) { if (strlen(dir) > 4) {
// Convert "sess000" to 00:00 Jan 1 1970 (day per session) // Convert "sess000" to 00:00 Jan 1 1970 (day per session)
if (strncmp(dir, "sess", 4) == 0) { if (strncmp(dir, "sess", 4) == 0) {
unsigned u; unsigned u;
if(sscanf(&dir[4], "%u", &u) == 1) {
if (sscanf(&dir[4], "%u", &u) == 1) {
date = u * 60 * 60 * 24; date = u * 60 * 60 * 24;
return true; return true;
} }
} else { } else {
if (stat_file(path, &date)) { if (stat_file(path, &date)) {
return true; return true;
} }
/* strptime not available for some reason /* strptime not available for some reason
// Get date from directory name // Get date from directory name
struct tm tt; struct tm tt;
@@ -499,27 +568,32 @@ LogListHelper::_get_session_date(const char* path, const char* dir, time_t& date
*/ */
} }
} }
return false; return false;
} }
//------------------------------------------------------------------- //-------------------------------------------------------------------
void void
LogListHelper::_scan_logs(FILE* f, const char* dir, time_t& date) LogListHelper::_scan_logs(FILE *f, const char *dir, time_t &date)
{ {
DIR *dp = opendir(dir); DIR *dp = opendir(dir);
if (dp) { if (dp) {
struct dirent entry, *result = nullptr; struct dirent entry, *result = nullptr;
while (readdir_r(dp, &entry, &result) == 0) { while (readdir_r(dp, &entry, &result) == 0) {
// no more entries? // no more entries?
if (result == nullptr) { if (result == nullptr) {
break; break;
} }
if (entry.d_type == PX4LOG_REGULAR_FILE) { if (entry.d_type == PX4LOG_REGULAR_FILE) {
time_t ldate = date; time_t ldate = date;
uint32_t size = 0; uint32_t size = 0;
char log_file_path[128]; char log_file_path[128];
snprintf(log_file_path, sizeof(log_file_path), "%s/%s", dir, entry.d_name); snprintf(log_file_path, sizeof(log_file_path), "%s/%s", dir, entry.d_name);
if(_get_log_time_size(log_file_path, entry.d_name, ldate, size)) {
if (_get_log_time_size(log_file_path, entry.d_name, ldate, size)) {
//-- Write entry out to list file //-- Write entry out to list file
fprintf(f, "%u %u %s\n", (unsigned)ldate, (unsigned)size, log_file_path); fprintf(f, "%u %u %s\n", (unsigned)ldate, (unsigned)size, log_file_path);
log_count++; log_count++;
@@ -531,23 +605,27 @@ LogListHelper::_scan_logs(FILE* f, const char* dir, time_t& date)
//------------------------------------------------------------------- //-------------------------------------------------------------------
bool bool
LogListHelper::_get_log_time_size(const char* path, const char* file, time_t& date, uint32_t& size) LogListHelper::_get_log_time_size(const char *path, const char *file, time_t &date, uint32_t &size)
{ {
if(file && file[0]) { if (file && file[0]) {
if(strstr(file, ".px4log") || strstr(file, ".ulg")) { if (strstr(file, ".px4log") || strstr(file, ".ulg")) {
// Convert "log000" to 00:00 (minute per flight in session) // Convert "log000" to 00:00 (minute per flight in session)
if (strncmp(file, "log", 3) == 0) { if (strncmp(file, "log", 3) == 0) {
unsigned u; unsigned u;
if(sscanf(&file[3], "%u", &u) == 1) {
if (sscanf(&file[3], "%u", &u) == 1) {
date += (u * 60); date += (u * 60);
if (stat_file(path, 0, &size)) { if (stat_file(path, 0, &size)) {
return true; return true;
} }
} }
} else { } else {
if (stat_file(path, &date, &size)) { if (stat_file(path, &date, &size)) {
return true; return true;
} }
/* strptime not available for some reason /* strptime not available for some reason
// Get time from file name // Get time from file name
struct tm tt; struct tm tt;
@@ -559,39 +637,48 @@ LogListHelper::_get_log_time_size(const char* path, const char* file, time_t& da
} }
} }
} }
return false; return false;
} }
//------------------------------------------------------------------- //-------------------------------------------------------------------
void void
LogListHelper::delete_all(const char* dir) LogListHelper::delete_all(const char *dir)
{ {
//-- Open log directory //-- Open log directory
DIR* dp = opendir(dir); DIR *dp = opendir(dir);
if (dp == nullptr) { if (dp == nullptr) {
return; return;
} }
struct dirent entry, *result = nullptr; struct dirent entry, *result = nullptr;
while (readdir_r(dp, &entry, &result) == 0) { while (readdir_r(dp, &entry, &result) == 0) {
// no more entries? // no more entries?
if (result == nullptr) { if (result == nullptr) {
break; break;
} }
if (entry.d_type == PX4LOG_DIRECTORY && entry.d_name[0] != '.') { if (entry.d_type == PX4LOG_DIRECTORY && entry.d_name[0] != '.') {
char log_path[128]; char log_path[128];
snprintf(log_path, sizeof(log_path), "%s/%s", dir, entry.d_name); snprintf(log_path, sizeof(log_path), "%s/%s", dir, entry.d_name);
LogListHelper::delete_all(log_path); LogListHelper::delete_all(log_path);
if(rmdir(log_path)) {
if (rmdir(log_path)) {
PX4LOG_WARN("MavlinkLogHandler::delete_all Error removing %s\n", log_path); PX4LOG_WARN("MavlinkLogHandler::delete_all Error removing %s\n", log_path);
} }
} }
if (entry.d_type == PX4LOG_REGULAR_FILE) { if (entry.d_type == PX4LOG_REGULAR_FILE) {
char log_path[128]; char log_path[128];
snprintf(log_path, sizeof(log_path), "%s/%s", dir, entry.d_name); snprintf(log_path, sizeof(log_path), "%s/%s", dir, entry.d_name);
if(unlink(log_path)) {
if (unlink(log_path)) {
PX4LOG_WARN("MavlinkLogHandler::delete_all Error deleting %s\n", log_path); PX4LOG_WARN("MavlinkLogHandler::delete_all Error deleting %s\n", log_path);
} }
} }
} }
closedir(dp); closedir(dp);
} }
src/modules/mavlink/mavlink_log_handler.h
+19 -19
View File
@@ -53,13 +53,13 @@ public:
~LogListHelper(); ~LogListHelper();
public: public:
static void delete_all(const char* dir); static void delete_all(const char *dir);
public: public:
bool get_entry (int idx, uint32_t& size, uint32_t& date, char* filename = 0); bool get_entry(int idx, uint32_t &size, uint32_t &date, char *filename = 0);
bool open_for_transmit(); bool open_for_transmit();
size_t get_log_data (uint8_t len, uint8_t* buffer); size_t get_log_data(uint8_t len, uint8_t *buffer);
enum { enum {
LOG_HANDLER_IDLE, LOG_HANDLER_IDLE,
@@ -76,14 +76,14 @@ public:
uint32_t current_log_size; uint32_t current_log_size;
uint32_t current_log_data_offset; uint32_t current_log_data_offset;
uint32_t current_log_data_remaining; uint32_t current_log_data_remaining;
FILE* current_log_filep; FILE *current_log_filep;
char current_log_filename[128]; char current_log_filename[128];
private: private:
void _init (); void _init();
bool _get_session_date (const char* path, const char* dir, time_t& date); bool _get_session_date(const char *path, const char *dir, time_t &date);
void _scan_logs (FILE* f, const char* dir, time_t& date); void _scan_logs(FILE *f, const char *dir, time_t &date);
bool _get_log_time_size (const char* path, const char* file, time_t& date, uint32_t& size); bool _get_log_time_size(const char *path, const char *file, time_t &date, uint32_t &size);
}; };
// MAVLink LOG_* Message Handler // MAVLink LOG_* Message Handler
@@ -95,23 +95,23 @@ public:
static MavlinkLogHandler *new_instance(Mavlink *mavlink); static MavlinkLogHandler *new_instance(Mavlink *mavlink);
// Handle possible LOG message // Handle possible LOG message
void handle_message (const mavlink_message_t *msg); void handle_message(const mavlink_message_t *msg);
// Overrides from MavlinkStream // Overrides from MavlinkStream
const char* get_name (void) const; const char *get_name(void) const;
uint8_t get_id (void); uint8_t get_id(void);
unsigned get_size (void); unsigned get_size(void);
void send (const hrt_abstime t); void send(const hrt_abstime t);
private: private:
void _log_message (const mavlink_message_t *msg); void _log_message(const mavlink_message_t *msg);
void _log_request_list (const mavlink_message_t *msg); void _log_request_list(const mavlink_message_t *msg);
void _log_request_data (const mavlink_message_t *msg); void _log_request_data(const mavlink_message_t *msg);
void _log_request_erase (const mavlink_message_t *msg); void _log_request_erase(const mavlink_message_t *msg);
void _log_request_end (const mavlink_message_t *msg); void _log_request_end(const mavlink_message_t *msg);
size_t _log_send_listing(); size_t _log_send_listing();
size_t _log_send_data (); size_t _log_send_data();
private: private:
LogListHelper *_pLogHandlerHelper; LogListHelper *_pLogHandlerHelper;
src/modules/mavlink/mavlink_main.cpp
+94 -37
View File
@@ -117,7 +117,8 @@ extern mavlink_system_t mavlink_system;
void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length) void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length)
{ {
Mavlink* m = Mavlink::get_instance((unsigned)chan); Mavlink *m = Mavlink::get_instance((unsigned)chan);
if (m != nullptr) { if (m != nullptr) {
m->send_bytes(ch, length); m->send_bytes(ch, length);
} }
@@ -125,7 +126,8 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int leng
void mavlink_end_uart_send(mavlink_channel_t chan, int length) void mavlink_end_uart_send(mavlink_channel_t chan, int length)
{ {
Mavlink* m = Mavlink::get_instance((unsigned)chan); Mavlink *m = Mavlink::get_instance((unsigned)chan);
if (m != nullptr) { if (m != nullptr) {
(void)m->send_packet(); (void)m->send_packet();
} }
@@ -136,9 +138,11 @@ void mavlink_end_uart_send(mavlink_channel_t chan, int length)
*/ */
mavlink_status_t *mavlink_get_channel_status(uint8_t channel) mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
{ {
Mavlink* m = Mavlink::get_instance((unsigned)channel); Mavlink *m = Mavlink::get_instance((unsigned)channel);
if (m != nullptr) { if (m != nullptr) {
return m->get_status(); return m->get_status();
} else { } else {
return nullptr; return nullptr;
} }
@@ -149,9 +153,11 @@ mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
*/ */
mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel) mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
{ {
Mavlink* m = Mavlink::get_instance((unsigned)channel); Mavlink *m = Mavlink::get_instance((unsigned)channel);
if (m != nullptr) { if (m != nullptr) {
return m->get_buffer(); return m->get_buffer();
} else { } else {
return nullptr; return nullptr;
} }
@@ -219,7 +225,7 @@ Mavlink::Mavlink() :
_rate_txerr(0.0f), _rate_txerr(0.0f),
_rate_rx(0.0f), _rate_rx(0.0f),
#ifdef __PX4_POSIX #ifdef __PX4_POSIX
_myaddr{}, _myaddr {},
_src_addr{}, _src_addr{},
_bcast_addr{}, _bcast_addr{},
_src_addr_initialized(false), _src_addr_initialized(false),
@@ -333,6 +339,7 @@ Mavlink::set_proto_version(unsigned version)
if (version == 1 || ((version == 0) && !_received_messages)) { if (version == 1 || ((version == 0) && !_received_messages)) {
get_status()->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; get_status()->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
} else if (version == 2) { } else if (version == 2) {
get_status()->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1); get_status()->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
} }
@@ -440,7 +447,7 @@ Mavlink::destroy_all_instances()
while (_mavlink_instances) { while (_mavlink_instances) {
inst_to_del = _mavlink_instances; inst_to_del = _mavlink_instances;
LL_DELETE(_mavlink_instances, inst_to_del); LL_DELETE(_mavlink_instances, inst_to_del);
delete (inst_to_del); delete(inst_to_del);
} }
printf("\n"); printf("\n");
@@ -621,11 +628,11 @@ int Mavlink::get_component_id()
int Mavlink::mavlink_open_uart(int baud, const char *uart_name) int Mavlink::mavlink_open_uart(int baud, const char *uart_name)
{ {
#ifndef B460800 #ifndef B460800
#define B460800 460800 #define B460800 460800
#endif #endif
#ifndef B921600 #ifndef B921600
#define B921600 921600 #define B921600 921600
#endif #endif
/* process baud rate */ /* process baud rate */
@@ -682,7 +689,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name)
/* back off 1800 ms to avoid running into the USB setup timing */ /* back off 1800 ms to avoid running into the USB setup timing */
while (_mode == MAVLINK_MODE_CONFIG && while (_mode == MAVLINK_MODE_CONFIG &&
hrt_absolute_time() < 1800U * 1000U) { hrt_absolute_time() < 1800U * 1000U) {
usleep(50000); usleep(50000);
} }
@@ -856,15 +863,16 @@ Mavlink::get_free_tx_buf()
int buf_free = 0; int buf_free = 0;
// if we are using network sockets, return max length of one packet // if we are using network sockets, return max length of one packet
if (get_protocol() == UDP || get_protocol() == TCP ) { if (get_protocol() == UDP || get_protocol() == TCP) {
return 1500; return 1500;
} else { } else {
// No FIONWRITE on Linux // No FIONWRITE on Linux
#if !defined(__PX4_LINUX) && !defined(__PX4_DARWIN) #if !defined(__PX4_LINUX) && !defined(__PX4_DARWIN)
(void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
#else #else
//Linux cp210x does not support TIOCOUTQ //Linux cp210x does not support TIOCOUTQ
buf_free = 256; buf_free = 256;
#endif #endif
if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) { if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) {
@@ -907,7 +915,7 @@ Mavlink::send_packet()
/* resend message via broadcast if no valid connection exists */ /* resend message via broadcast if no valid connection exists */
if ((_mode != MAVLINK_MODE_ONBOARD) && broadcast_enabled() && if ((_mode != MAVLINK_MODE_ONBOARD) && broadcast_enabled() &&
(!get_client_source_initialized() (!get_client_source_initialized()
|| (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) { || (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) {
if (!_broadcast_address_found) { if (!_broadcast_address_found) {
find_broadcast_address(); find_broadcast_address();
@@ -923,6 +931,7 @@ Mavlink::send_packet()
PX4_ERR("sending broadcast failed, errno: %d: %s", errno, strerror(errno)); PX4_ERR("sending broadcast failed, errno: %d: %s", errno, strerror(errno));
_broadcast_failed_warned = true; _broadcast_failed_warned = true;
} }
} else { } else {
_broadcast_failed_warned = false; _broadcast_failed_warned = false;
} }
@@ -960,8 +969,9 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
if (get_protocol() == SERIAL) { if (get_protocol() == SERIAL) {
/* check if there is space in the buffer, let it overflow else */ /* check if there is space in the buffer, let it overflow else */
unsigned buf_free = get_free_tx_buf(); unsigned buf_free = get_free_tx_buf();
if (buf_free < packet_len) { if (buf_free < packet_len) {
/* no enough space in buffer to send */ /* no enough space in buffer to send */
count_txerr(); count_txerr();
count_txerrbytes(packet_len); count_txerrbytes(packet_len);
pthread_mutex_unlock(&_send_mutex); pthread_mutex_unlock(&_send_mutex);
@@ -986,6 +996,7 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
ret = packet_len; ret = packet_len;
} }
} }
#endif #endif
if (ret != (size_t) packet_len) { if (ret != (size_t) packet_len) {
@@ -1018,16 +1029,19 @@ Mavlink::find_broadcast_address()
ifconf.ifc_len = 0; ifconf.ifc_len = 0;
ret = ioctl(_socket_fd, SIOCGIFCONF, &ifconf); ret = ioctl(_socket_fd, SIOCGIFCONF, &ifconf);
if (ret != 0) { if (ret != 0) {
PX4_WARN("getting required buffer size failed"); PX4_WARN("getting required buffer size failed");
return; return;
} }
#endif #endif
PX4_DEBUG("need to allocate %d bytes", ifconf.ifc_len); PX4_DEBUG("need to allocate %d bytes", ifconf.ifc_len);
// Allocate buffer. // Allocate buffer.
ifconf.ifc_req = (struct ifreq *)(new uint8_t[ifconf.ifc_len]); ifconf.ifc_req = (struct ifreq *)(new uint8_t[ifconf.ifc_len]);
if (ifconf.ifc_req == nullptr) { if (ifconf.ifc_req == nullptr) {
PX4_ERR("Could not allocate ifconf buffer"); PX4_ERR("Could not allocate ifconf buffer");
return; return;
@@ -1036,6 +1050,7 @@ Mavlink::find_broadcast_address()
memset(ifconf.ifc_req, 0, ifconf.ifc_len); memset(ifconf.ifc_req, 0, ifconf.ifc_len);
ret = ioctl(_socket_fd, SIOCGIFCONF, &ifconf); ret = ioctl(_socket_fd, SIOCGIFCONF, &ifconf);
if (ret != 0) { if (ret != 0) {
PX4_ERR("getting network config failed"); PX4_ERR("getting network config failed");
delete[] ifconf.ifc_req; delete[] ifconf.ifc_req;
@@ -1044,7 +1059,7 @@ Mavlink::find_broadcast_address()
size_t offset = 0; size_t offset = 0;
// Later used to point to next network interface in buffer. // Later used to point to next network interface in buffer.
struct ifreq *cur_ifreq = (struct ifreq *)&(((uint8_t *)ifconf.ifc_req)[offset]); struct ifreq *cur_ifreq = (struct ifreq *) & (((uint8_t *)ifconf.ifc_req)[offset]);
// The ugly `for` construct is used because it allows to use // The ugly `for` construct is used because it allows to use
// `continue` and `break`. // `continue` and `break`.
@@ -1055,17 +1070,17 @@ Mavlink::find_broadcast_address()
// the interface name size plus whatever is greater, either the // the interface name size plus whatever is greater, either the
// sizeof sockaddr or ifr_addr.sa_len. // sizeof sockaddr or ifr_addr.sa_len.
offset += IF_NAMESIZE offset += IF_NAMESIZE
+ (sizeof(struct sockaddr) > cur_ifreq->ifr_addr.sa_len ? + (sizeof(struct sockaddr) > cur_ifreq->ifr_addr.sa_len ?
sizeof(struct sockaddr) : cur_ifreq->ifr_addr.sa_len) sizeof(struct sockaddr) : cur_ifreq->ifr_addr.sa_len)
#else #else
// On Linux, it's much easier to traverse the buffer, every entry // On Linux, it's much easier to traverse the buffer, every entry
// has the constant length. // has the constant length.
offset += sizeof(struct ifreq) offset += sizeof(struct ifreq)
#endif #endif
) { ) {
// Point to next network interface in buffer. // Point to next network interface in buffer.
cur_ifreq = (struct ifreq *)&(((uint8_t *)ifconf.ifc_req)[offset]); cur_ifreq = (struct ifreq *) & (((uint8_t *)ifconf.ifc_req)[offset]);
PX4_DEBUG("looking at %s", cur_ifreq->ifr_name); PX4_DEBUG("looking at %s", cur_ifreq->ifr_name);
@@ -1079,9 +1094,13 @@ Mavlink::find_broadcast_address()
} }
struct ifreq bc_ifreq; struct ifreq bc_ifreq;
memset(&bc_ifreq, 0, sizeof(bc_ifreq)); memset(&bc_ifreq, 0, sizeof(bc_ifreq));
strncpy(bc_ifreq.ifr_name, cur_ifreq->ifr_name, IF_NAMESIZE); strncpy(bc_ifreq.ifr_name, cur_ifreq->ifr_name, IF_NAMESIZE);
ret = ioctl(_socket_fd, SIOCGIFBRDADDR, &bc_ifreq); ret = ioctl(_socket_fd, SIOCGIFBRDADDR, &bc_ifreq);
if (ret != 0) { if (ret != 0) {
PX4_DEBUG("getting broadcast address failed for %s", cur_ifreq->ifr_name); PX4_DEBUG("getting broadcast address failed for %s", cur_ifreq->ifr_name);
continue; continue;
@@ -1109,6 +1128,7 @@ Mavlink::find_broadcast_address()
_bcast_addr.sin_addr = bc_addr; _bcast_addr.sin_addr = bc_addr;
_broadcast_address_found = true; _broadcast_address_found = true;
} else { } else {
PX4_INFO("ignoring additional network interface %s, IP: %s", PX4_INFO("ignoring additional network interface %s, IP: %s",
cur_ifreq->ifr_name, inet_ntoa(sin_addr)); cur_ifreq->ifr_name, inet_ntoa(sin_addr));
@@ -1119,9 +1139,11 @@ Mavlink::find_broadcast_address()
_bcast_addr.sin_port = htons(_remote_port); _bcast_addr.sin_port = htons(_remote_port);
int broadcast_opt = 1; int broadcast_opt = 1;
if (setsockopt(_socket_fd, SOL_SOCKET, SO_BROADCAST, &broadcast_opt, sizeof(broadcast_opt)) < 0) { if (setsockopt(_socket_fd, SOL_SOCKET, SO_BROADCAST, &broadcast_opt, sizeof(broadcast_opt)) < 0) {
PX4_WARN("setting broadcast permission failed"); PX4_WARN("setting broadcast permission failed");
} }
_broadcast_address_not_found_warned = false; _broadcast_address_not_found_warned = false;
} else { } else {
@@ -1162,6 +1184,7 @@ Mavlink::init_udp()
_src_addr.sin_family = AF_INET; _src_addr.sin_family = AF_INET;
inet_aton("127.0.0.1", &_src_addr.sin_addr); inet_aton("127.0.0.1", &_src_addr.sin_addr);
} }
_src_addr.sin_port = htons(_remote_port); _src_addr.sin_port = htons(_remote_port);
#endif #endif
@@ -1515,15 +1538,18 @@ Mavlink::get_rate_mult()
return _rate_mult; return _rate_mult;
} }
MavlinkShell* MavlinkShell *
Mavlink::get_shell() Mavlink::get_shell()
{ {
if (!_mavlink_shell) { if (!_mavlink_shell) {
_mavlink_shell = new MavlinkShell(); _mavlink_shell = new MavlinkShell();
if (!_mavlink_shell) { if (!_mavlink_shell) {
PX4_ERR("Failed to allocate a shell"); PX4_ERR("Failed to allocate a shell");
} else { } else {
int ret = _mavlink_shell->start(); int ret = _mavlink_shell->start();
if (ret != 0) { if (ret != 0) {
PX4_ERR("Failed to start shell (%i)", ret); PX4_ERR("Failed to start shell (%i)", ret);
delete _mavlink_shell; delete _mavlink_shell;
@@ -1590,14 +1616,17 @@ Mavlink::update_rate_mult()
/* scale down if we have a TX err rate suggesting link congestion */ /* scale down if we have a TX err rate suggesting link congestion */
if (_rate_txerr > 0.0f && !radio_critical) { if (_rate_txerr > 0.0f && !radio_critical) {
hardware_mult = (_rate_tx) / (_rate_tx + _rate_txerr); hardware_mult = (_rate_tx) / (_rate_tx + _rate_txerr);
} else if (radio_found && tstatus.telem_time != _last_hw_rate_timestamp) { } else if (radio_found && tstatus.telem_time != _last_hw_rate_timestamp) {
if (tstatus.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) { if (tstatus.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) {
/* this indicates link congestion, reduce rate by 20% */ /* this indicates link congestion, reduce rate by 20% */
hardware_mult *= 0.80f; hardware_mult *= 0.80f;
} else if (tstatus.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) { } else if (tstatus.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
/* this indicates link congestion, reduce rate by 2.5% */ /* this indicates link congestion, reduce rate by 2.5% */
hardware_mult *= 0.975f; hardware_mult *= 0.975f;
} else if (tstatus.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) { } else if (tstatus.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) {
/* this indicates spare bandwidth, increase by 2.5% */ /* this indicates spare bandwidth, increase by 2.5% */
hardware_mult *= 1.025f; hardware_mult *= 1.025f;
@@ -1644,7 +1673,7 @@ Mavlink::task_main(int argc, char *argv[])
int myoptind = 1; int myoptind = 1;
const char *myoptarg = NULL; const char *myoptarg = NULL;
#ifdef __PX4_POSIX #ifdef __PX4_POSIX
char* eptr; char *eptr;
int temp_int_arg; int temp_int_arg;
#endif #endif
@@ -1676,38 +1705,49 @@ Mavlink::task_main(int argc, char *argv[])
break; break;
#ifdef __PX4_POSIX #ifdef __PX4_POSIX
case 'u': case 'u':
temp_int_arg = strtoul(myoptarg, &eptr, 10); temp_int_arg = strtoul(myoptarg, &eptr, 10);
if ( *eptr == '\0' ) {
if (*eptr == '\0') {
_network_port = temp_int_arg; _network_port = temp_int_arg;
set_protocol(UDP); set_protocol(UDP);
} else { } else {
warnx("invalid data udp_port '%s'", myoptarg); warnx("invalid data udp_port '%s'", myoptarg);
err_flag = true; err_flag = true;
} }
break; break;
case 'o': case 'o':
temp_int_arg = strtoul(myoptarg, &eptr, 10); temp_int_arg = strtoul(myoptarg, &eptr, 10);
if ( *eptr == '\0' ) {
if (*eptr == '\0') {
_remote_port = temp_int_arg; _remote_port = temp_int_arg;
set_protocol(UDP); set_protocol(UDP);
} else { } else {
warnx("invalid remote udp_port '%s'", myoptarg); warnx("invalid remote udp_port '%s'", myoptarg);
err_flag = true; err_flag = true;
} }
break; break;
case 't': case 't':
_src_addr.sin_family = AF_INET; _src_addr.sin_family = AF_INET;
if (inet_aton(myoptarg, &_src_addr.sin_addr)) { if (inet_aton(myoptarg, &_src_addr.sin_addr)) {
_src_addr_initialized = true; _src_addr_initialized = true;
} else { } else {
warnx("invalid partner ip '%s'", myoptarg); warnx("invalid partner ip '%s'", myoptarg);
err_flag = true; err_flag = true;
} }
break; break;
#else #else
case 'u': case 'u':
case 'o': case 'o':
case 't': case 't':
@@ -1797,6 +1837,7 @@ Mavlink::task_main(int argc, char *argv[])
if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) { if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) {
warn("could not open %s", _device_name); warn("could not open %s", _device_name);
return ERROR; return ERROR;
} else if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) { } else if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) {
/* the config link is optional */ /* the config link is optional */
return OK; return OK;
@@ -1991,6 +2032,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("MISSION_ITEM", 50.0f); configure_stream("MISSION_ITEM", 50.0f);
configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f); configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
configure_stream("MANUAL_CONTROL", 5.0f); configure_stream("MANUAL_CONTROL", 5.0f);
default: default:
break; break;
} }
@@ -2053,6 +2095,7 @@ Mavlink::task_main(int argc, char *argv[])
/* set channel */ /* set channel */
fprintf(fs, "ATS3=%u\n", _radio_id); fprintf(fs, "ATS3=%u\n", _radio_id);
usleep(200000); usleep(200000);
} else { } else {
/* reset to factory defaults */ /* reset to factory defaults */
fprintf(fs, "AT&F\n"); fprintf(fs, "AT&F\n");
@@ -2073,6 +2116,7 @@ Mavlink::task_main(int argc, char *argv[])
// config thing, we leave the file struct // config thing, we leave the file struct
// allocated. // allocated.
//fclose(fs); //fclose(fs);
} else { } else {
PX4_WARN("open fd %d failed", _uart_fd); PX4_WARN("open fd %d failed", _uart_fd);
} }
@@ -2099,6 +2143,7 @@ Mavlink::task_main(int argc, char *argv[])
} }
struct mavlink_log_s mavlink_log; struct mavlink_log_s mavlink_log;
if (mavlink_log_sub->update_if_changed(&mavlink_log)) { if (mavlink_log_sub->update_if_changed(&mavlink_log)) {
_logbuffer.put(&mavlink_log); _logbuffer.put(&mavlink_log);
} }
@@ -2118,26 +2163,29 @@ Mavlink::task_main(int argc, char *argv[])
if (_subscribe_to_stream != nullptr) { if (_subscribe_to_stream != nullptr) {
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
if (_subscribe_to_stream_rate > 0.0f) { if (_subscribe_to_stream_rate > 0.0f) {
if ( get_protocol() == SERIAL ) { if (get_protocol() == SERIAL) {
PX4_DEBUG("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, PX4_DEBUG("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
(double)_subscribe_to_stream_rate); (double)_subscribe_to_stream_rate);
} else if ( get_protocol() == UDP ) {
} else if (get_protocol() == UDP) {
PX4_DEBUG("stream %s on UDP port %d enabled with rate %.1f Hz", _subscribe_to_stream, _network_port, PX4_DEBUG("stream %s on UDP port %d enabled with rate %.1f Hz", _subscribe_to_stream, _network_port,
(double)_subscribe_to_stream_rate); (double)_subscribe_to_stream_rate);
} }
} else { } else {
if ( get_protocol() == SERIAL ) { if (get_protocol() == SERIAL) {
PX4_INFO("stream %s on device %s disabled", _subscribe_to_stream, _device_name); PX4_INFO("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
} else if ( get_protocol() == UDP ) {
} else if (get_protocol() == UDP) {
PX4_INFO("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port); PX4_INFO("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port);
} }
} }
} else { } else {
if ( get_protocol() == SERIAL ) { if (get_protocol() == SERIAL) {
PX4_WARN("stream %s on device %s not found", _subscribe_to_stream, _device_name); PX4_WARN("stream %s on device %s not found", _subscribe_to_stream, _device_name);
} else if ( get_protocol() == UDP ) {
} else if (get_protocol() == UDP) {
PX4_WARN("stream %s on UDP port %d not found", _subscribe_to_stream, _network_port); PX4_WARN("stream %s on UDP port %d not found", _subscribe_to_stream, _network_port);
} }
} }
@@ -2302,7 +2350,7 @@ Mavlink::start(int argc, char *argv[])
if (ic == Mavlink::MAVLINK_MAX_INSTANCES) { if (ic == Mavlink::MAVLINK_MAX_INSTANCES) {
warnx("Maximum MAVLink instance count of %d reached.", warnx("Maximum MAVLink instance count of %d reached.",
(int)Mavlink::MAVLINK_MAX_INSTANCES); (int)Mavlink::MAVLINK_MAX_INSTANCES);
return 1; return 1;
} }
@@ -2399,7 +2447,7 @@ Mavlink::stream_command(int argc, char *argv[])
float rate = -1.0f; float rate = -1.0f;
const char *stream_name = nullptr; const char *stream_name = nullptr;
unsigned short network_port = 0; unsigned short network_port = 0;
char* eptr; char *eptr;
int temp_int_arg; int temp_int_arg;
bool provided_device = false; bool provided_device = false;
bool provided_network_port = false; bool provided_network_port = false;
@@ -2438,15 +2486,20 @@ Mavlink::stream_command(int argc, char *argv[])
} else if (0 == strcmp(argv[i], "-s") && i < argc - 1) { } else if (0 == strcmp(argv[i], "-s") && i < argc - 1) {
stream_name = argv[i + 1]; stream_name = argv[i + 1];
i++; i++;
} else if (0 == strcmp(argv[i], "-u") && i < argc - 1) { } else if (0 == strcmp(argv[i], "-u") && i < argc - 1) {
provided_network_port = true; provided_network_port = true;
temp_int_arg = strtoul(argv[i + 1], &eptr, 10); temp_int_arg = strtoul(argv[i + 1], &eptr, 10);
if ( *eptr == '\0' ) {
if (*eptr == '\0') {
network_port = temp_int_arg; network_port = temp_int_arg;
} else { } else {
err_flag = true; err_flag = true;
} }
i++; i++;
} else { } else {
err_flag = true; err_flag = true;
} }
@@ -2457,10 +2510,13 @@ Mavlink::stream_command(int argc, char *argv[])
if (!err_flag && rate >= 0.0f && stream_name != nullptr) { if (!err_flag && rate >= 0.0f && stream_name != nullptr) {
Mavlink *inst = nullptr; Mavlink *inst = nullptr;
if (provided_device && !provided_network_port) { if (provided_device && !provided_network_port) {
inst = get_instance_for_device(device_name); inst = get_instance_for_device(device_name);
} else if (provided_network_port && !provided_device) { } else if (provided_network_port && !provided_device) {
inst = get_instance_for_network_port(network_port); inst = get_instance_for_network_port(network_port);
} else if (provided_device && provided_network_port) { } else if (provided_device && provided_network_port) {
warnx("please provide either a device name or a network port"); warnx("please provide either a device name or a network port");
return 1; return 1;
@@ -2475,6 +2531,7 @@ Mavlink::stream_command(int argc, char *argv[])
// because this is so easy to get wrong and not fatal. Warning is sufficient. // because this is so easy to get wrong and not fatal. Warning is sufficient.
if (provided_device) { if (provided_device) {
warnx("mavlink for device %s is not running", device_name); warnx("mavlink for device %s is not running", device_name);
} else { } else {
warnx("mavlink for network on port %hu is not running", network_port); warnx("mavlink for network on port %hu is not running", network_port);
} }
@@ -2499,8 +2556,8 @@ Mavlink::set_boot_complete()
Mavlink *inst; Mavlink *inst;
LL_FOREACH(::_mavlink_instances, inst) { LL_FOREACH(::_mavlink_instances, inst) {
if ((inst->get_mode() != MAVLINK_MODE_ONBOARD) && if ((inst->get_mode() != MAVLINK_MODE_ONBOARD) &&
(!inst->broadcast_enabled()) && (!inst->broadcast_enabled()) &&
((inst->get_protocol() == UDP) || (inst->get_protocol() == TCP))) { ((inst->get_protocol() == UDP) || (inst->get_protocol() == TCP))) {
PX4_INFO("MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)"); PX4_INFO("MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)");
} }
} }
@@ -2543,7 +2600,7 @@ int mavlink_main(int argc, char *argv[])
usage(); usage();
return 1; return 1;
} else if (!strcmp(argv[1], "stop-all") ) { } else if (!strcmp(argv[1], "stop-all")) {
return Mavlink::destroy_all_instances(); return Mavlink::destroy_all_instances();
} else if (!strcmp(argv[1], "status")) { } else if (!strcmp(argv[1], "status")) {
src/modules/mavlink/mavlink_main.h
+32 -24
View File
@@ -170,22 +170,29 @@ public:
BROADCAST_MODE_ON BROADCAST_MODE_ON
}; };
static const char *mavlink_mode_str(enum MAVLINK_MODE mode) { static const char *mavlink_mode_str(enum MAVLINK_MODE mode)
{
switch (mode) { switch (mode) {
case MAVLINK_MODE_NORMAL: case MAVLINK_MODE_NORMAL:
return "Normal"; return "Normal";
case MAVLINK_MODE_CUSTOM:
return "Custom"; case MAVLINK_MODE_CUSTOM:
case MAVLINK_MODE_ONBOARD: return "Custom";
return "Onboard";
case MAVLINK_MODE_OSD: case MAVLINK_MODE_ONBOARD:
return "OSD"; return "Onboard";
case MAVLINK_MODE_MAGIC:
return "Magic"; case MAVLINK_MODE_OSD:
case MAVLINK_MODE_CONFIG: return "OSD";
return "Config";
default: case MAVLINK_MODE_MAGIC:
return "Unknown"; return "Magic";
case MAVLINK_MODE_CONFIG:
return "Config";
default:
return "Unknown";
} }
} }
@@ -230,7 +237,8 @@ public:
*/ */
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
/** /**
* Enable / disable Hardware in the Loop simulation mode. * Enable / disable Hardware in the Loop simulation mode.
@@ -285,7 +293,7 @@ public:
void handle_message(const mavlink_message_t *msg); void handle_message(const mavlink_message_t *msg);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0); MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance = 0);
int get_instance_id(); int get_instance_id();
@@ -339,7 +347,7 @@ public:
void send_statustext(unsigned char severity, const char *string); void send_statustext(unsigned char severity, const char *string);
void send_autopilot_capabilites(); void send_autopilot_capabilites();
MavlinkStream * get_streams() const { return _streams; } MavlinkStream *get_streams() const { return _streams; }
float get_rate_mult(); float get_rate_mult();
@@ -380,7 +388,7 @@ public:
/** /**
* Get the receive status of this MAVLink link * Get the receive status of this MAVLink link
*/ */
struct telemetry_status_s& get_rx_status() { return _rstatus; } struct telemetry_status_s &get_rx_status() { return _rstatus; }
ringbuffer::RingBuffer *get_logbuffer() { return &_logbuffer; } ringbuffer::RingBuffer *get_logbuffer() { return &_logbuffer; }
@@ -392,9 +400,9 @@ public:
unsigned short get_remote_port() { return _remote_port; } unsigned short get_remote_port() { return _remote_port; }
int get_socket_fd () { return _socket_fd; }; int get_socket_fd() { return _socket_fd; };
#ifdef __PX4_POSIX #ifdef __PX4_POSIX
struct sockaddr_in * get_client_source_address() { return &_src_addr; } struct sockaddr_in *get_client_source_address() { return &_src_addr; }
void set_client_source_initialized() { _src_addr_initialized = true; } void set_client_source_initialized() { _src_addr_initialized = true; }
@@ -419,7 +427,7 @@ public:
void set_logging_enabled(bool logging) { _logging_enabled = logging; } void set_logging_enabled(bool logging) { _logging_enabled = logging; }
int get_data_rate() { return _datarate; } int get_data_rate() { return _datarate; }
void set_data_rate(int rate) { if (rate > 0) _datarate = rate; } void set_data_rate(int rate) { if (rate > 0) { _datarate = rate; } }
/** get the Mavlink shell. Create a new one if there isn't one. It is *always* created via MavlinkReceiver thread. /** get the Mavlink shell. Create a new one if there isn't one. It is *always* created via MavlinkReceiver thread.
* Returns nullptr if shell cannot be created */ * Returns nullptr if shell cannot be created */
@@ -608,6 +616,6 @@ private:
int task_main(int argc, char *argv[]); int task_main(int argc, char *argv[]);
/* do not allow copying this class */ /* do not allow copying this class */
Mavlink(const Mavlink&); Mavlink(const Mavlink &);
Mavlink operator=(const Mavlink&); Mavlink operator=(const Mavlink &);
}; };
src/modules/mavlink/mavlink_messages.cpp
+380 -343
View File
File diff suppressed because it is too large Load Diff
src/modules/mavlink/mavlink_mission.cpp
+26 -12
View File
@@ -66,9 +66,9 @@ int MavlinkMissionManager::_current_seq = 0;
bool MavlinkMissionManager::_transfer_in_progress = false; bool MavlinkMissionManager::_transfer_in_progress = false;
#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \ #define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \
((_msg.target_component == mavlink_system.compid) || \ ((_msg.target_component == mavlink_system.compid) || \
(_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \ (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
(_msg.target_component == MAV_COMP_ID_ALL))) (_msg.target_component == MAV_COMP_ID_ALL)))
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink), MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_state(MAVLINK_WPM_STATE_IDLE), _state(MAVLINK_WPM_STATE_IDLE),
@@ -120,9 +120,11 @@ void
MavlinkMissionManager::init_offboard_mission() MavlinkMissionManager::init_offboard_mission()
{ {
mission_s mission_state; mission_s mission_state;
if (!_dataman_init) { if (!_dataman_init) {
_dataman_init = true; _dataman_init = true;
int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s); int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s);
if (ret > 0) { if (ret > 0) {
_dataman_id = mission_state.dataman_id; _dataman_id = mission_state.dataman_id;
_count = mission_state.count; _count = mission_state.count;
@@ -132,6 +134,7 @@ MavlinkMissionManager::init_offboard_mission()
_dataman_id = 0; _dataman_id = 0;
_count = 0; _count = 0;
_current_seq = 0; _current_seq = 0;
} else { } else {
PX4_WARN("offboard mission init failed"); PX4_WARN("offboard mission init failed");
_dataman_id = 0; _dataman_id = 0;
@@ -139,6 +142,7 @@ MavlinkMissionManager::init_offboard_mission()
_current_seq = 0; _current_seq = 0;
} }
} }
_my_dataman_id = _dataman_id; _my_dataman_id = _dataman_id;
} }
@@ -176,6 +180,7 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int
} else { } else {
warnx("WPM: ERROR: can't save mission state"); warnx("WPM: ERROR: can't save mission state");
if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) {
_mavlink->send_statustext_critical("Mission storage: Unable to write to microSD"); _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD");
} }
@@ -261,6 +266,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
} else { } else {
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) {
_mavlink->send_statustext_critical("Mission storage: Unable to read from microSD"); _mavlink->send_statustext_critical("Mission storage: Unable to read from microSD");
} }
@@ -588,11 +594,11 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
if (_state == MAVLINK_WPM_STATE_IDLE) { if (_state == MAVLINK_WPM_STATE_IDLE) {
_time_last_recv = hrt_absolute_time(); _time_last_recv = hrt_absolute_time();
if(_transfer_in_progress) if (_transfer_in_progress) {
{
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
return; return;
} }
_transfer_in_progress = true; _transfer_in_progress = true;
if (wpc.count > _max_count) { if (wpc.count > _max_count) {
@@ -683,6 +689,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
} }
struct mission_item_s mission_item = {}; struct mission_item_s mission_item = {};
int ret = parse_mavlink_mission_item(&wp, &mission_item); int ret = parse_mavlink_mission_item(&wp, &mission_item);
if (ret != OK) { if (ret != OK) {
@@ -698,7 +705,8 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id); dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id);
if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item,
sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); } if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); }
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
@@ -729,6 +737,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
} else { } else {
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
} }
_transfer_in_progress = false; _transfer_in_progress = false;
} else { } else {
@@ -753,6 +762,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
if (update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0) == OK) { if (update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0) == OK) {
if (_verbose) { warnx("WPM: CLEAR_ALL OK"); } if (_verbose) { warnx("WPM: CLEAR_ALL OK"); }
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
} else { } else {
@@ -768,10 +778,11 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
} }
int int
MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item,
struct mission_item_s *mission_item)
{ {
if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL || if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL ||
mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) { mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
// only support global waypoints for now // only support global waypoints for now
@@ -781,6 +792,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL) { if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL) {
mission_item->altitude_is_relative = false; mission_item->altitude_is_relative = false;
} else if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) { } else if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
mission_item->altitude_is_relative = true; mission_item->altitude_is_relative = true;
} }
@@ -897,7 +909,8 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
int int
MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item,
mavlink_mission_item_t *mavlink_mission_item)
{ {
mavlink_mission_item->frame = mission_item->frame; mavlink_mission_item->frame = mission_item->frame;
mavlink_mission_item->command = mission_item->nav_cmd; mavlink_mission_item->command = mission_item->nav_cmd;
@@ -943,6 +956,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
if (mission_item->altitude_is_relative) { if (mission_item->altitude_is_relative) {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
} else { } else {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL; mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
} }
@@ -997,10 +1011,10 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
void MavlinkMissionManager::check_active_mission(void) void MavlinkMissionManager::check_active_mission(void)
{ {
if(!(_my_dataman_id==_dataman_id)) if (!(_my_dataman_id == _dataman_id)) {
{
if (_verbose) { warnx("WPM: New mission detected (possibly over different Mavlink instance) Updating"); } if (_verbose) { warnx("WPM: New mission detected (possibly over different Mavlink instance) Updating"); }
_my_dataman_id=_dataman_id;
_my_dataman_id = _dataman_id;
this->send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count); this->send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count);
} }
} }
src/modules/mavlink/mavlink_mission.h
+8 -5
View File
@@ -67,7 +67,8 @@ enum MAVLINK_WPM_CODES {
#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds #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 #define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds
class MavlinkMissionManager : public MavlinkStream { class MavlinkMissionManager : public MavlinkStream
{
public: public:
~MavlinkMissionManager(); ~MavlinkMissionManager();
@@ -113,7 +114,7 @@ private:
static int _dataman_id; ///< Global Dataman storage ID for active mission static int _dataman_id; ///< Global Dataman storage ID for active mission
int _my_dataman_id; ///< class Dataman storage ID int _my_dataman_id; ///< class Dataman storage ID
static bool _dataman_init; ///< Dataman initialized static bool _dataman_init; ///< Dataman initialized
static unsigned _count; ///< Count of items in active mission static unsigned _count; ///< Count of items in active mission
static int _current_seq; ///< Current item sequence in active mission static int _current_seq; ///< Current item sequence in active mission
@@ -133,11 +134,12 @@ private:
bool _verbose; bool _verbose;
static constexpr unsigned int FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT = 2; ///< Error count limit before stopping to report FS errors static constexpr unsigned int FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT =
2; ///< Error count limit before stopping to report FS errors
/* do not allow top copying this class */ /* do not allow top copying this class */
MavlinkMissionManager(MavlinkMissionManager &); MavlinkMissionManager(MavlinkMissionManager &);
MavlinkMissionManager& operator = (const MavlinkMissionManager &); MavlinkMissionManager &operator = (const MavlinkMissionManager &);
void init_offboard_mission(); void init_offboard_mission();
@@ -196,7 +198,8 @@ private:
/** /**
* Format mission_item_s as mavlink MISSION_ITEM message. * Format mission_item_s as mavlink MISSION_ITEM message.
*/ */
int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); int format_mavlink_mission_item(const struct mission_item_s *mission_item,
mavlink_mission_item_t *mavlink_mission_item);
protected: protected:
explicit MavlinkMissionManager(Mavlink *mavlink); explicit MavlinkMissionManager(Mavlink *mavlink);
src/modules/mavlink/mavlink_orb_subscription.cpp
+5
View File
@@ -80,6 +80,7 @@ MavlinkOrbSubscription::update(uint64_t *time, void *data)
// if topic was published between orb_stat and orb_copy calls. // if topic was published between orb_stat and orb_copy calls.
uint64_t time_topic; uint64_t time_topic;
if (orb_stat(_fd, &time_topic)) { if (orb_stat(_fd, &time_topic)) {
/* error getting last topic publication time */ /* error getting last topic publication time */
time_topic = 0; time_topic = 0;
@@ -90,11 +91,13 @@ MavlinkOrbSubscription::update(uint64_t *time, void *data)
/* error copying topic data */ /* error copying topic data */
memset(data, 0, _topic->o_size); memset(data, 0, _topic->o_size);
} }
return false; return false;
} else { } else {
/* data copied successfully */ /* data copied successfully */
_published = true; _published = true;
if (time_topic != *time) { if (time_topic != *time) {
*time = time_topic; *time = time_topic;
return true; return true;
@@ -115,6 +118,7 @@ bool
MavlinkOrbSubscription::update_if_changed(void *data) MavlinkOrbSubscription::update_if_changed(void *data)
{ {
bool updated; bool updated;
if (orb_check(_fd, &updated)) { if (orb_check(_fd, &updated)) {
return false; return false;
} }
@@ -128,6 +132,7 @@ MavlinkOrbSubscription::update_if_changed(void *data)
/* error copying topic data */ /* error copying topic data */
memset(data, 0, _topic->o_size); memset(data, 0, _topic->o_size);
} }
return false; return false;
} }
src/modules/mavlink/mavlink_orb_subscription.h
+2 -2
View File
@@ -97,8 +97,8 @@ private:
bool _published; ///< topic was ever published bool _published; ///< topic was ever published
/* do not allow copying this class */ /* do not allow copying this class */
MavlinkOrbSubscription(const MavlinkOrbSubscription&); MavlinkOrbSubscription(const MavlinkOrbSubscription &);
MavlinkOrbSubscription operator=(const MavlinkOrbSubscription&); MavlinkOrbSubscription operator=(const MavlinkOrbSubscription &);
}; };
src/modules/mavlink/mavlink_parameters.cpp
+27 -2
View File
@@ -62,6 +62,7 @@ MavlinkParametersManager::~MavlinkParametersManager()
if (_uavcan_parameter_value_sub >= 0) { if (_uavcan_parameter_value_sub >= 0) {
orb_unsubscribe(_uavcan_parameter_value_sub); orb_unsubscribe(_uavcan_parameter_value_sub);
} }
if (_uavcan_parameter_request_pub) { if (_uavcan_parameter_request_pub) {
orb_unadvertise(_uavcan_parameter_request_pub); orb_unadvertise(_uavcan_parameter_request_pub);
} }
@@ -92,6 +93,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
(req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
if (_send_all_index < 0) { if (_send_all_index < 0) {
_send_all_index = PARAM_HASH; _send_all_index = PARAM_HASH;
} else { } else {
/* a restart should skip the hash check on the ground */ /* a restart should skip the hash check on the ground */
_send_all_index = 0; _send_all_index = 0;
@@ -108,10 +110,12 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
if (_uavcan_parameter_request_pub == nullptr) { if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else { } else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
} }
} }
break; break;
} }
@@ -160,9 +164,11 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
req.param_index = -1; req.param_index = -1;
strncpy(req.param_id, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1); strncpy(req.param_id, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1);
req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
if (set.param_type == MAV_PARAM_TYPE_REAL32) { if (set.param_type == MAV_PARAM_TYPE_REAL32) {
req.param_type = MAV_PARAM_TYPE_REAL32; req.param_type = MAV_PARAM_TYPE_REAL32;
req.real_value = set.param_value; req.real_value = set.param_value;
} else { } else {
int32_t val; int32_t val;
memcpy(&val, &set.param_value, sizeof(int32_t)); memcpy(&val, &set.param_value, sizeof(int32_t));
@@ -172,10 +178,12 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
if (_uavcan_parameter_request_pub == nullptr) { if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else { } else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
} }
} }
break; break;
} }
@@ -202,6 +210,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
param_value.param_type = MAV_PARAM_TYPE_UINT32; param_value.param_type = MAV_PARAM_TYPE_UINT32;
memcpy(&param_value.param_value, &hash, sizeof(hash)); memcpy(&param_value.param_value, &hash, sizeof(hash));
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &param_value); mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &param_value);
} else { } else {
/* local name buffer to enforce null-terminated string */ /* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
@@ -211,6 +220,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
/* attempt to find parameter and send it */ /* attempt to find parameter and send it */
send_param(param_find_no_notification(name)); send_param(param_find_no_notification(name));
} }
} else { } else {
/* when index is >= 0, send this parameter again */ /* when index is >= 0, send this parameter again */
int ret = send_param(param_for_used_index(req_read.param_index)); int ret = send_param(param_for_used_index(req_read.param_index));
@@ -219,6 +229,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] unknown param ID: %u", req_read.param_index); sprintf(buf, "[pm] unknown param ID: %u", req_read.param_index);
_mavlink->send_statustext_info(buf); _mavlink->send_statustext_info(buf);
} else if (ret == 2) { } else if (ret == 2) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] failed loading param from storage ID: %u", req_read.param_index); sprintf(buf, "[pm] failed loading param from storage ID: %u", req_read.param_index);
@@ -228,7 +239,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
} }
if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 && if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 &&
(req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
// publish set request to UAVCAN driver via uORB. // publish set request to UAVCAN driver via uORB.
uavcan_parameter_request_s req; uavcan_parameter_request_s req;
req.message_type = msg->msgid; req.message_type = msg->msgid;
@@ -239,10 +250,12 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
if (_uavcan_parameter_request_pub == nullptr) { if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else { } else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
} }
} }
break; break;
} }
@@ -258,18 +271,22 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
/* Copy values from msg to uorb using the parameter_rc_channel_index as index */ /* Copy values from msg to uorb using the parameter_rc_channel_index as index */
size_t i = map_rc.parameter_rc_channel_index; size_t i = map_rc.parameter_rc_channel_index;
_rc_param_map.param_index[i] = map_rc.param_index; _rc_param_map.param_index[i] = map_rc.param_index;
strncpy(&(_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); strncpy(&(_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]), map_rc.param_id,
MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */ /* enforce null termination */
_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1) + rc_parameter_map_s::PARAM_ID_LEN] = '\0'; _rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1) + rc_parameter_map_s::PARAM_ID_LEN] = '\0';
_rc_param_map.scale[i] = map_rc.scale; _rc_param_map.scale[i] = map_rc.scale;
_rc_param_map.value0[i] = map_rc.param_value0; _rc_param_map.value0[i] = map_rc.param_value0;
_rc_param_map.value_min[i] = map_rc.param_value_min; _rc_param_map.value_min[i] = map_rc.param_value_min;
_rc_param_map.value_max[i] = map_rc.param_value_max; _rc_param_map.value_max[i] = map_rc.param_value_max;
if (map_rc.param_index == -2) { // -2 means unset map if (map_rc.param_index == -2) { // -2 means unset map
_rc_param_map.valid[i] = false; _rc_param_map.valid[i] = false;
} else { } else {
_rc_param_map.valid[i] = true; _rc_param_map.valid[i] = true;
} }
_rc_param_map.timestamp = hrt_absolute_time(); _rc_param_map.timestamp = hrt_absolute_time();
if (_rc_param_map_pub == nullptr) { if (_rc_param_map_pub == nullptr) {
@@ -280,6 +297,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
} }
} }
break; break;
} }
@@ -300,6 +318,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
bool param_value_ready; bool param_value_ready;
orb_check(_uavcan_parameter_value_sub, &param_value_ready); orb_check(_uavcan_parameter_value_sub, &param_value_ready);
if (space_available && param_value_ready) { if (space_available && param_value_ready) {
struct uavcan_parameter_value_s value; struct uavcan_parameter_value_s value;
orb_copy(ORB_ID(uavcan_parameter_value), _uavcan_parameter_value_sub, &value); orb_copy(ORB_ID(uavcan_parameter_value), _uavcan_parameter_value_sub, &value);
@@ -308,16 +327,20 @@ MavlinkParametersManager::send(const hrt_abstime t)
msg.param_count = value.param_count; msg.param_count = value.param_count;
msg.param_index = value.param_index; msg.param_index = value.param_index;
strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
if (value.param_type == MAV_PARAM_TYPE_REAL32) { if (value.param_type == MAV_PARAM_TYPE_REAL32) {
msg.param_type = MAVLINK_TYPE_FLOAT; msg.param_type = MAVLINK_TYPE_FLOAT;
msg.param_value = value.real_value; msg.param_value = value.real_value;
} else { } else {
int32_t val; int32_t val;
val = (int32_t)value.int_value; val = (int32_t)value.int_value;
memcpy(&msg.param_value, &val, sizeof(int32_t)); memcpy(&msg.param_value, &val, sizeof(int32_t));
msg.param_type = MAVLINK_TYPE_INT32_T; msg.param_type = MAVLINK_TYPE_INT32_T;
} }
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg); mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
} else if (_send_all_index >= 0 && _mavlink->boot_complete()) { } else if (_send_all_index >= 0 && _mavlink->boot_complete()) {
/* send all parameters if requested, but only after the system has booted */ /* send all parameters if requested, but only after the system has booted */
@@ -351,6 +374,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
/* look for the first parameter which is used */ /* look for the first parameter which is used */
param_t p; param_t p;
do { do {
/* walk through all parameters, including unused ones */ /* walk through all parameters, including unused ones */
p = param_for_index(_send_all_index); p = param_for_index(_send_all_index);
@@ -364,6 +388,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) { if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) {
_send_all_index = -1; _send_all_index = -1;
} }
} else if (_send_all_index == PARAM_HASH && hrt_absolute_time() > 20 * 1000 * 1000) { } else if (_send_all_index == PARAM_HASH && hrt_absolute_time() > 20 * 1000 * 1000) {
/* the boot did not seem to ever complete, warn user and set boot complete */ /* the boot did not seem to ever complete, warn user and set boot complete */
_mavlink->send_statustext_critical("WARNING: SYSTEM BOOT INCOMPLETE. CHECK CONFIG."); _mavlink->send_statustext_critical("WARNING: SYSTEM BOOT INCOMPLETE. CHECK CONFIG.");
src/modules/mavlink/mavlink_parameters.h
+1 -1
View File
@@ -82,7 +82,7 @@ private:
/* do not allow top copying this class */ /* do not allow top copying this class */
MavlinkParametersManager(MavlinkParametersManager &); MavlinkParametersManager(MavlinkParametersManager &);
MavlinkParametersManager& operator = (const MavlinkParametersManager &); MavlinkParametersManager &operator = (const MavlinkParametersManager &);
protected: protected:
explicit MavlinkParametersManager(Mavlink *mavlink); explicit MavlinkParametersManager(Mavlink *mavlink);
src/modules/mavlink/mavlink_receiver.cpp
+3 -2
View File
@@ -1201,11 +1201,12 @@ MavlinkReceiver::handle_message_serial_control(mavlink_message_t *msg)
// we only support shell commands // we only support shell commands
if (serial_control_mavlink.device != SERIAL_CONTROL_DEV_SHELL if (serial_control_mavlink.device != SERIAL_CONTROL_DEV_SHELL
|| (serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_REPLY)) { || (serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_REPLY)) {
return; return;
} }
MavlinkShell* shell = _mavlink->get_shell(); MavlinkShell *shell = _mavlink->get_shell();
if (shell) { if (shell) {
// we ignore the timeout, EXCLUSIVE & BLOCKING flags of the SERIAL_CONTROL message // we ignore the timeout, EXCLUSIVE & BLOCKING flags of the SERIAL_CONTROL message
if (serial_control_mavlink.count > 0) { if (serial_control_mavlink.count > 0) {
src/modules/mavlink/mavlink_receiver.h
+1 -1
View File
@@ -155,7 +155,7 @@ private:
* @param interval the interval in us to send the message at * @param interval the interval in us to send the message at
* @param data_rate the total link data rate in bytes per second * @param data_rate the total link data rate in bytes per second
*/ */
void set_message_interval(int msgId, float interval, int data_rate=-1); void set_message_interval(int msgId, float interval, int data_rate = -1);
void get_message_interval(int msgId); void get_message_interval(int msgId);
/** /**
src/modules/mavlink/mavlink_shell.cpp
+16 -7
View File
@@ -62,6 +62,7 @@ MavlinkShell::~MavlinkShell()
PX4_INFO("Stopping mavlink shell"); PX4_INFO("Stopping mavlink shell");
close(_to_shell_fd); close(_to_shell_fd);
} }
if (_from_shell_fd >= 0) { if (_from_shell_fd >= 0) {
close(_from_shell_fd); close(_from_shell_fd);
} }
@@ -91,6 +92,7 @@ int MavlinkShell::start()
if (pipe(p1) != 0) { if (pipe(p1) != 0) {
return -errno; return -errno;
} }
if (pipe(p2) != 0) { if (pipe(p2) != 0) {
close(p1[0]); close(p1[0]);
close(p1[1]); close(p1[1]);
@@ -105,22 +107,26 @@ int MavlinkShell::start()
_shell_fds[1] = p1[1]; _shell_fds[1] = p1[1];
int fd_backups[2]; //we don't touch stderr, we will redirect it to stdout in the startup of the shell task int fd_backups[2]; //we don't touch stderr, we will redirect it to stdout in the startup of the shell task
for (int i = 0; i < 2; ++i) { for (int i = 0; i < 2; ++i) {
fd_backups[i] = dup(i); fd_backups[i] = dup(i);
if (fd_backups[i] == -1) { if (fd_backups[i] == -1) {
ret = -errno; ret = -errno;
} }
} }
dup2(_shell_fds[0], 0); dup2(_shell_fds[0], 0);
dup2(_shell_fds[1], 1); dup2(_shell_fds[1], 1);
if (ret == 0) { if (ret == 0) {
_task = px4_task_spawn_cmd("mavlink_shell", _task = px4_task_spawn_cmd("mavlink_shell",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT, SCHED_PRIORITY_DEFAULT,
2048, 2048,
&MavlinkShell::shell_start_thread, &MavlinkShell::shell_start_thread,
nullptr); nullptr);
if (_task < 0) { if (_task < 0) {
ret = -1; ret = -1;
} }
@@ -131,6 +137,7 @@ int MavlinkShell::start()
if (dup2(fd_backups[i], i) == -1) { if (dup2(fd_backups[i], i) == -1) {
ret = -errno; ret = -errno;
} }
close(fd_backups[i]); close(fd_backups[i]);
} }
@@ -152,12 +159,12 @@ int MavlinkShell::shell_start_thread(int argc, char *argv[])
return 0; return 0;
} }
size_t MavlinkShell::write(uint8_t* buffer, size_t len) size_t MavlinkShell::write(uint8_t *buffer, size_t len)
{ {
return ::write(_to_shell_fd, buffer, len); return ::write(_to_shell_fd, buffer, len);
} }
size_t MavlinkShell::read(uint8_t* buffer, size_t len) size_t MavlinkShell::read(uint8_t *buffer, size_t len)
{ {
return ::read(_from_shell_fd, buffer, len); return ::read(_from_shell_fd, buffer, len);
} }
@@ -165,8 +172,10 @@ size_t MavlinkShell::read(uint8_t* buffer, size_t len)
size_t MavlinkShell::available() size_t MavlinkShell::available()
{ {
int ret = 0; int ret = 0;
if (ioctl(_from_shell_fd, FIONREAD, (unsigned long)&ret) == OK) { if (ioctl(_from_shell_fd, FIONREAD, (unsigned long)&ret) == OK) {
return ret; return ret;
} }
return 0; return 0;
} }
src/modules/mavlink/mavlink_shell.h
+3 -3
View File
@@ -64,14 +64,14 @@ public:
* Write to the shell * Write to the shell
* @return number of written bytes * @return number of written bytes
*/ */
size_t write(uint8_t* buffer, size_t len); size_t write(uint8_t *buffer, size_t len);
/** /**
* Read from the shell. This is blocking, if 0 bytes are available, this will block. * Read from the shell. This is blocking, if 0 bytes are available, this will block.
* @param len buffer length * @param len buffer length
* @return number of bytes read. * @return number of bytes read.
*/ */
size_t read(uint8_t* buffer, size_t len); size_t read(uint8_t *buffer, size_t len);
/** /**
* Get the number of bytes that can be read. * Get the number of bytes that can be read.
@@ -82,7 +82,7 @@ private:
int _to_shell_fd = -1; /** fd to write to the shell */ int _to_shell_fd = -1; /** fd to write to the shell */
int _from_shell_fd = -1; /** fd to read from the shell */ int _from_shell_fd = -1; /** fd to read from the shell */
int _shell_fds[2] = {-1, -1}; /** stdin & out used by the shell */ int _shell_fds[2] = { -1, -1}; /** stdin & out used by the shell */
px4_task_t _task; px4_task_t _task;
static int shell_start_thread(int argc, char *argv[]); static int shell_start_thread(int argc, char *argv[]);