fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 23:27: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_2 = 0;
MavlinkFTP::MavlinkFTP(Mavlink* mavlink) :
MavlinkFTP::MavlinkFTP(Mavlink *mavlink) :
MavlinkStream(mavlink),
_session_info{},
_utRcvMsgFunc{},
@@ -63,10 +63,10 @@ MavlinkFTP::MavlinkFTP(Mavlink* mavlink) :
MavlinkFTP::~MavlinkFTP()
{
}
const char*
const char *
MavlinkFTP::get_name(void) const
{
return "MAVLINK_FTP";
@@ -83,13 +83,13 @@ MavlinkFTP::get_size(void)
{
if (_session_info.stream_download) {
return MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
} else {
return 0;
}
}
MavlinkStream*
MavlinkStream *
MavlinkFTP::new_instance(Mavlink *mavlink)
{
return new MavlinkFTP(mavlink);
@@ -144,15 +144,15 @@ void
MavlinkFTP::handle_message(const mavlink_message_t *msg)
{
//warnx("MavlinkFTP::handle_message %d %d", buf_size_1, buf_size_2);
if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
mavlink_file_transfer_protocol_t ftp_request;
mavlink_msg_file_transfer_protocol_decode(msg, &ftp_request);
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: received ftp protocol message target_system: %d", ftp_request.target_system);
#endif
if (ftp_request.target_system == _getServerSystemId()) {
_process_request(&ftp_request, msg->sysid);
return;
@@ -162,7 +162,7 @@ MavlinkFTP::handle_message(const mavlink_message_t *msg)
/// @brief Processes an FTP message
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;
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
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
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);
stream_send = true;
break;
case kCmdWriteFile:
errorCode = _workWrite(payload);
break;
@@ -235,33 +236,35 @@ MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t* ftp_req, uint8_t
case kCmdCreateDirectory:
errorCode = _workCreateDirectory(payload);
break;
case kCmdRemoveDirectory:
errorCode = _workRemoveDirectory(payload);
break;
case kCmdCalcFileCRC32:
errorCode = _workCalcFileCRC32(payload);
break;
default:
errorCode = kErrUnknownCommand;
break;
break;
}
out:
payload->seq_number++;
// handle success vs. error
if (errorCode == kErrNone) {
payload->req_opcode = payload->opcode;
payload->opcode = kRspAck;
} else {
int r_errno = errno;
payload->req_opcode = payload->opcode;
payload->opcode = kRspNak;
payload->size = 1;
payload->data[0] = errorCode;
if (errorCode == kErrFailErrno) {
payload->size = 2;
payload->data[1] = r_errno;
@@ -278,14 +281,14 @@ out:
/// @brief Sends the specified FTP response message out through mavlink
void
MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req)
MavlinkFTP::_reply(mavlink_file_transfer_protocol_t *ftp_req)
{
#ifdef MAVLINK_FTP_DEBUG
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]);
warnx("FTP: %s seq_number: %d", payload->opcode == kRspAck ? "Ack" : "Nak", payload->seq_number);
#endif
ftp_req->target_network = 0;
ftp_req->target_component = 0;
#ifdef MAVLINK_FTP_UNIT_TEST
@@ -299,7 +302,7 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req)
/// @brief Responds to a List command
MavlinkFTP::ErrorCode
MavlinkFTP::_workList(PayloadHeader* payload, bool list_hidden)
MavlinkFTP::_workList(PayloadHeader *payload, bool list_hidden)
{
char dirPath[kMaxDataLength];
strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength);
@@ -333,7 +336,7 @@ MavlinkFTP::_workList(PayloadHeader* payload, bool list_hidden)
// read the directory entry
if (readdir_r(dp, &entry, &result)) {
#ifdef MAVLINK_FTP_UNIT_TEST
warnx("readdir_r failed");
warnx("readdir_r failed");
#else
_mavlink->send_statustext_critical("FTP: list readdir_r failure");
_mavlink->send_statustext_critical(dirPath);
@@ -355,6 +358,7 @@ MavlinkFTP::_workList(PayloadHeader* payload, bool list_hidden)
// to seek past EOF.
errorCode = kErrEOF;
}
// Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that
break;
}
@@ -366,6 +370,7 @@ MavlinkFTP::_workList(PayloadHeader* payload, bool list_hidden)
// Determine the directory entry type
switch (entry.d_type) {
#ifdef __PX4_NUTTX
case DTYPE_FILE:
#else
case DT_REG:
@@ -374,51 +379,60 @@ MavlinkFTP::_workList(PayloadHeader* payload, bool list_hidden)
direntType = kDirentFile;
snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name);
struct stat st;
if (stat(buf, &st) == 0) {
fileSize = st.st_size;
}
break;
#ifdef __PX4_NUTTX
case DTYPE_DIRECTORY:
#else
case DT_DIR:
#endif
if ((!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
direntType = kDirentSkip;
} else {
direntType = kDirentDir;
}
break;
default:
// We only send back file and diretory entries, skip everything else
direntType = kDirentSkip;
}
if (direntType == kDirentSkip) {
// Skip send only dirent identifier
buf[0] = '\0';
} else if (direntType == kDirentFile) {
// Files send filename and file length
snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize);
} else {
// Everything else just sends name
strncpy(buf, entry.d_name, sizeof(buf));
buf[sizeof(buf)-1] = 0;
buf[sizeof(buf) - 1] = 0;
}
size_t nameLen = strlen(buf);
// Do we have room for the name, the one char directory identifier and the null terminator?
if ((offset + nameLen + 2) > kMaxDataLength) {
break;
}
// Move the data into the buffer
payload->data[offset++] = direntType;
strcpy((char *)&payload->data[offset], buf);
#ifdef MAVLINK_FTP_DEBUG
printf("FTP: list %s %s\n", dirPath, (char *)&payload->data[offset-1]);
printf("FTP: list %s %s\n", dirPath, (char *)&payload->data[offset - 1]);
#endif
offset += nameLen + 1;
}
@@ -431,7 +445,7 @@ MavlinkFTP::_workList(PayloadHeader* payload, bool list_hidden)
/// @brief Responds to an Open command
MavlinkFTP::ErrorCode
MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
MavlinkFTP::_workOpen(PayloadHeader *payload, int oflag)
{
if (_session_info.fd >= 0) {
warnx("FTP: Open failed - out of sessions\n");
@@ -439,41 +453,47 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
}
char *filename = _data_as_cstring(payload);
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: open '%s'", filename);
#endif
uint32_t fileSize = 0;
struct stat st;
if (stat(filename, &st) != 0) {
// fail only if requested open for read
if (oflag & O_RDONLY)
if (oflag & O_RDONLY) {
return kErrFailErrno;
else
} else {
st.st_size = 0;
}
}
fileSize = st.st_size;
// Set mode to 666 incase oflag has O_CREAT
int fd = ::open(filename, oflag, PX4_O_MODE_666);
if (fd < 0) {
return kErrFailErrno;
}
_session_info.fd = fd;
_session_info.file_size = fileSize;
_session_info.stream_download = false;
payload->session = 0;
payload->size = sizeof(uint32_t);
*((uint32_t*)payload->data) = fileSize;
*((uint32_t *)payload->data) = fileSize;
return kErrNone;
}
/// @brief Responds to a Read command
MavlinkFTP::ErrorCode
MavlinkFTP::_workRead(PayloadHeader* payload)
MavlinkFTP::_workRead(PayloadHeader *payload)
{
if (payload->session != 0 || _session_info.fd < 0) {
return kErrInvalidSession;
@@ -482,18 +502,20 @@ MavlinkFTP::_workRead(PayloadHeader* payload)
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: read offset:%d", payload->offset);
#endif
// We have to test seek past EOF ourselves, lseek will allow seek past EOF
if (payload->offset >= _session_info.file_size) {
warnx("request past EOF");
return kErrEOF;
}
if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) {
warnx("seek fail");
return kErrFailErrno;
}
int bytes_read = ::read(_session_info.fd, &payload->data[0], kMaxDataLength);
if (bytes_read < 0) {
// Negative return indicates error other than eof
warnx("read fail %d", bytes_read);
@@ -507,12 +529,12 @@ MavlinkFTP::_workRead(PayloadHeader* payload)
/// @brief Responds to a Stream command
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) {
return kErrInvalidSession;
}
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: burst offset:%d", payload->offset);
#endif
@@ -528,7 +550,7 @@ MavlinkFTP::_workBurst(PayloadHeader* payload, uint8_t target_system_id)
/// @brief Responds to a Write command
MavlinkFTP::ErrorCode
MavlinkFTP::_workWrite(PayloadHeader* payload)
MavlinkFTP::_workWrite(PayloadHeader *payload)
{
if (payload->session != 0 && _session_info.fd < 0) {
return kErrInvalidSession;
@@ -541,6 +563,7 @@ MavlinkFTP::_workWrite(PayloadHeader* payload)
}
int bytes_written = ::write(_session_info.fd, &payload->data[0], payload->size);
if (bytes_written < 0) {
// Negative return indicates error other than eof
warnx("write fail %d", bytes_written);
@@ -548,21 +571,22 @@ MavlinkFTP::_workWrite(PayloadHeader* payload)
}
payload->size = sizeof(uint32_t);
*((uint32_t*)payload->data) = bytes_written;
*((uint32_t *)payload->data) = bytes_written;
return kErrNone;
}
/// @brief Responds to a RemoveFile command
MavlinkFTP::ErrorCode
MavlinkFTP::_workRemoveFile(PayloadHeader* payload)
MavlinkFTP::_workRemoveFile(PayloadHeader *payload)
{
char file[kMaxDataLength];
strncpy(file, _data_as_cstring(payload), kMaxDataLength);
if (unlink(file) == 0) {
payload->size = 0;
return kErrNone;
} else {
return kErrFailErrno;
}
@@ -570,7 +594,7 @@ MavlinkFTP::_workRemoveFile(PayloadHeader* payload)
/// @brief Responds to a TruncateFile command
MavlinkFTP::ErrorCode
MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
MavlinkFTP::_workTruncateFile(PayloadHeader *payload)
{
char file[kMaxDataLength];
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.
struct stat st;
if (stat(file, &st) != 0) {
return kErrFailErrno;
}
@@ -599,20 +624,22 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
if (payload->offset == (unsigned)st.st_size) {
// nothing to do
return kErrNone;
}
else if (payload->offset == 0) {
} else if (payload->offset == 0) {
// 1: truncate all data
int fd = ::open(file, O_TRUNC | O_WRONLY);
if (fd < 0) {
return kErrFailErrno;
}
::close(fd);
return kErrNone;
}
else if (payload->offset > (unsigned)st.st_size) {
} else if (payload->offset > (unsigned)st.st_size) {
// 2: extend file
int fd = ::open(file, O_WRONLY);
if (fd < 0) {
return kErrFailErrno;
}
@@ -625,16 +652,18 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
bool ok = 1 == ::write(fd, "", 1);
::close(fd);
return (ok)? kErrNone : kErrFailErrno;
}
else {
return (ok) ? kErrNone : kErrFailErrno;
} else {
// 3: truncate
if (_copy_file(file, temp_file, payload->offset) != 0) {
return kErrFailErrno;
}
if (_copy_file(temp_file, file, payload->offset) != 0) {
return kErrFailErrno;
}
if (::unlink(temp_file) != 0) {
return kErrFailErrno;
}
@@ -645,16 +674,16 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
/// @brief Responds to a Terminate command
MavlinkFTP::ErrorCode
MavlinkFTP::_workTerminate(PayloadHeader* payload)
MavlinkFTP::_workTerminate(PayloadHeader *payload)
{
if (payload->session != 0 || _session_info.fd < 0) {
return kErrInvalidSession;
}
::close(_session_info.fd);
_session_info.fd = -1;
_session_info.stream_download = false;
payload->size = 0;
return kErrNone;
@@ -662,7 +691,7 @@ MavlinkFTP::_workTerminate(PayloadHeader* payload)
/// @brief Responds to a Reset command
MavlinkFTP::ErrorCode
MavlinkFTP::_workReset(PayloadHeader* payload)
MavlinkFTP::_workReset(PayloadHeader *payload)
{
if (_session_info.fd != -1) {
::close(_session_info.fd);
@@ -671,13 +700,13 @@ MavlinkFTP::_workReset(PayloadHeader* payload)
}
payload->size = 0;
return kErrNone;
}
/// @brief Responds to a Rename command
MavlinkFTP::ErrorCode
MavlinkFTP::_workRename(PayloadHeader* payload)
MavlinkFTP::_workRename(PayloadHeader *payload)
{
char oldpath[kMaxDataLength];
char newpath[kMaxDataLength];
@@ -697,6 +726,7 @@ MavlinkFTP::_workRename(PayloadHeader* payload)
if (rename(oldpath, newpath) == 0) {
payload->size = 0;
return kErrNone;
} else {
return kErrFailErrno;
}
@@ -704,14 +734,15 @@ MavlinkFTP::_workRename(PayloadHeader* payload)
/// @brief Responds to a RemoveDirectory command
MavlinkFTP::ErrorCode
MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload)
MavlinkFTP::_workRemoveDirectory(PayloadHeader *payload)
{
char dir[kMaxDataLength];
strncpy(dir, _data_as_cstring(payload), kMaxDataLength);
if (rmdir(dir) == 0) {
payload->size = 0;
return kErrNone;
} else {
return kErrFailErrno;
}
@@ -719,14 +750,15 @@ MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload)
/// @brief Responds to a CreateDirectory command
MavlinkFTP::ErrorCode
MavlinkFTP::_workCreateDirectory(PayloadHeader* payload)
MavlinkFTP::_workCreateDirectory(PayloadHeader *payload)
{
char dir[kMaxDataLength];
strncpy(dir, _data_as_cstring(payload), kMaxDataLength);
if (mkdir(dir, S_IRWXU | S_IRWXG | S_IRWXO) == 0) {
payload->size = 0;
return kErrNone;
} else {
return kErrFailErrno;
}
@@ -734,7 +766,7 @@ MavlinkFTP::_workCreateDirectory(PayloadHeader* payload)
/// @brief Responds to a CalcFileCRC32 command
MavlinkFTP::ErrorCode
MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload)
MavlinkFTP::_workCalcFileCRC32(PayloadHeader *payload)
{
char file_buf[256];
uint32_t checksum = 0;
@@ -742,12 +774,14 @@ MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload)
strncpy(file_buf, _data_as_cstring(payload), kMaxDataLength);
int fd = ::open(file_buf, O_RDONLY);
if (fd < 0) {
return kErrFailErrno;
}
do {
bytes_read = ::read(fd, file_buf, sizeof(file_buf));
if (bytes_read < 0) {
int r_errno = errno;
::close(fd);
@@ -755,30 +789,31 @@ MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload)
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));
::close(fd);
payload->size = sizeof(uint32_t);
*((uint32_t*)payload->data) = checksum;
*((uint32_t *)payload->data) = checksum;
return kErrNone;
}
/// @brief Guarantees that the payload data is null terminated.
/// @return Returns a pointer to the payload data as a char *
char *
MavlinkFTP::_data_as_cstring(PayloadHeader* payload)
MavlinkFTP::_data_as_cstring(PayloadHeader *payload)
{
// guarantee nul termination
if (payload->size < kMaxDataLength) {
payload->data[payload->size] = '\0';
} else {
payload->data[kMaxDataLength - 1] = '\0';
}
// and return data
return (char *)&(payload->data[0]);
return (char *) & (payload->data[0]);
}
/// @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;
src_fd = ::open(src_path, O_RDONLY);
if (src_fd < 0) {
return -1;
}
@@ -799,7 +835,8 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length
#ifdef __PX4_POSIX
, 0666
#endif
);
);
if (dst_fd < 0) {
op_errno = errno;
::close(src_fd);
@@ -809,20 +846,22 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length
while (length > 0) {
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);
if (bytes_read == 0) {
// EOF
break;
}
else if (bytes_read < 0) {
} else if (bytes_read < 0) {
warnx("cp: read");
op_errno = errno;
break;
}
bytes_written = ::write(dst_fd, buff, bytes_read);
if (bytes_written != bytes_read) {
warnx("cp: short write");
op_errno = errno;
@@ -836,7 +875,7 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length
::close(dst_fd);
errno = op_errno;
return (length > 0)? -1 : 0;
return (length > 0) ? -1 : 0;
}
void MavlinkFTP::send(const hrt_abstime t)
@@ -845,29 +884,32 @@ void MavlinkFTP::send(const hrt_abstime t)
if (!_session_info.stream_download) {
return;
}
#ifndef MAVLINK_FTP_UNIT_TEST
// Skip send if not enough room
unsigned max_bytes_to_send = _mavlink->get_free_tx_buf();
#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
if (max_bytes_to_send < get_size()) {
return;
}
#endif
// Send stream packets until buffer is full
bool more_data;
do {
more_data = false;
ErrorCode error_code = kErrNone;
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->session = 0;
payload->opcode = kRspAck;
@@ -878,6 +920,7 @@ void MavlinkFTP::send(const hrt_abstime t)
#ifdef MAVLINK_FTP_DEBUG
warnx("stream send: offset %d", _session_info.stream_offset);
#endif
// We have to test seek past EOF ourselves, lseek will allow seek past EOF
if (_session_info.stream_offset >= _session_info.file_size) {
error_code = kErrEOF;
@@ -885,7 +928,7 @@ void MavlinkFTP::send(const hrt_abstime t)
warnx("stream download: sending Nak EOF");
#endif
}
if (error_code == kErrNone) {
if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) {
error_code = kErrFailErrno;
@@ -894,43 +937,51 @@ void MavlinkFTP::send(const hrt_abstime t)
#endif
}
}
if (error_code == kErrNone) {
int bytes_read = ::read(_session_info.fd, &payload->data[0], kMaxDataLength);
if (bytes_read < 0) {
// Negative return indicates error other than eof
error_code = kErrFailErrno;
#ifdef MAVLINK_FTP_DEBUG
warnx("stream download: read fail");
#endif
} else {
payload->size = bytes_read;
_session_info.stream_offset += bytes_read;
_session_info.stream_chunk_transmitted += bytes_read;
}
}
if (error_code != kErrNone) {
payload->opcode = kRspNak;
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
if (error_code == kErrFailErrno) {
int r_errno = errno;
payload->size = 2;
payload->data[1] = r_errno;
}
_session_info.stream_download = false;
} else {
#ifndef MAVLINK_FTP_UNIT_TEST
if (max_bytes_to_send < (get_size()*2)) {
if (max_bytes_to_send < (get_size() * 2)) {
more_data = false;
/* perform transfers in 35K chunks - this is determined empirical */
if (_session_info.stream_chunk_transmitted > 35000) {
payload->burst_complete = true;
_session_info.stream_download = false;
_session_info.stream_chunk_transmitted = 0;
}
} else {
#endif
more_data = true;
@@ -938,9 +989,10 @@ void MavlinkFTP::send(const hrt_abstime t)
#ifndef MAVLINK_FTP_UNIT_TEST
max_bytes_to_send -= get_size();
}
#endif
}
ftp_msg.target_system = _session_info.stream_target_system_id;
_reply(&ftp_msg);
} while (more_data);
src/modules/mavlink/mavlink_ftp.h
+30 -33
View File
@@ -35,7 +35,7 @@
/// @file mavlink_ftp.h
/// @author px4dev, Don Gagne <don@thegagnes.com>
#include <dirent.h>
#include <queue.h>
@@ -55,12 +55,12 @@ public:
~MavlinkFTP();
static MavlinkStream *new_instance(Mavlink *mavlink);
/// Handle possible FTP message
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.
/// @param rcvmsgFunc Function which will be called to handle outgoing mavlink messages.
/// @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
/// 32 bit alignment to avoid usage of any pack pragmas.
struct PayloadHeader
{
struct PayloadHeader {
uint16_t seq_number; ///< sequence number for message
uint8_t session; ///< Session id for read and write commands
uint8_t opcode; ///< Command opcode
@@ -79,11 +78,10 @@ public:
uint8_t padding; ///< 32 bit aligment padding
uint32_t offset; ///< Offsets for List and Read commands
uint8_t data[]; ///< command data, varies by Opcode
};
};
/// @brief Command opcodes
enum Opcode : uint8_t
{
enum Opcode : uint8_t {
kCmdNone, ///< ignored, always acked
kCmdTerminateSession, ///< Terminates open Read session
kCmdResetSessions, ///< Terminates all open Read sessions
@@ -100,14 +98,13 @@ public:
kCmdRename, ///< Rename <path1> to <path2>
kCmdCalcFileCRC32, ///< Calculate CRC32 for file at <path>
kCmdBurstReadFile, ///< Burst download session file
kRspAck = 128, ///< Ack response
kRspNak ///< Nak response
};
/// @brief Error codes returned in Nak response PayloadHeader.data[0].
enum ErrorCode : uint8_t
{
enum ErrorCode : uint8_t {
kErrNone,
kErrFail, ///< Unknown failure
kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1]
@@ -116,48 +113,48 @@ public:
kErrNoSessionsAvailable, ///< All available Sessions in use
kErrEOF, ///< Offset past end of file for List and Read commands
kErrUnknownCommand ///< Unknown command opcode
};
};
// MavlinkStream overrides
virtual const char *get_name(void) const;
virtual uint8_t get_id(void);
virtual unsigned get_size(void);
private:
char *_data_as_cstring(PayloadHeader* payload);
void _process_request(mavlink_file_transfer_protocol_t* ftp_req, uint8_t target_system_id);
void _reply(mavlink_file_transfer_protocol_t* ftp_req);
char *_data_as_cstring(PayloadHeader *payload);
void _process_request(mavlink_file_transfer_protocol_t *ftp_req, uint8_t target_system_id);
void _reply(mavlink_file_transfer_protocol_t *ftp_req);
int _copy_file(const char *src_path, const char *dst_path, size_t length);
ErrorCode _workList(PayloadHeader *payload, bool list_hidden = false);
ErrorCode _workOpen(PayloadHeader *payload, int oflag);
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 _workTerminate(PayloadHeader *payload);
ErrorCode _workReset(PayloadHeader* payload);
ErrorCode _workReset(PayloadHeader *payload);
ErrorCode _workRemoveDirectory(PayloadHeader *payload);
ErrorCode _workCreateDirectory(PayloadHeader *payload);
ErrorCode _workRemoveFile(PayloadHeader *payload);
ErrorCode _workTruncateFile(PayloadHeader *payload);
ErrorCode _workRename(PayloadHeader *payload);
ErrorCode _workCalcFileCRC32(PayloadHeader *payload);
uint8_t _getServerSystemId(void);
uint8_t _getServerComponentId(void);
uint8_t _getServerChannel(void);
// Overrides from MavlinkStream
virtual void send(const hrt_abstime t);
static const char kDirentFile = 'F'; ///< Identifies File returned from List command
static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command
static const char kDirentSkip = 'S'; ///< Identifies Skipped entry from List command
/// @brief Maximum data size in RequestHeader::data
static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader);
struct SessionInfo {
int fd;
uint32_t file_size;
@@ -168,15 +165,15 @@ private:
unsigned stream_chunk_transmitted;
};
struct SessionInfo _session_info; ///< Session info, fd=-1 for no active session
ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending
void *_worker_data; ///< Additional parameter to _utRcvMsgFunc;
/* do not allow copying this class */
MavlinkFTP(const MavlinkFTP&);
MavlinkFTP operator=(const MavlinkFTP&);
MavlinkFTP(const MavlinkFTP &);
MavlinkFTP operator=(const MavlinkFTP &);
// Mavlink test needs to be able to call send
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
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;
if (stat(file, &st) == 0) {
if (date) *date = st.st_mtime;
if (size) *size = st.st_size;
if (date) { *date = st.st_mtime; }
if (size) { *size = st.st_size; }
return true;
}
return false;
}
@@ -97,12 +102,15 @@ MavlinkLogHandler::handle_message(const mavlink_message_t *msg)
case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
_log_request_list(msg);
break;
case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
_log_request_data(msg);
break;
case MAVLINK_MSG_ID_LOG_ERASE:
_log_request_erase(msg);
break;
case MAVLINK_MSG_ID_LOG_REQUEST_END:
_log_request_end(msg);
break;
@@ -110,7 +118,7 @@ MavlinkLogHandler::handle_message(const mavlink_message_t *msg)
}
//-------------------------------------------------------------------
const char*
const char *
MavlinkLogHandler::get_name(void) const
{
return "MAVLINK_LOG_HANDLER";
@@ -130,10 +138,12 @@ MavlinkLogHandler::get_size(void)
//-- Sending Log Entries
if (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_LISTING) {
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) {
return MAVLINK_MSG_ID_LOG_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
//-- Idle
//-- Idle
} else {
return 0;
}
@@ -144,14 +154,18 @@ void
MavlinkLogHandler::send(const hrt_abstime /*t*/)
{
//-- 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;
//-- 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();
}
//-- 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();
}
}
@@ -162,29 +176,35 @@ MavlinkLogHandler::_log_request_list(const mavlink_message_t *msg)
{
mavlink_log_request_list_t request;
mavlink_msg_log_request_list_decode(msg, &request);
//-- Check for re-requests (data loss) or new request
if(_pLogHandlerHelper) {
if (_pLogHandlerHelper) {
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
//-- Is this a new request?
if((request.end - request.start) > _pLogHandlerHelper->log_count) {
if ((request.end - request.start) > _pLogHandlerHelper->log_count) {
delete _pLogHandlerHelper;
_pLogHandlerHelper = NULL;
}
}
if(!_pLogHandlerHelper) {
if (!_pLogHandlerHelper) {
//-- Prepare new request
_pLogHandlerHelper = new LogListHelper;
}
if (_pLogHandlerHelper->log_count)
{
if (_pLogHandlerHelper->log_count) {
//-- Define (and clamp) range
_pLogHandlerHelper->next_entry = request.start < _pLogHandlerHelper->log_count ? request.start : _pLogHandlerHelper->log_count - 1;
_pLogHandlerHelper->last_entry = request.end < _pLogHandlerHelper->log_count ? request.end : _pLogHandlerHelper->log_count - 1;
_pLogHandlerHelper->next_entry = request.start < _pLogHandlerHelper->log_count ? request.start :
_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",
_pLogHandlerHelper->next_entry,
_pLogHandlerHelper->last_entry,
_pLogHandlerHelper->log_count);
_pLogHandlerHelper->next_entry,
_pLogHandlerHelper->last_entry,
_pLogHandlerHelper->log_count);
//-- Enable streaming
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_LISTING;
}
@@ -194,44 +214,54 @@ void
MavlinkLogHandler::_log_request_data(const mavlink_message_t *msg)
{
//-- 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");
return;
}
mavlink_log_request_data_t request;
mavlink_msg_log_request_data_decode(msg, &request);
//-- Does the requested log exist?
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;
}
//-- If we were sending log entries, stop it
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
if (_pLogHandlerHelper->current_log_index != request.id) {
//-- Init send log dataset
_pLogHandlerHelper->current_log_filename[0] = 0;
_pLogHandlerHelper->current_log_index = request.id;
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->current_log_data_offset = request.ofs;
if (_pLogHandlerHelper->current_log_data_offset >= _pLogHandlerHelper->current_log_size) {
_pLogHandlerHelper->current_log_data_remaining = 0;
} else {
_pLogHandlerHelper->current_log_data_remaining = _pLogHandlerHelper->current_log_size - request.ofs;
}
if (_pLogHandlerHelper->current_log_data_remaining > request.count) {
_pLogHandlerHelper->current_log_data_remaining = request.count;
}
//-- Enable streaming
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_SENDING_DATA;
}
//-------------------------------------------------------------------
void
MavlinkLogHandler::_log_request_erase(const mavlink_message_t* /*msg*/)
MavlinkLogHandler::_log_request_erase(const mavlink_message_t * /*msg*/)
{
/*
mavlink_log_erase_t request;
@@ -241,36 +271,43 @@ MavlinkLogHandler::_log_request_erase(const mavlink_message_t* /*msg*/)
delete _pLogHandlerHelper;
_pLogHandlerHelper = 0;
}
//-- Delete all logs
LogListHelper::delete_all(kLogRoot);
//-- Now delete all "msgs_*" from root
DIR* dp = opendir(kSDRoot);
DIR *dp = opendir(kSDRoot);
if (dp) {
struct dirent entry, *result = nullptr;
while (readdir_r(dp, &entry, &result) == 0) {
// no more entries?
if (!result) {
break;
}
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];
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);
}
}
}
}
closedir(dp);
}
}
//-------------------------------------------------------------------
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");
if (_pLogHandlerHelper) {
delete _pLogHandlerHelper;
_pLogHandlerHelper = 0;
@@ -290,19 +327,22 @@ MavlinkLogHandler::_log_send_listing()
response.num_logs = _pLogHandlerHelper->log_count;
response.last_log_num = _pLogHandlerHelper->last_entry;
mavlink_msg_log_entry_send_struct(_mavlink->get_channel(), &response);
//-- If we're done listing, flag it.
if (_pLogHandlerHelper->next_entry == _pLogHandlerHelper->last_entry) {
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
} else {
_pLogHandlerHelper->next_entry++;
}
PX4LOG_WARN("MavlinkLogHandler::_log_send_listing id: %u count: %u last: %u size: %u date: %u status: %d\n",
response.id,
response.num_logs,
response.last_log_num,
response.size,
response.time_utc,
_pLogHandlerHelper->current_status);
response.id,
response.num_logs,
response.last_log_num,
response.size,
response.time_utc,
_pLogHandlerHelper->current_status);
return sizeof(response);
}
@@ -313,9 +353,11 @@ MavlinkLogHandler::_log_send_data()
mavlink_log_data_t response;
memset(&response, 0, sizeof(response));
uint32_t len = _pLogHandlerHelper->current_log_data_remaining;
if (len > sizeof(response.data)) {
len = sizeof(response.data);
}
size_t read_size = _pLogHandlerHelper->get_log_data(len, response.data);
response.ofs = _pLogHandlerHelper->current_log_data_offset;
response.id = _pLogHandlerHelper->current_log_index;
@@ -323,9 +365,11 @@ MavlinkLogHandler::_log_send_data()
mavlink_msg_log_data_send_struct(_mavlink->get_channel(), &response);
_pLogHandlerHelper->current_log_data_offset += read_size;
_pLogHandlerHelper->current_log_data_remaining -= read_size;
if (read_size < sizeof(response.data) || _pLogHandlerHelper->current_log_data_remaining == 0) {
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
}
return sizeof(response);
}
@@ -354,33 +398,39 @@ LogListHelper::~LogListHelper()
//-------------------------------------------------------------------
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()
size = 0;
date = 0;
bool result = false;
//-- Open list of log files
FILE* f = ::fopen(kLogData, "r");
FILE *f = ::fopen(kLogData, "r");
if (f) {
//--- Find requested entry
char line[160];
int count = 0;
while (fgets(line, sizeof(line), f)) {
//-- Found our "index"
if(count++ == idx) {
if (count++ == idx) {
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);
}
result = true;
break;
}
}
}
fclose(f);
}
return result;
}
@@ -392,30 +442,38 @@ LogListHelper::open_for_transmit()
::fclose(current_log_filep);
current_log_filep = 0;
}
current_log_filep = ::fopen(current_log_filename, "rb");
if (!current_log_filep) {
PX4LOG_WARN("MavlinkLogHandler::open_for_transmit Could not open %s\n", current_log_filename);
return false;
}
return true;
}
//-------------------------------------------------------------------
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;
}
if (!current_log_filep) {
PX4LOG_WARN("MavlinkLogHandler::get_log_data file not open %s\n", current_log_filename);
return 0;
}
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);
PX4LOG_WARN("MavlinkLogHandler::get_log_data Seek error in %s\n", current_log_filename);
return 0;
}
size_t result = fread(buffer, 1, len, current_log_filep);
return result;
}
@@ -436,36 +494,44 @@ LogListHelper::_init()
unlink(kLogData);
// Open log directory
DIR *dp = opendir(kLogRoot);
if (dp == nullptr) {
// No log directory. Nothing to do.
return;
}
// Create work file
FILE* f = ::fopen(kTmpData, "w");
FILE *f = ::fopen(kTmpData, "w");
if (!f) {
PX4LOG_WARN("MavlinkLogHandler::init Error creating %s\n", kTmpData);
closedir(dp);
return;
}
// Scan directory and collect log files
struct dirent entry, *result = nullptr;
while (readdir_r(dp, &entry, &result) == 0) {
// no more entries?
if (result == nullptr) {
break;
}
if (entry.d_type == PX4LOG_DIRECTORY)
{
if (entry.d_type == PX4LOG_DIRECTORY) {
time_t tt = 0;
char log_path[128];
snprintf(log_path, sizeof(log_path), "%s/%s", kLogRoot, entry.d_name);
if (_get_session_date(log_path, entry.d_name, tt)) {
_scan_logs(f, log_path, tt);
}
}
}
closedir(dp);
fclose(f);
// Rename temp file to data file
if (rename(kTmpData, kLogData)) {
PX4LOG_WARN("MavlinkLogHandler::init Error renaming %s\n", kTmpData);
@@ -475,20 +541,23 @@ LogListHelper::_init()
//-------------------------------------------------------------------
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)
if (strncmp(dir, "sess", 4) == 0) {
unsigned u;
if(sscanf(&dir[4], "%u", &u) == 1) {
if (sscanf(&dir[4], "%u", &u) == 1) {
date = u * 60 * 60 * 24;
return true;
}
} else {
if (stat_file(path, &date)) {
return true;
}
/* strptime not available for some reason
// Get date from directory name
struct tm tt;
@@ -499,27 +568,32 @@ LogListHelper::_get_session_date(const char* path, const char* dir, time_t& date
*/
}
}
return false;
}
//-------------------------------------------------------------------
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);
if (dp) {
struct dirent entry, *result = nullptr;
while (readdir_r(dp, &entry, &result) == 0) {
// no more entries?
if (result == nullptr) {
break;
}
if (entry.d_type == PX4LOG_REGULAR_FILE) {
time_t ldate = date;
uint32_t size = 0;
char log_file_path[128];
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
fprintf(f, "%u %u %s\n", (unsigned)ldate, (unsigned)size, log_file_path);
log_count++;
@@ -531,23 +605,27 @@ LogListHelper::_scan_logs(FILE* f, const char* dir, time_t& date)
//-------------------------------------------------------------------
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(strstr(file, ".px4log") || strstr(file, ".ulg")) {
if (file && file[0]) {
if (strstr(file, ".px4log") || strstr(file, ".ulg")) {
// Convert "log000" to 00:00 (minute per flight in session)
if (strncmp(file, "log", 3) == 0) {
unsigned u;
if(sscanf(&file[3], "%u", &u) == 1) {
if (sscanf(&file[3], "%u", &u) == 1) {
date += (u * 60);
if (stat_file(path, 0, &size)) {
return true;
}
}
} else {
if (stat_file(path, &date, &size)) {
return true;
}
/* strptime not available for some reason
// Get time from file name
struct tm tt;
@@ -559,39 +637,48 @@ LogListHelper::_get_log_time_size(const char* path, const char* file, time_t& da
}
}
}
return false;
}
//-------------------------------------------------------------------
void
LogListHelper::delete_all(const char* dir)
LogListHelper::delete_all(const char *dir)
{
//-- Open log directory
DIR* dp = opendir(dir);
DIR *dp = opendir(dir);
if (dp == nullptr) {
return;
}
struct dirent entry, *result = nullptr;
while (readdir_r(dp, &entry, &result) == 0) {
// no more entries?
if (result == nullptr) {
break;
}
if (entry.d_type == PX4LOG_DIRECTORY && entry.d_name[0] != '.') {
char log_path[128];
snprintf(log_path, sizeof(log_path), "%s/%s", dir, entry.d_name);
LogListHelper::delete_all(log_path);
if(rmdir(log_path)) {
if (rmdir(log_path)) {
PX4LOG_WARN("MavlinkLogHandler::delete_all Error removing %s\n", log_path);
}
}
if (entry.d_type == PX4LOG_REGULAR_FILE) {
char log_path[128];
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);
}
}
}
closedir(dp);
}
src/modules/mavlink/mavlink_log_handler.h
+19 -19
View File
@@ -53,13 +53,13 @@ public:
~LogListHelper();
public:
static void delete_all(const char* dir);
static void delete_all(const char *dir);
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();
size_t get_log_data (uint8_t len, uint8_t* buffer);
size_t get_log_data(uint8_t len, uint8_t *buffer);
enum {
LOG_HANDLER_IDLE,
@@ -76,14 +76,14 @@ public:
uint32_t current_log_size;
uint32_t current_log_data_offset;
uint32_t current_log_data_remaining;
FILE* current_log_filep;
FILE *current_log_filep;
char current_log_filename[128];
private:
void _init ();
bool _get_session_date (const char* path, 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);
void _init();
bool _get_session_date(const char *path, 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);
};
// MAVLink LOG_* Message Handler
@@ -95,23 +95,23 @@ public:
static MavlinkLogHandler *new_instance(Mavlink *mavlink);
// Handle possible LOG message
void handle_message (const mavlink_message_t *msg);
void handle_message(const mavlink_message_t *msg);
// Overrides from MavlinkStream
const char* get_name (void) const;
uint8_t get_id (void);
unsigned get_size (void);
void send (const hrt_abstime t);
const char *get_name(void) const;
uint8_t get_id(void);
unsigned get_size(void);
void send(const hrt_abstime t);
private:
void _log_message (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_erase (const mavlink_message_t *msg);
void _log_request_end (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_data(const mavlink_message_t *msg);
void _log_request_erase(const mavlink_message_t *msg);
void _log_request_end(const mavlink_message_t *msg);
size_t _log_send_listing();
size_t _log_send_data ();
size_t _log_send_data();
private:
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)
{
Mavlink* m = Mavlink::get_instance((unsigned)chan);
Mavlink *m = Mavlink::get_instance((unsigned)chan);
if (m != nullptr) {
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)
{
Mavlink* m = Mavlink::get_instance((unsigned)chan);
Mavlink *m = Mavlink::get_instance((unsigned)chan);
if (m != nullptr) {
(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* m = Mavlink::get_instance((unsigned)channel);
Mavlink *m = Mavlink::get_instance((unsigned)channel);
if (m != nullptr) {
return m->get_status();
} else {
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* m = Mavlink::get_instance((unsigned)channel);
Mavlink *m = Mavlink::get_instance((unsigned)channel);
if (m != nullptr) {
return m->get_buffer();
} else {
return nullptr;
}
@@ -219,7 +225,7 @@ Mavlink::Mavlink() :
_rate_txerr(0.0f),
_rate_rx(0.0f),
#ifdef __PX4_POSIX
_myaddr{},
_myaddr {},
_src_addr{},
_bcast_addr{},
_src_addr_initialized(false),
@@ -333,6 +339,7 @@ Mavlink::set_proto_version(unsigned version)
if (version == 1 || ((version == 0) && !_received_messages)) {
get_status()->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
} else if (version == 2) {
get_status()->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
}
@@ -440,7 +447,7 @@ Mavlink::destroy_all_instances()
while (_mavlink_instances) {
inst_to_del = _mavlink_instances;
LL_DELETE(_mavlink_instances, inst_to_del);
delete (inst_to_del);
delete(inst_to_del);
}
printf("\n");
@@ -621,11 +628,11 @@ int Mavlink::get_component_id()
int Mavlink::mavlink_open_uart(int baud, const char *uart_name)
{
#ifndef B460800
#define B460800 460800
#define B460800 460800
#endif
#ifndef B921600
#define B921600 921600
#define B921600 921600
#endif
/* 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 */
while (_mode == MAVLINK_MODE_CONFIG &&
hrt_absolute_time() < 1800U * 1000U) {
hrt_absolute_time() < 1800U * 1000U) {
usleep(50000);
}
@@ -856,15 +863,16 @@ Mavlink::get_free_tx_buf()
int buf_free = 0;
// 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;
} else {
// No FIONWRITE on Linux
#if !defined(__PX4_LINUX) && !defined(__PX4_DARWIN)
(void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
#else
//Linux cp210x does not support TIOCOUTQ
buf_free = 256;
//Linux cp210x does not support TIOCOUTQ
buf_free = 256;
#endif
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 */
if ((_mode != MAVLINK_MODE_ONBOARD) && broadcast_enabled() &&
(!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) {
find_broadcast_address();
@@ -923,6 +931,7 @@ Mavlink::send_packet()
PX4_ERR("sending broadcast failed, errno: %d: %s", errno, strerror(errno));
_broadcast_failed_warned = true;
}
} else {
_broadcast_failed_warned = false;
}
@@ -960,8 +969,9 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
if (get_protocol() == SERIAL) {
/* check if there is space in the buffer, let it overflow else */
unsigned buf_free = get_free_tx_buf();
if (buf_free < packet_len) {
/* no enough space in buffer to send */
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
pthread_mutex_unlock(&_send_mutex);
@@ -986,6 +996,7 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
ret = packet_len;
}
}
#endif
if (ret != (size_t) packet_len) {
@@ -1018,16 +1029,19 @@ Mavlink::find_broadcast_address()
ifconf.ifc_len = 0;
ret = ioctl(_socket_fd, SIOCGIFCONF, &ifconf);
if (ret != 0) {
PX4_WARN("getting required buffer size failed");
return;
}
#endif
PX4_DEBUG("need to allocate %d bytes", ifconf.ifc_len);
// Allocate buffer.
ifconf.ifc_req = (struct ifreq *)(new uint8_t[ifconf.ifc_len]);
if (ifconf.ifc_req == nullptr) {
PX4_ERR("Could not allocate ifconf buffer");
return;
@@ -1036,6 +1050,7 @@ Mavlink::find_broadcast_address()
memset(ifconf.ifc_req, 0, ifconf.ifc_len);
ret = ioctl(_socket_fd, SIOCGIFCONF, &ifconf);
if (ret != 0) {
PX4_ERR("getting network config failed");
delete[] ifconf.ifc_req;
@@ -1044,7 +1059,7 @@ Mavlink::find_broadcast_address()
size_t offset = 0;
// 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
// `continue` and `break`.
@@ -1055,17 +1070,17 @@ Mavlink::find_broadcast_address()
// the interface name size plus whatever is greater, either the
// sizeof sockaddr or ifr_addr.sa_len.
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
// On Linux, it's much easier to traverse the buffer, every entry
// has the constant length.
offset += sizeof(struct ifreq)
// On Linux, it's much easier to traverse the buffer, every entry
// has the constant length.
offset += sizeof(struct ifreq)
#endif
) {
// 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);
@@ -1079,9 +1094,13 @@ Mavlink::find_broadcast_address()
}
struct ifreq bc_ifreq;
memset(&bc_ifreq, 0, sizeof(bc_ifreq));
strncpy(bc_ifreq.ifr_name, cur_ifreq->ifr_name, IF_NAMESIZE);
ret = ioctl(_socket_fd, SIOCGIFBRDADDR, &bc_ifreq);
if (ret != 0) {
PX4_DEBUG("getting broadcast address failed for %s", cur_ifreq->ifr_name);
continue;
@@ -1109,6 +1128,7 @@ Mavlink::find_broadcast_address()
_bcast_addr.sin_addr = bc_addr;
_broadcast_address_found = true;
} else {
PX4_INFO("ignoring additional network interface %s, IP: %s",
cur_ifreq->ifr_name, inet_ntoa(sin_addr));
@@ -1119,9 +1139,11 @@ Mavlink::find_broadcast_address()
_bcast_addr.sin_port = htons(_remote_port);
int broadcast_opt = 1;
if (setsockopt(_socket_fd, SOL_SOCKET, SO_BROADCAST, &broadcast_opt, sizeof(broadcast_opt)) < 0) {
PX4_WARN("setting broadcast permission failed");
}
_broadcast_address_not_found_warned = false;
} else {
@@ -1162,6 +1184,7 @@ Mavlink::init_udp()
_src_addr.sin_family = AF_INET;
inet_aton("127.0.0.1", &_src_addr.sin_addr);
}
_src_addr.sin_port = htons(_remote_port);
#endif
@@ -1515,15 +1538,18 @@ Mavlink::get_rate_mult()
return _rate_mult;
}
MavlinkShell*
MavlinkShell *
Mavlink::get_shell()
{
if (!_mavlink_shell) {
_mavlink_shell = new MavlinkShell();
if (!_mavlink_shell) {
PX4_ERR("Failed to allocate a shell");
} else {
int ret = _mavlink_shell->start();
if (ret != 0) {
PX4_ERR("Failed to start shell (%i)", ret);
delete _mavlink_shell;
@@ -1590,14 +1616,17 @@ Mavlink::update_rate_mult()
/* scale down if we have a TX err rate suggesting link congestion */
if (_rate_txerr > 0.0f && !radio_critical) {
hardware_mult = (_rate_tx) / (_rate_tx + _rate_txerr);
} else if (radio_found && tstatus.telem_time != _last_hw_rate_timestamp) {
if (tstatus.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) {
/* this indicates link congestion, reduce rate by 20% */
hardware_mult *= 0.80f;
} else if (tstatus.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
/* this indicates link congestion, reduce rate by 2.5% */
hardware_mult *= 0.975f;
} else if (tstatus.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) {
/* this indicates spare bandwidth, increase by 2.5% */
hardware_mult *= 1.025f;
@@ -1644,7 +1673,7 @@ Mavlink::task_main(int argc, char *argv[])
int myoptind = 1;
const char *myoptarg = NULL;
#ifdef __PX4_POSIX
char* eptr;
char *eptr;
int temp_int_arg;
#endif
@@ -1676,38 +1705,49 @@ Mavlink::task_main(int argc, char *argv[])
break;
#ifdef __PX4_POSIX
case 'u':
temp_int_arg = strtoul(myoptarg, &eptr, 10);
if ( *eptr == '\0' ) {
if (*eptr == '\0') {
_network_port = temp_int_arg;
set_protocol(UDP);
} else {
warnx("invalid data udp_port '%s'", myoptarg);
err_flag = true;
}
break;
case 'o':
temp_int_arg = strtoul(myoptarg, &eptr, 10);
if ( *eptr == '\0' ) {
if (*eptr == '\0') {
_remote_port = temp_int_arg;
set_protocol(UDP);
} else {
warnx("invalid remote udp_port '%s'", myoptarg);
err_flag = true;
}
break;
case 't':
_src_addr.sin_family = AF_INET;
if (inet_aton(myoptarg, &_src_addr.sin_addr)) {
_src_addr_initialized = true;
} else {
warnx("invalid partner ip '%s'", myoptarg);
err_flag = true;
}
break;
#else
case 'u':
case 'o':
case 't':
@@ -1797,6 +1837,7 @@ Mavlink::task_main(int argc, char *argv[])
if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) {
warn("could not open %s", _device_name);
return ERROR;
} else if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) {
/* the config link is optional */
return OK;
@@ -1991,6 +2032,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("MISSION_ITEM", 50.0f);
configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
configure_stream("MANUAL_CONTROL", 5.0f);
default:
break;
}
@@ -2053,6 +2095,7 @@ Mavlink::task_main(int argc, char *argv[])
/* set channel */
fprintf(fs, "ATS3=%u\n", _radio_id);
usleep(200000);
} else {
/* reset to factory defaults */
fprintf(fs, "AT&F\n");
@@ -2073,6 +2116,7 @@ Mavlink::task_main(int argc, char *argv[])
// config thing, we leave the file struct
// allocated.
//fclose(fs);
} else {
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;
if (mavlink_log_sub->update_if_changed(&mavlink_log)) {
_logbuffer.put(&mavlink_log);
}
@@ -2118,26 +2163,29 @@ Mavlink::task_main(int argc, char *argv[])
if (_subscribe_to_stream != nullptr) {
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
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,
(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,
(double)_subscribe_to_stream_rate);
}
} else {
if ( get_protocol() == SERIAL ) {
if (get_protocol() == SERIAL) {
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);
}
}
} else {
if ( get_protocol() == SERIAL ) {
if (get_protocol() == SERIAL) {
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);
}
}
@@ -2302,7 +2350,7 @@ Mavlink::start(int argc, char *argv[])
if (ic == Mavlink::MAVLINK_MAX_INSTANCES) {
warnx("Maximum MAVLink instance count of %d reached.",
(int)Mavlink::MAVLINK_MAX_INSTANCES);
(int)Mavlink::MAVLINK_MAX_INSTANCES);
return 1;
}
@@ -2399,7 +2447,7 @@ Mavlink::stream_command(int argc, char *argv[])
float rate = -1.0f;
const char *stream_name = nullptr;
unsigned short network_port = 0;
char* eptr;
char *eptr;
int temp_int_arg;
bool provided_device = 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) {
stream_name = argv[i + 1];
i++;
} else if (0 == strcmp(argv[i], "-u") && i < argc - 1) {
provided_network_port = true;
temp_int_arg = strtoul(argv[i + 1], &eptr, 10);
if ( *eptr == '\0' ) {
if (*eptr == '\0') {
network_port = temp_int_arg;
} else {
err_flag = true;
}
i++;
} else {
err_flag = true;
}
@@ -2457,10 +2510,13 @@ Mavlink::stream_command(int argc, char *argv[])
if (!err_flag && rate >= 0.0f && stream_name != nullptr) {
Mavlink *inst = nullptr;
if (provided_device && !provided_network_port) {
inst = get_instance_for_device(device_name);
} else if (provided_network_port && !provided_device) {
inst = get_instance_for_network_port(network_port);
} else if (provided_device && provided_network_port) {
warnx("please provide either a device name or a network port");
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.
if (provided_device) {
warnx("mavlink for device %s is not running", device_name);
} else {
warnx("mavlink for network on port %hu is not running", network_port);
}
@@ -2499,8 +2556,8 @@ Mavlink::set_boot_complete()
Mavlink *inst;
LL_FOREACH(::_mavlink_instances, inst) {
if ((inst->get_mode() != MAVLINK_MODE_ONBOARD) &&
(!inst->broadcast_enabled()) &&
((inst->get_protocol() == UDP) || (inst->get_protocol() == TCP))) {
(!inst->broadcast_enabled()) &&
((inst->get_protocol() == UDP) || (inst->get_protocol() == TCP))) {
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();
return 1;
} else if (!strcmp(argv[1], "stop-all") ) {
} else if (!strcmp(argv[1], "stop-all")) {
return Mavlink::destroy_all_instances();
} else if (!strcmp(argv[1], "status")) {
src/modules/mavlink/mavlink_main.h
+32 -24
View File
@@ -170,22 +170,29 @@ public:
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) {
case MAVLINK_MODE_NORMAL:
return "Normal";
case MAVLINK_MODE_CUSTOM:
return "Custom";
case MAVLINK_MODE_ONBOARD:
return "Onboard";
case MAVLINK_MODE_OSD:
return "OSD";
case MAVLINK_MODE_MAGIC:
return "Magic";
case MAVLINK_MODE_CONFIG:
return "Config";
default:
return "Unknown";
case MAVLINK_MODE_NORMAL:
return "Normal";
case MAVLINK_MODE_CUSTOM:
return "Custom";
case MAVLINK_MODE_ONBOARD:
return "Onboard";
case MAVLINK_MODE_OSD:
return "OSD";
case MAVLINK_MODE_MAGIC:
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 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.
@@ -285,7 +293,7 @@ public:
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();
@@ -339,7 +347,7 @@ public:
void send_statustext(unsigned char severity, const char *string);
void send_autopilot_capabilites();
MavlinkStream * get_streams() const { return _streams; }
MavlinkStream *get_streams() const { return _streams; }
float get_rate_mult();
@@ -380,7 +388,7 @@ public:
/**
* 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; }
@@ -392,9 +400,9 @@ public:
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
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; }
@@ -419,7 +427,7 @@ public:
void set_logging_enabled(bool logging) { _logging_enabled = logging; }
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.
* Returns nullptr if shell cannot be created */
@@ -608,6 +616,6 @@ private:
int task_main(int argc, char *argv[]);
/* do not allow copying this class */
Mavlink(const Mavlink&);
Mavlink operator=(const Mavlink&);
Mavlink(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;
#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \
((_msg.target_component == mavlink_system.compid) || \
(_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
(_msg.target_component == MAV_COMP_ID_ALL)))
((_msg.target_component == mavlink_system.compid) || \
(_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
(_msg.target_component == MAV_COMP_ID_ALL)))
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_state(MAVLINK_WPM_STATE_IDLE),
@@ -120,9 +120,11 @@ void
MavlinkMissionManager::init_offboard_mission()
{
mission_s mission_state;
if (!_dataman_init) {
_dataman_init = true;
int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s);
if (ret > 0) {
_dataman_id = mission_state.dataman_id;
_count = mission_state.count;
@@ -132,6 +134,7 @@ MavlinkMissionManager::init_offboard_mission()
_dataman_id = 0;
_count = 0;
_current_seq = 0;
} else {
PX4_WARN("offboard mission init failed");
_dataman_id = 0;
@@ -139,6 +142,7 @@ MavlinkMissionManager::init_offboard_mission()
_current_seq = 0;
}
}
_my_dataman_id = _dataman_id;
}
@@ -176,6 +180,7 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int
} else {
warnx("WPM: ERROR: can't save mission state");
if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) {
_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 {
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) {
_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) {
_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);
return;
}
_transfer_in_progress = true;
if (wpc.count > _max_count) {
@@ -683,6 +689,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
}
struct mission_item_s mission_item = {};
int ret = parse_mavlink_mission_item(&wp, &mission_item);
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);
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); }
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 {
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
}
_transfer_in_progress = false;
} 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 (_verbose) { warnx("WPM: CLEAR_ALL OK"); }
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
} else {
@@ -768,10 +778,11 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
}
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 ||
mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
// 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) {
mission_item->altitude_is_relative = false;
} else if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
mission_item->altitude_is_relative = true;
}
@@ -897,7 +909,8 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
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->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) {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
} else {
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)
{
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"); }
_my_dataman_id=_dataman_id;
_my_dataman_id = _dataman_id;
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_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds
class MavlinkMissionManager : public MavlinkStream {
class MavlinkMissionManager : public MavlinkStream
{
public:
~MavlinkMissionManager();
@@ -113,7 +114,7 @@ private:
static int _dataman_id; ///< Global Dataman storage ID for active mission
int _my_dataman_id; ///< class Dataman storage ID
static bool _dataman_init; ///< Dataman initialized
static unsigned _count; ///< Count of items in active mission
static int _current_seq; ///< Current item sequence in active mission
@@ -133,11 +134,12 @@ private:
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 */
MavlinkMissionManager(MavlinkMissionManager &);
MavlinkMissionManager& operator = (const MavlinkMissionManager &);
MavlinkMissionManager &operator = (const MavlinkMissionManager &);
void init_offboard_mission();
@@ -196,7 +198,8 @@ private:
/**
* 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:
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.
uint64_t time_topic;
if (orb_stat(_fd, &time_topic)) {
/* error getting last topic publication time */
time_topic = 0;
@@ -90,11 +91,13 @@ MavlinkOrbSubscription::update(uint64_t *time, void *data)
/* error copying topic data */
memset(data, 0, _topic->o_size);
}
return false;
} else {
/* data copied successfully */
_published = true;
if (time_topic != *time) {
*time = time_topic;
return true;
@@ -115,6 +118,7 @@ bool
MavlinkOrbSubscription::update_if_changed(void *data)
{
bool updated;
if (orb_check(_fd, &updated)) {
return false;
}
@@ -128,6 +132,7 @@ MavlinkOrbSubscription::update_if_changed(void *data)
/* error copying topic data */
memset(data, 0, _topic->o_size);
}
return false;
}
src/modules/mavlink/mavlink_orb_subscription.h
+2 -2
View File
@@ -97,8 +97,8 @@ private:
bool _published; ///< topic was ever published
/* do not allow copying this class */
MavlinkOrbSubscription(const MavlinkOrbSubscription&);
MavlinkOrbSubscription operator=(const MavlinkOrbSubscription&);
MavlinkOrbSubscription(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) {
orb_unsubscribe(_uavcan_parameter_value_sub);
}
if (_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)) {
if (_send_all_index < 0) {
_send_all_index = PARAM_HASH;
} else {
/* a restart should skip the hash check on the ground */
_send_all_index = 0;
@@ -108,10 +110,12 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
}
break;
}
@@ -160,9 +164,11 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
req.param_index = -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';
if (set.param_type == MAV_PARAM_TYPE_REAL32) {
req.param_type = MAV_PARAM_TYPE_REAL32;
req.real_value = set.param_value;
} else {
int32_t val;
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) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
}
break;
}
@@ -202,6 +210,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
param_value.param_type = MAV_PARAM_TYPE_UINT32;
memcpy(&param_value.param_value, &hash, sizeof(hash));
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &param_value);
} else {
/* local name buffer to enforce null-terminated string */
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 */
send_param(param_find_no_notification(name));
}
} else {
/* when index is >= 0, send this parameter again */
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];
sprintf(buf, "[pm] unknown param ID: %u", req_read.param_index);
_mavlink->send_statustext_info(buf);
} else if (ret == 2) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
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 &&
(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.
uavcan_parameter_request_s req;
req.message_type = msg->msgid;
@@ -239,10 +250,12 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
}
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 */
size_t i = map_rc.parameter_rc_channel_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 */
_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.value0[i] = map_rc.param_value0;
_rc_param_map.value_min[i] = map_rc.param_value_min;
_rc_param_map.value_max[i] = map_rc.param_value_max;
if (map_rc.param_index == -2) { // -2 means unset map
_rc_param_map.valid[i] = false;
} else {
_rc_param_map.valid[i] = true;
}
_rc_param_map.timestamp = hrt_absolute_time();
if (_rc_param_map_pub == nullptr) {
@@ -280,6 +297,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
}
}
break;
}
@@ -300,6 +318,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
bool param_value_ready;
orb_check(_uavcan_parameter_value_sub, &param_value_ready);
if (space_available && param_value_ready) {
struct uavcan_parameter_value_s 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_index = value.param_index;
strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
if (value.param_type == MAV_PARAM_TYPE_REAL32) {
msg.param_type = MAVLINK_TYPE_FLOAT;
msg.param_value = value.real_value;
} else {
int32_t val;
val = (int32_t)value.int_value;
memcpy(&msg.param_value, &val, sizeof(int32_t));
msg.param_type = MAVLINK_TYPE_INT32_T;
}
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
} else if (_send_all_index >= 0 && _mavlink->boot_complete()) {
/* 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 */
param_t p;
do {
/* walk through all parameters, including unused ones */
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())) {
_send_all_index = -1;
}
} 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 */
_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 */
MavlinkParametersManager(MavlinkParametersManager &);
MavlinkParametersManager& operator = (const MavlinkParametersManager &);
MavlinkParametersManager &operator = (const MavlinkParametersManager &);
protected:
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
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;
}
MavlinkShell* shell = _mavlink->get_shell();
MavlinkShell *shell = _mavlink->get_shell();
if (shell) {
// we ignore the timeout, EXCLUSIVE & BLOCKING flags of the SERIAL_CONTROL message
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 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);
/**
src/modules/mavlink/mavlink_shell.cpp
+16 -7
View File
@@ -62,6 +62,7 @@ MavlinkShell::~MavlinkShell()
PX4_INFO("Stopping mavlink shell");
close(_to_shell_fd);
}
if (_from_shell_fd >= 0) {
close(_from_shell_fd);
}
@@ -91,6 +92,7 @@ int MavlinkShell::start()
if (pipe(p1) != 0) {
return -errno;
}
if (pipe(p2) != 0) {
close(p1[0]);
close(p1[1]);
@@ -105,22 +107,26 @@ int MavlinkShell::start()
_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
for (int i = 0; i < 2; ++i) {
fd_backups[i] = dup(i);
if (fd_backups[i] == -1) {
ret = -errno;
}
}
dup2(_shell_fds[0], 0);
dup2(_shell_fds[1], 1);
if (ret == 0) {
_task = px4_task_spawn_cmd("mavlink_shell",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
&MavlinkShell::shell_start_thread,
nullptr);
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
&MavlinkShell::shell_start_thread,
nullptr);
if (_task < 0) {
ret = -1;
}
@@ -131,6 +137,7 @@ int MavlinkShell::start()
if (dup2(fd_backups[i], i) == -1) {
ret = -errno;
}
close(fd_backups[i]);
}
@@ -152,12 +159,12 @@ int MavlinkShell::shell_start_thread(int argc, char *argv[])
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);
}
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);
}
@@ -165,8 +172,10 @@ size_t MavlinkShell::read(uint8_t* buffer, size_t len)
size_t MavlinkShell::available()
{
int ret = 0;
if (ioctl(_from_shell_fd, FIONREAD, (unsigned long)&ret) == OK) {
return ret;
}
return 0;
}
src/modules/mavlink/mavlink_shell.h
+3 -3
View File
@@ -64,14 +64,14 @@ public:
* Write to the shell
* @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.
* @param len buffer length
* @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.
@@ -82,7 +82,7 @@ private:
int _to_shell_fd = -1; /** fd to write to 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;
static int shell_start_thread(int argc, char *argv[]);