From f5290693680b47c5cb47bc76c5c99a9da72260ce Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 9 Jan 2017 12:43:28 -0800 Subject: [PATCH] Fixed code format issues Signed-off-by: Mark Charlebois --- src/modules/muorb/adsp/px4muorb.cpp | 4 ++++ src/modules/muorb/adsp/uORBFastRpcChannel.cpp | 20 +++++++++++++------ src/modules/muorb/adsp/uORBFastRpcChannel.hpp | 2 +- .../muorb/krait/uORBKraitFastRpcChannel.cpp | 8 +++++--- src/modules/uORB/uORBDevices.cpp | 2 ++ src/modules/uORB/uORBManager.cpp | 3 +++ 6 files changed, 29 insertions(+), 10 deletions(-) diff --git a/src/modules/muorb/adsp/px4muorb.cpp b/src/modules/muorb/adsp/px4muorb.cpp index 96124e8bc5..798b5ae387 100644 --- a/src/modules/muorb/adsp/px4muorb.cpp +++ b/src/modules/muorb/adsp/px4muorb.cpp @@ -169,9 +169,11 @@ int px4muorb_topic_advertised(const char *topic_name) if (rxHandler != nullptr) { rc = rxHandler->process_remote_topic(topic_name, 1); + } else { rc = -1; } + return rc; } @@ -184,9 +186,11 @@ int px4muorb_topic_unadvertised(const char *topic_name) if (rxHandler != nullptr) { rc = rxHandler->process_remote_topic(topic_name, 0); + } else { rc = -1; } + return rc; } diff --git a/src/modules/muorb/adsp/uORBFastRpcChannel.cpp b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp index 76034b88c0..c91c7f2e20 100644 --- a/src/modules/muorb/adsp/uORBFastRpcChannel.cpp +++ b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp @@ -527,6 +527,7 @@ int16_t uORB::FastRpcChannel::get_bulk_data } } } + if (ControlQSize() != 0) { //PX4_DEBUG( "get_bulk_data: QSize: %d", ControlQSize() ); topic_count_to_return += ControlQSize(); @@ -575,6 +576,7 @@ int16_t uORB::FastRpcChannel::get_bulk_data } } } + } else { PX4_ERR("[get_data_bulk] Error: Semaphore is up when there is no data on the control/data queues"); rc = -1; @@ -617,12 +619,15 @@ int32_t uORB::FastRpcChannel::get_msg_size_at(bool isData, int32_t index) // the assumption here is that this is called within the context of semaphore, // hence lock/unlock is not needed. int32_t rc = 0; + if (isData) { rc += _DataMsgQueue[ index ]._Length; rc += _DataMsgQueue[ index ]._MsgName.size() + 1; + } else { rc += _ControlMsgQueue[ index ]._MsgName.size() + 1; } + rc += _PACKET_HEADER_SIZE; return rc; } @@ -637,8 +642,8 @@ int32_t uORB::FastRpcChannel::copy_msg_to_buffer(bool isData, int32_t src_index, // * the dst_buffer is validated to uint16_t msg_size = (isData ? - (uint16_t)(_DataMsgQueue[ src_index ]._MsgName.size()) : - (uint16_t)(_ControlMsgQueue[ src_index ]._MsgName.size())); + (uint16_t)(_DataMsgQueue[ src_index ]._MsgName.size()) : + (uint16_t)(_ControlMsgQueue[ src_index ]._MsgName.size())); // compute the different offsets to pack the packets. int32_t field_header_offset = offset; @@ -647,8 +652,9 @@ int32_t uORB::FastRpcChannel::copy_msg_to_buffer(bool isData, int32_t src_index, int16_t msg_type = isData ? _DATA_MSG_TYPE : _ControlMsgQueue[ src_index ]._Type; - struct BulkTransferHeader header = { (uint16_t)msg_type,( uint16_t)(msg_size + 1), - (uint16_t)(isData ? (_DataMsgQueue[ src_index ]._Length) : 0) }; + struct BulkTransferHeader header = { (uint16_t)msg_type, (uint16_t)(msg_size + 1), + (uint16_t)(isData ? (_DataMsgQueue[ src_index ]._Length) : 0) + }; //PX4_DEBUG( "Offsets: header[%d] name[%d] data[%d]", @@ -693,12 +699,14 @@ int32_t uORB::FastRpcChannel::copy_msg_to_buffer(bool isData, int32_t src_index, } else { PX4_WARN("Error coping the Msg to dst buffer, insuffienct space. "); + if (isData) { PX4_WARN("Data... offset[%ld] len[%ld] data_msg_len[%ld]", - offset, dst_buffer_len, (field_data_offset - offset) + _DataMsgQueue[ src_index ]._Length); + offset, dst_buffer_len, (field_data_offset - offset) + _DataMsgQueue[ src_index ]._Length); + } else { PX4_WARN("ControlMsg... offset[%ld] len[%ld]", - offset, dst_buffer_len, (field_data_offset - offset)); + offset, dst_buffer_len, (field_data_offset - offset)); } } diff --git a/src/modules/muorb/adsp/uORBFastRpcChannel.hpp b/src/modules/muorb/adsp/uORBFastRpcChannel.hpp index fe5f686c14..b908091b29 100644 --- a/src/modules/muorb/adsp/uORBFastRpcChannel.hpp +++ b/src/modules/muorb/adsp/uORBFastRpcChannel.hpp @@ -188,7 +188,7 @@ private: // data members static const int32_t _PACKET_FIELD_TOPIC_NAME_LEN_SIZE_IN_BYTES = 2; static const int32_t _PACKET_FIELD_DATA_LEN_IN_BYTES = 2; static const int32_t _PACKET_HEADER_SIZE = 1 + //first byte is the MSG Type - _PACKET_FIELD_TOPIC_NAME_LEN_SIZE_IN_BYTES + _PACKET_FIELD_DATA_LEN_IN_BYTES; + _PACKET_FIELD_TOPIC_NAME_LEN_SIZE_IN_BYTES + _PACKET_FIELD_DATA_LEN_IN_BYTES; struct FastRpcDataMsg { int32_t _MaxBufferSize; diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp index bb79fc007c..6d47505158 100644 --- a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp @@ -275,12 +275,14 @@ void uORB::KraitFastRpcChannel::fastrpc_recv_thread() if (header->_MsgType == _DATA_MSG_TYPE) { //PX4_DEBUG( "Received topic data for: [%s] len[%d]\n", messageName, data_length ); _RxHandler->process_received_message(messageName, - header->_DataLen, topic_data); + header->_DataLen, topic_data); + } else if (header->_MsgType == _CONTROL_MSG_TYPE_ADVERTISE) { - PX4_DEBUG( "Received topic advertise message for: [%s] len[%d]\n", messageName, data_length ); + PX4_DEBUG("Received topic advertise message for: [%s] len[%d]\n", messageName, data_length); _RxHandler->process_remote_topic(messageName, true); + } else if (header->_MsgType == _CONTROL_MSG_TYPE_UNADVERTISE) { - PX4_DEBUG( "Received topic unadvertise message for: [%s] len[%d]\n", messageName, data_length ); + PX4_DEBUG("Received topic unadvertise message for: [%s] len[%d]\n", messageName, data_length); _RxHandler->process_remote_topic(messageName, false); } } diff --git a/src/modules/uORB/uORBDevices.cpp b/src/modules/uORB/uORBDevices.cpp index 472dd56941..834127fb51 100644 --- a/src/modules/uORB/uORBDevices.cpp +++ b/src/modules/uORB/uORBDevices.cpp @@ -492,9 +492,11 @@ int uORB::DeviceNode::unadvertise(orb_advert_t handle) int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta, int priority) { uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); + if (ch != nullptr && meta != nullptr) { return ch->topic_advertised(meta->o_name); } + return -1; } /* diff --git a/src/modules/uORB/uORBManager.cpp b/src/modules/uORB/uORBManager.cpp index 0a5f4da176..86ad44a1de 100644 --- a/src/modules/uORB/uORBManager.cpp +++ b/src/modules/uORB/uORBManager.cpp @@ -136,9 +136,11 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) return stat(path, &buffer); #else ret = px4_access(path, F_OK); + if (ret == -1 && meta != nullptr && _remote_topics.size() > 0) { ret = (_remote_topics.find(meta->o_name) != _remote_topics.end()) ? OK : ERROR; } + return ret; #endif } @@ -446,6 +448,7 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name, bool isAdver if (isAdvertisement) { _remote_topics.insert(topic_name); + } else { _remote_topics.erase(topic_name); }