From 720c445f210aec9bd0ef9e25d94c6d8ed393a91f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Aug 2016 23:43:33 +0200 Subject: [PATCH] MAVLink: Code style --- src/modules/mavlink/mavlink_ftp.cpp | 210 +++-- src/modules/mavlink/mavlink_ftp.h | 63 +- src/modules/mavlink/mavlink_log_handler.cpp | 201 +++-- src/modules/mavlink/mavlink_log_handler.h | 38 +- src/modules/mavlink/mavlink_main.cpp | 131 +++- src/modules/mavlink/mavlink_main.h | 56 +- src/modules/mavlink/mavlink_messages.cpp | 723 +++++++++--------- src/modules/mavlink/mavlink_mission.cpp | 38 +- src/modules/mavlink/mavlink_mission.h | 13 +- .../mavlink/mavlink_orb_subscription.cpp | 5 + .../mavlink/mavlink_orb_subscription.h | 4 +- src/modules/mavlink/mavlink_parameters.cpp | 29 +- src/modules/mavlink/mavlink_parameters.h | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 5 +- src/modules/mavlink/mavlink_receiver.h | 2 +- src/modules/mavlink/mavlink_shell.cpp | 23 +- src/modules/mavlink/mavlink_shell.h | 6 +- 17 files changed, 922 insertions(+), 627 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 674fe66c83..ece85facba 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -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(&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(&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(&ftp_msg.payload[0]); - + PayloadHeader *payload = reinterpret_cast(&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); diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 017ac07d67..c2f3a87d75 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -35,7 +35,7 @@ /// @file mavlink_ftp.h /// @author px4dev, Don Gagne - + #include #include @@ -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 to kCmdCalcFileCRC32, ///< Calculate CRC32 for file at 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; }; diff --git a/src/modules/mavlink/mavlink_log_handler.cpp b/src/modules/mavlink/mavlink_log_handler.cpp index 0f75e0e49a..7a477d2d62 100644 --- a/src/modules/mavlink/mavlink_log_handler.cpp +++ b/src/modules/mavlink/mavlink_log_handler.cpp @@ -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); } diff --git a/src/modules/mavlink/mavlink_log_handler.h b/src/modules/mavlink/mavlink_log_handler.h index 231fc1d39f..a44791c311 100644 --- a/src/modules/mavlink/mavlink_log_handler.h +++ b/src/modules/mavlink/mavlink_log_handler.h @@ -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; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4daf2cd59c..d074423193 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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")) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index a0a2f72f39..cf4bb46765 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -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 &); }; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4d04e57ea3..16f856e55d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -94,7 +94,7 @@ static uint16_t cm_uint16_from_m_float(float m); static void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_state, - uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); uint16_t cm_uint16_from_m_float(float m) @@ -110,7 +110,7 @@ cm_uint16_from_m_float(float m) } void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_state, - uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) + uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { *mavlink_state = 0; *mavlink_base_mode = 0; @@ -134,106 +134,108 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_st custom_mode.data = 0; const uint8_t auto_mode_flags = MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; switch (status->nav_state) { - case vehicle_status_s::NAVIGATION_STATE_MANUAL: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - break; + case vehicle_status_s::NAVIGATION_STATE_MANUAL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; - case vehicle_status_s::NAVIGATION_STATE_ACRO: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; - break; + case vehicle_status_s::NAVIGATION_STATE_ACRO: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; + break; - case vehicle_status_s::NAVIGATION_STATE_RATTITUDE: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_RATTITUDE; - break; + case vehicle_status_s::NAVIGATION_STATE_RATTITUDE: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_RATTITUDE; + break; - case vehicle_status_s::NAVIGATION_STATE_STAB: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED; - break; + case vehicle_status_s::NAVIGATION_STATE_STAB: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED; + break; - case vehicle_status_s::NAVIGATION_STATE_ALTCTL: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; - break; + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; + break; - case vehicle_status_s::NAVIGATION_STATE_POSCTL: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; // TODO: is POSCTL GUIDED? - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; - break; + case vehicle_status_s::NAVIGATION_STATE_POSCTL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; // TODO: is POSCTL GUIDED? + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; + break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: - *mavlink_base_mode |= auto_mode_flags; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; - break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: + *mavlink_base_mode |= auto_mode_flags; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; + break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - *mavlink_base_mode |= auto_mode_flags; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + *mavlink_base_mode |= auto_mode_flags; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: - *mavlink_base_mode |= auto_mode_flags; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + *mavlink_base_mode |= auto_mode_flags; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: - *mavlink_base_mode |= auto_mode_flags; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET; - break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: + *mavlink_base_mode |= auto_mode_flags; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET; + break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: - /* fallthrough */ - case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: - *mavlink_base_mode |= auto_mode_flags; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; - break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: - case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: - case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: - case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: - /* fallthrough */ - case vehicle_status_s::NAVIGATION_STATE_DESCEND: - *mavlink_base_mode |= auto_mode_flags; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; - break; + /* fallthrough */ + case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: + *mavlink_base_mode |= auto_mode_flags; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: - *mavlink_base_mode |= auto_mode_flags; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; - break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: - case vehicle_status_s::NAVIGATION_STATE_TERMINATION: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - break; + /* fallthrough */ + case vehicle_status_s::NAVIGATION_STATE_DESCEND: + *mavlink_base_mode |= auto_mode_flags; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + break; - case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: - *mavlink_base_mode |= auto_mode_flags; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; - break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: + *mavlink_base_mode |= auto_mode_flags; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; + break; - case vehicle_status_s::NAVIGATION_STATE_MAX: - /* this is an unused case, ignore */ - break; + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; + + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: + *mavlink_base_mode |= auto_mode_flags; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; + break; + + case vehicle_status_s::NAVIGATION_STATE_MAX: + /* this is an unused case, ignore */ + break; } @@ -281,10 +283,10 @@ public: return MAVLINK_MSG_ID_HEARTBEAT; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -296,7 +298,8 @@ public: return MAVLINK_MSG_ID_HEARTBEAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } - bool const_rate() { + bool const_rate() + { return true; } @@ -305,7 +308,7 @@ private: /* do not allow top copying this class */ MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &); - MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &); + MavlinkStreamHeartbeat &operator = (const MavlinkStreamHeartbeat &); protected: explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -328,7 +331,7 @@ protected: get_mavlink_mode_state(&status, &system_status, &base_mode, &custom_mode); mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4, - base_mode, custom_mode, system_status); + base_mode, custom_mode, system_status); } }; @@ -350,24 +353,25 @@ public: return MAVLINK_MSG_ID_STATUSTEXT; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamStatustext(mavlink); } - unsigned get_size() { + unsigned get_size() + { return _mavlink->get_logbuffer()->empty() ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); } private: /* do not allow top copying this class */ MavlinkStreamStatustext(MavlinkStreamStatustext &); - MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &); + MavlinkStreamStatustext &operator = (const MavlinkStreamStatustext &); unsigned _write_err_count = 0; static const unsigned write_err_threshold = 5; @@ -379,11 +383,14 @@ protected: explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) {} - ~MavlinkStreamStatustext() { + ~MavlinkStreamStatustext() + { #ifndef __PX4_POSIX_EAGLE + if (_fp != nullptr) { fclose(_fp); } + #endif } @@ -393,6 +400,7 @@ protected: if (!_mavlink->get_logbuffer()->empty()) { struct mavlink_log_s mavlink_log; + if (_mavlink->get_logbuffer()->get(&mavlink_log)) { mavlink_statustext_t msg; @@ -419,8 +427,10 @@ protected: if (_fp != nullptr) { fputs(tstamp, _fp); fputs(": ", _fp); + if (EOF == fputs(msg.text, _fp)) { _write_err_count++; + } else { _write_err_count = 0; } @@ -429,6 +439,7 @@ protected: (void)fclose(_fp); _fp = nullptr; PX4_WARN("mavlink logging disabled"); + } else { (void)fputs("\n", _fp); #ifdef __PX4_NUTTX @@ -455,12 +466,14 @@ protected: #ifdef __PX4_NUTTX fsync(_fp->fs_filedes); #endif + } else { PX4_WARN("Failed to open MAVLink log: %s", log_file_path); _write_err_count = write_err_threshold; //only try to open the file once } } } + #endif } } @@ -485,17 +498,18 @@ public: return MAVLINK_MSG_ID_COMMAND_LONG; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCommandLong(mavlink); } - unsigned get_size() { + unsigned get_size() + { return 0; // commands stream is not regular and not predictable } @@ -505,7 +519,7 @@ private: /* do not allow top copying this class */ MavlinkStreamCommandLong(MavlinkStreamCommandLong &); - MavlinkStreamCommandLong& operator = (const MavlinkStreamCommandLong &); + MavlinkStreamCommandLong &operator = (const MavlinkStreamCommandLong &); protected: explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -553,15 +567,15 @@ public: return "SYS_STATUS"; } - static uint8_t get_id_static() + static uint8_t get_id_static() { return MAVLINK_MSG_ID_SYS_STATUS; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -580,7 +594,7 @@ private: /* do not allow top copying this class */ MavlinkStreamSysStatus(MavlinkStreamSysStatus &); - MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &); + MavlinkStreamSysStatus &operator = (const MavlinkStreamSysStatus &); protected: explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -602,6 +616,7 @@ protected: if (updated_status) { if (status.arming_state >= vehicle_status_s::ARMING_STATE_ARMED) { _mavlink->set_logging_enabled(true); + } else { _mavlink->set_logging_enabled(false); } @@ -637,9 +652,11 @@ protected: bat_msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100 : -1; bat_msg.battery_remaining = (battery_status.connected) ? battery_status.remaining * 100.0f : -1; bat_msg.temperature = INT16_MAX; + for (unsigned int i = 0; i < (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); i++) { if ((int)i < battery_status.cell_count && battery_status.connected) { bat_msg.voltages[i] = (battery_status.voltage_v / battery_status.cell_count) * 1000.0f; + } else { bat_msg.voltages[i] = UINT16_MAX; } @@ -669,10 +686,10 @@ public: return MAVLINK_MSG_ID_HIGHRES_IMU; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -698,7 +715,7 @@ private: /* do not allow top copying this class */ MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); - MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); + MavlinkStreamHighresIMU &operator = (const MavlinkStreamHighresIMU &); protected: explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -743,6 +760,7 @@ protected: fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); _baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative; } + _differential_pressure_sub->update(&_differential_pressure_time, &differential_pressure); mavlink_highres_imu_t msg; @@ -787,10 +805,10 @@ public: return MAVLINK_MSG_ID_ATTITUDE; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -808,7 +826,7 @@ private: /* do not allow top copying this class */ MavlinkStreamAttitude(MavlinkStreamAttitude &); - MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); + MavlinkStreamAttitude &operator = (const MavlinkStreamAttitude &); protected: @@ -856,10 +874,10 @@ public: return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -877,7 +895,7 @@ private: /* do not allow top copying this class */ MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); - MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); + MavlinkStreamAttitudeQuaternion &operator = (const MavlinkStreamAttitudeQuaternion &); protected: explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -926,10 +944,10 @@ public: return MAVLINK_MSG_ID_VFR_HUD; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -962,7 +980,7 @@ private: /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); - MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); + MavlinkStreamVFRHUD &operator = (const MavlinkStreamVFRHUD &); protected: explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1001,15 +1019,18 @@ protected: msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; + if (_pos_time > 0) { /* use global estimate */ msg.alt = pos.alt; + } else { /* fall back to baro altitude */ sensor_combined_s sensor; (void)_sensor_sub->update(&_sensor_time, &sensor); msg.alt = sensor.baro_alt_meter; } + msg.climb = -pos.vel_d; mavlink_msg_vfr_hud_send_struct(_mavlink->get_channel(), &msg); @@ -1036,10 +1057,10 @@ public: return MAVLINK_MSG_ID_GPS_RAW_INT; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -1057,7 +1078,7 @@ private: /* do not allow top copying this class */ MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); - MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); + MavlinkStreamGPSRawInt &operator = (const MavlinkStreamGPSRawInt &); protected: explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1080,8 +1101,8 @@ protected: msg.eph = gps.hdop * 100; //cm_uint16_from_m_float(gps.eph); msg.epv = gps.vdop * 100; //cm_uint16_from_m_float(gps.epv); msg.vel = cm_uint16_from_m_float(gps.vel_m_s), - msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, - msg.satellites_visible = gps.satellites_used; + msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + msg.satellites_visible = gps.satellites_used; mavlink_msg_gps_raw_int_send_struct(_mavlink->get_channel(), &msg); } @@ -1091,28 +1112,33 @@ protected: class MavlinkStreamSystemTime : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamSystemTime::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "SYSTEM_TIME"; } - static uint8_t get_id_static() { + static uint8_t get_id_static() + { return MAVLINK_MSG_ID_SYSTEM_TIME; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamSystemTime(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_SYSTEM_TIME_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1125,7 +1151,8 @@ protected: explicit MavlinkStreamSystemTime(Mavlink *mavlink) : MavlinkStream(mavlink) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { mavlink_system_time_t msg; timespec tv; @@ -1141,28 +1168,33 @@ protected: class MavlinkStreamTimesync : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamTimesync::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "TIMESYNC"; } - static uint8_t get_id_static() { + static uint8_t get_id_static() + { return MAVLINK_MSG_ID_TIMESYNC; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamTimesync(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_TIMESYNC_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1175,7 +1207,8 @@ protected: explicit MavlinkStreamTimesync(Mavlink *mavlink) : MavlinkStream(mavlink) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { mavlink_timesync_t msg; msg.tc1 = 0; @@ -1203,10 +1236,10 @@ public: return MAVLINK_MSG_ID_ADSB_VEHICLE; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -1224,7 +1257,7 @@ private: /* do not allow top copying this class */ MavlinkStreamADSBVehicle(MavlinkStreamADSBVehicle &); - MavlinkStreamADSBVehicle& operator = (const MavlinkStreamADSBVehicle &); + MavlinkStreamADSBVehicle &operator = (const MavlinkStreamADSBVehicle &); protected: explicit MavlinkStreamADSBVehicle(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1276,10 +1309,10 @@ public: return MAVLINK_MSG_ID_CAMERA_TRIGGER; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -1297,7 +1330,7 @@ private: /* do not allow top copying this class */ MavlinkStreamCameraTrigger(MavlinkStreamCameraTrigger &); - MavlinkStreamCameraTrigger& operator = (const MavlinkStreamCameraTrigger &); + MavlinkStreamCameraTrigger &operator = (const MavlinkStreamCameraTrigger &); protected: explicit MavlinkStreamCameraTrigger(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1341,10 +1374,10 @@ public: return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -1365,7 +1398,7 @@ private: /* do not allow top copying this class */ MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); - MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); + MavlinkStreamGlobalPositionInt &operator = (const MavlinkStreamGlobalPositionInt &); protected: explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1404,70 +1437,70 @@ protected: class MavlinkStreamVisionPositionNED : public MavlinkStream { public: - const char *get_name() const - { - return MavlinkStreamVisionPositionNED::get_name_static(); - } + const char *get_name() const + { + return MavlinkStreamVisionPositionNED::get_name_static(); + } - static const char *get_name_static() - { - return "VISION_POSITION_NED"; - } + static const char *get_name_static() + { + return "VISION_POSITION_NED"; + } - static uint8_t get_id_static() - { - return MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; - } + static uint8_t get_id_static() + { + return MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; + } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } - static MavlinkStream *new_instance(Mavlink *mavlink) - { - return new MavlinkStreamVisionPositionNED(mavlink); - } + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamVisionPositionNED(mavlink); + } - unsigned get_size() - { - return (_pos_time > 0) ? MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; - } + unsigned get_size() + { + return (_pos_time > 0) ? MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } private: - MavlinkOrbSubscription *_pos_sub; - uint64_t _pos_time; + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; - /* do not allow top copying this class */ - MavlinkStreamVisionPositionNED(MavlinkStreamVisionPositionNED &); - MavlinkStreamVisionPositionNED& operator = (const MavlinkStreamVisionPositionNED &); + /* do not allow top copying this class */ + MavlinkStreamVisionPositionNED(MavlinkStreamVisionPositionNED &); + MavlinkStreamVisionPositionNED &operator = (const MavlinkStreamVisionPositionNED &); protected: - explicit MavlinkStreamVisionPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink), - _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vision_position_estimate))), - _pos_time(0) - {} + explicit MavlinkStreamVisionPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vision_position_estimate))), + _pos_time(0) + {} - void send(const hrt_abstime t) - { - struct vision_position_estimate_s vpos; - memset(&vpos, 0, sizeof(vpos)); + void send(const hrt_abstime t) + { + struct vision_position_estimate_s vpos; + memset(&vpos, 0, sizeof(vpos)); - if (_pos_sub->update(&_pos_time, &vpos)) { - mavlink_vision_position_estimate_t vmsg; - vmsg.usec = vpos.timestamp; - vmsg.x = vpos.x; - vmsg.y = vpos.y; - vmsg.z = vpos.z; - math::Quaternion q(vpos.q); - math::Vector<3> rpy = q.to_euler(); - vmsg.roll = rpy(0); - vmsg.pitch = rpy(1); - vmsg.yaw = rpy(2); + if (_pos_sub->update(&_pos_time, &vpos)) { + mavlink_vision_position_estimate_t vmsg; + vmsg.usec = vpos.timestamp; + vmsg.x = vpos.x; + vmsg.y = vpos.y; + vmsg.z = vpos.z; + math::Quaternion q(vpos.q); + math::Vector<3> rpy = q.to_euler(); + vmsg.roll = rpy(0); + vmsg.pitch = rpy(1); + vmsg.yaw = rpy(2); - mavlink_msg_vision_position_estimate_send_struct(_mavlink->get_channel(), &vmsg); - } - } + mavlink_msg_vision_position_estimate_send_struct(_mavlink->get_channel(), &vmsg); + } + } }; class MavlinkStreamLocalPositionNED : public MavlinkStream @@ -1488,10 +1521,10 @@ public: return MAVLINK_MSG_ID_LOCAL_POSITION_NED; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -1509,7 +1542,7 @@ private: /* do not allow top copying this class */ MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); - MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); + MavlinkStreamLocalPositionNED &operator = (const MavlinkStreamLocalPositionNED &); protected: explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1556,10 +1589,10 @@ public: return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -1577,7 +1610,7 @@ private: /* do not allow top copying this class */ MavlinkStreamLocalPositionNEDCOV(MavlinkStreamLocalPositionNEDCOV &); - MavlinkStreamLocalPositionNEDCOV& operator = (const MavlinkStreamLocalPositionNEDCOV &); + MavlinkStreamLocalPositionNEDCOV &operator = (const MavlinkStreamLocalPositionNEDCOV &); protected: explicit MavlinkStreamLocalPositionNEDCOV(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1602,9 +1635,11 @@ protected: msg.ax = est.states[6]; msg.ay = est.states[7]; msg.az = est.states[8]; + for (int i = 0; i < 9; i++) { msg.covariance[i] = est.covariances[i]; } + msg.covariance[9] = est.nan_flags; msg.covariance[10] = est.health_flags; msg.covariance[11] = est.timeout_flags; @@ -1632,10 +1667,10 @@ public: return MAVLINK_MSG_ID_VIBRATION; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -1653,7 +1688,7 @@ private: /* do not allow top copying this class */ MavlinkStreamEstimatorStatus(MavlinkStreamEstimatorStatus &); - MavlinkStreamEstimatorStatus& operator = (const MavlinkStreamEstimatorStatus &); + MavlinkStreamEstimatorStatus &operator = (const MavlinkStreamEstimatorStatus &); protected: explicit MavlinkStreamEstimatorStatus(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1700,10 +1735,10 @@ public: return MAVLINK_MSG_ID_ATT_POS_MOCAP; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -1721,7 +1756,7 @@ private: /* do not allow top copying this class */ MavlinkStreamAttPosMocap(MavlinkStreamAttPosMocap &); - MavlinkStreamAttPosMocap& operator = (const MavlinkStreamAttPosMocap &); + MavlinkStreamAttPosMocap &operator = (const MavlinkStreamAttPosMocap &); protected: explicit MavlinkStreamAttPosMocap(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1769,10 +1804,10 @@ public: return MAVLINK_MSG_ID_HOME_POSITION; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -1789,7 +1824,7 @@ private: /* do not allow top copying this class */ MavlinkStreamHomePosition(MavlinkStreamHomePosition &); - MavlinkStreamHomePosition& operator = (const MavlinkStreamHomePosition &); + MavlinkStreamHomePosition &operator = (const MavlinkStreamHomePosition &); protected: explicit MavlinkStreamHomePosition(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1844,24 +1879,24 @@ public: return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static const char *get_name_static() { switch (N) { - case 0: + case 0: return "SERVO_OUTPUT_RAW_0"; - case 1: + case 1: return "SERVO_OUTPUT_RAW_1"; - case 2: + case 2: return "SERVO_OUTPUT_RAW_2"; - case 3: + case 3: return "SERVO_OUTPUT_RAW_3"; } } @@ -1882,7 +1917,7 @@ private: /* do not allow top copying this class */ MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); - MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); + MavlinkStreamServoOutputRaw &operator = (const MavlinkStreamServoOutputRaw &); protected: explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1927,16 +1962,16 @@ public: static const char *get_name_static() { switch (N) { - case 0: + case 0: return "ACTUATOR_CONTROL_TARGET0"; - case 1: + case 1: return "ACTUATOR_CONTROL_TARGET1"; - case 2: + case 2: return "ACTUATOR_CONTROL_TARGET2"; - case 3: + case 3: return "ACTUATOR_CONTROL_TARGET3"; } } @@ -1946,10 +1981,10 @@ public: return MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -1967,7 +2002,7 @@ private: /* do not allow top copying this class */ MavlinkStreamActuatorControlTarget(MavlinkStreamActuatorControlTarget &); - MavlinkStreamActuatorControlTarget& operator = (const MavlinkStreamActuatorControlTarget &); + MavlinkStreamActuatorControlTarget &operator = (const MavlinkStreamActuatorControlTarget &); protected: explicit MavlinkStreamActuatorControlTarget(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1976,19 +2011,19 @@ protected: { // XXX this can be removed once the multiplatform system remaps topics switch (N) { - case 0: + case 0: _att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); break; - case 1: + case 1: _att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_1)); break; - case 2: + case 2: _att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_2)); break; - case 3: + case 3: _att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_3)); break; } @@ -2032,10 +2067,10 @@ public: return MAVLINK_MSG_ID_HIL_CONTROLS; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -2056,7 +2091,7 @@ private: /* do not allow top copying this class */ MavlinkStreamHILControls(MavlinkStreamHILControls &); - MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); + MavlinkStreamHILControls &operator = (const MavlinkStreamHILControls &); protected: explicit MavlinkStreamHILControls(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -2089,10 +2124,10 @@ protected: /* scale outputs depending on system type */ if (system_type == MAV_TYPE_QUADROTOR || - system_type == MAV_TYPE_HEXAROTOR || - system_type == MAV_TYPE_OCTOROTOR || - system_type == MAV_TYPE_VTOL_DUOROTOR || - system_type == MAV_TYPE_VTOL_QUADROTOR) { + system_type == MAV_TYPE_HEXAROTOR || + system_type == MAV_TYPE_OCTOROTOR || + system_type == MAV_TYPE_VTOL_DUOROTOR || + system_type == MAV_TYPE_VTOL_QUADROTOR) { /* multirotors: set number of rotor outputs depending on type */ @@ -2196,10 +2231,10 @@ public: return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -2216,7 +2251,7 @@ private: /* do not allow top copying this class */ MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &); - MavlinkStreamPositionTargetGlobalInt& operator = (const MavlinkStreamPositionTargetGlobalInt &); + MavlinkStreamPositionTargetGlobalInt &operator = (const MavlinkStreamPositionTargetGlobalInt &); protected: explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -2230,7 +2265,7 @@ protected: if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { mavlink_position_target_global_int_t msg{}; - msg.time_boot_ms = hrt_absolute_time()/1000; + msg.time_boot_ms = hrt_absolute_time() / 1000; msg.coordinate_frame = MAV_FRAME_GLOBAL; msg.lat_int = pos_sp_triplet.current.lat * 1e7; msg.lon_int = pos_sp_triplet.current.lon * 1e7; @@ -2260,10 +2295,10 @@ public: return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -2281,7 +2316,7 @@ private: /* do not allow top copying this class */ MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); - MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); + MavlinkStreamLocalPositionSetpoint &operator = (const MavlinkStreamLocalPositionSetpoint &); protected: explicit MavlinkStreamLocalPositionSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -2333,10 +2368,10 @@ public: return MAVLINK_MSG_ID_ATTITUDE_TARGET; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -2356,7 +2391,7 @@ private: /* do not allow top copying this class */ MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &); - MavlinkStreamAttitudeTarget& operator = (const MavlinkStreamAttitudeTarget &); + MavlinkStreamAttitudeTarget &operator = (const MavlinkStreamAttitudeTarget &); protected: explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -2410,10 +2445,10 @@ public: return MAVLINK_MSG_ID_RC_CHANNELS; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -2431,7 +2466,7 @@ private: /* do not allow top copying this class */ MavlinkStreamRCChannels(MavlinkStreamRCChannels &); - MavlinkStreamRCChannels& operator = (const MavlinkStreamRCChannels &); + MavlinkStreamRCChannels &operator = (const MavlinkStreamRCChannels &); protected: explicit MavlinkStreamRCChannels(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -2511,10 +2546,10 @@ public: return MAVLINK_MSG_ID_MANUAL_CONTROL; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -2532,7 +2567,7 @@ private: /* do not allow top copying this class */ MavlinkStreamManualControl(MavlinkStreamManualControl &); - MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); + MavlinkStreamManualControl &operator = (const MavlinkStreamManualControl &); protected: explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -2584,10 +2619,10 @@ public: return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -2605,7 +2640,7 @@ private: /* do not allow top copying this class */ MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &); - MavlinkStreamOpticalFlowRad& operator = (const MavlinkStreamOpticalFlowRad &); + MavlinkStreamOpticalFlowRad &operator = (const MavlinkStreamOpticalFlowRad &); protected: explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -2657,10 +2692,10 @@ public: return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -2678,7 +2713,7 @@ private: /* do not allow top copying this class */ MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); - MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); + MavlinkStreamNamedValueFloat &operator = (const MavlinkStreamNamedValueFloat &); protected: explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -2722,10 +2757,10 @@ public: return MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -2735,7 +2770,7 @@ public: unsigned get_size() { return (_fw_pos_ctrl_status_sub->is_published()) ? - MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: @@ -2744,7 +2779,7 @@ private: /* do not allow top copying this class */ MavlinkStreamNavControllerOutput(MavlinkStreamNavControllerOutput &); - MavlinkStreamNavControllerOutput& operator = (const MavlinkStreamNavControllerOutput &); + MavlinkStreamNavControllerOutput &operator = (const MavlinkStreamNavControllerOutput &); protected: explicit MavlinkStreamNavControllerOutput(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -2795,10 +2830,10 @@ public: return 0; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -2815,7 +2850,7 @@ private: /* do not allow top copying this class */ MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); - MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); + MavlinkStreamCameraCapture &operator = (const MavlinkStreamCameraCapture &); protected: explicit MavlinkStreamCameraCapture(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -2837,7 +2872,8 @@ protected: msg.param2 = 0; msg.param3 = 0; /* set camera capture ON/OFF depending on arming state */ - msg.param4 = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) ? 1 : 0; + msg.param4 = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED + || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) ? 1 : 0; msg.param5 = 0; msg.param6 = 0; msg.param7 = 0; @@ -2864,10 +2900,10 @@ public: return MAVLINK_MSG_ID_DISTANCE_SENSOR; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -2885,7 +2921,7 @@ private: /* do not allow top copying this class */ MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); - MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); + MavlinkStreamDistanceSensor &operator = (const MavlinkStreamDistanceSensor &); protected: explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -2953,10 +2989,10 @@ public: return MAVLINK_MSG_ID_EXTENDED_SYS_STATE; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -3082,7 +3118,7 @@ private: /* do not allow top copying this class */ MavlinkStreamAltitude(MavlinkStreamAltitude &); - MavlinkStreamAltitude& operator = (const MavlinkStreamAltitude &); + MavlinkStreamAltitude &operator = (const MavlinkStreamAltitude &); protected: explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -3111,6 +3147,7 @@ protected: if (global_pos.terrain_alt_valid) { msg.altitude_terrain = global_pos.terrain_alt; msg.bottom_clearance = global_pos.alt - global_pos.terrain_alt; + } else { msg.altitude_terrain = NAN; msg.bottom_clearance = NAN; @@ -3162,10 +3199,10 @@ public: return MAVLINK_MSG_ID_WIND_COV; } - uint8_t get_id() - { - return get_id_static(); - } + uint8_t get_id() + { + return get_id_static(); + } static MavlinkStream *new_instance(Mavlink *mavlink) { @@ -3186,7 +3223,7 @@ private: /* do not allow top copying this class */ MavlinkStreamWind(MavlinkStreamWind &); - MavlinkStreamWind& operator = (const MavlinkStreamWind &); + MavlinkStreamWind &operator = (const MavlinkStreamWind &); protected: explicit MavlinkStreamWind(Mavlink *mavlink) : MavlinkStream(mavlink), diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 5b7725f374..4fd30bd3e9 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -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); } } diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index c37322d37b..04b7645b3b 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -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); diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 87cf8624d3..1ef41e94e2 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -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; } diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 913f19ec97..41c047bc1c 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -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 &); }; diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index b846daf1e0..6facb77f1e 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -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(¶m_value.param_value, &hash, sizeof(hash)); mavlink_msg_param_value_send_struct(_mavlink->get_channel(), ¶m_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, ¶m_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."); diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index 1ef29411c7..bd3678d73e 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -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); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 600bdd3673..8de279da74 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index c271810ea6..7cc293a05c 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -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); /** diff --git a/src/modules/mavlink/mavlink_shell.cpp b/src/modules/mavlink/mavlink_shell.cpp index ea509151ef..6a9085af03 100644 --- a/src/modules/mavlink/mavlink_shell.cpp +++ b/src/modules/mavlink/mavlink_shell.cpp @@ -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; } diff --git a/src/modules/mavlink/mavlink_shell.h b/src/modules/mavlink/mavlink_shell.h index f4a788a023..c5138befdd 100644 --- a/src/modules/mavlink/mavlink_shell.h +++ b/src/modules/mavlink/mavlink_shell.h @@ -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[]);