From 60ec1c897a5aeb55b5613fe8fefb83343e769142 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 11:12:50 -0700 Subject: [PATCH] QuRT: Added muorb files muorb is used to proxy messages between the Krait and DSP. Signed-off-by: Mark Charlebois --- src/lib/version/version.h | 3 + src/modules/muorb/adsp/module.mk | 50 +++ src/modules/muorb/adsp/muorb_fastrpc.cpp | 157 +++++++++ src/modules/muorb/adsp/muorb_fastrpc.h | 55 +++ src/modules/muorb/adsp/uORBFastRpcChannel.cpp | 318 ++++++++++++++++++ src/modules/muorb/adsp/uORBFastRpcChannel.hpp | 251 ++++++++++++++ src/modules/muorb/krait/module.mk | 49 +++ src/modules/muorb/krait/muorb_main.cpp | 84 +++++ .../muorb/krait/uORBKraitFastRpcChannel.cpp | 190 +++++++++++ .../muorb/krait/uORBKraitFastRpcChannel.hpp | 144 ++++++++ 10 files changed, 1301 insertions(+) create mode 100644 src/modules/muorb/adsp/module.mk create mode 100644 src/modules/muorb/adsp/muorb_fastrpc.cpp create mode 100644 src/modules/muorb/adsp/muorb_fastrpc.h create mode 100644 src/modules/muorb/adsp/uORBFastRpcChannel.cpp create mode 100644 src/modules/muorb/adsp/uORBFastRpcChannel.hpp create mode 100644 src/modules/muorb/krait/module.mk create mode 100644 src/modules/muorb/krait/muorb_main.cpp create mode 100644 src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp create mode 100644 src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp diff --git a/src/lib/version/version.h b/src/lib/version/version.h index 9d7e471adc..b71ba1d0d9 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -62,4 +62,7 @@ #ifdef CONFIG_ARCH_BOARD_SITL #define HW_ARCH "LINUXTEST" #endif +#ifdef CONFIG_ARCH_BOARD_EAGLE +#define HW_ARCH "LINUXTEST" +#endif #endif /* VERSION_H_ */ diff --git a/src/modules/muorb/adsp/module.mk b/src/modules/muorb/adsp/module.mk new file mode 100644 index 0000000000..65d0e8e555 --- /dev/null +++ b/src/modules/muorb/adsp/module.mk @@ -0,0 +1,50 @@ +############################################################################ +# +# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build muorb +# + + +ifeq ($(PX4_TARGET_OS),qurt) + +SRCS = \ + muorb_fastrpc.cpp \ + uORBFastRpcChannel.cpp + +INCLUDE_DIRS += \ + ${PX4_BASE}/src/modules/uORB + +endif + +MAXOPTIMIZATION = -Os diff --git a/src/modules/muorb/adsp/muorb_fastrpc.cpp b/src/modules/muorb/adsp/muorb_fastrpc.cpp new file mode 100644 index 0000000000..febe578258 --- /dev/null +++ b/src/modules/muorb/adsp/muorb_fastrpc.cpp @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#include "muorb_fastrpc.h" +#include "qurt.h" +#include "uORBFastRpcChannel.hpp" +#include "uORBManager.hpp" + +#include +#include +#include +#include +#include +#include "px4_log.h" +#include "uORB/topics/sensor_combined.h" +#include "uORB.h" + +#define _ENABLE_MUORB 1 + +__BEGIN_DECLS + +int dspal_main(int argc, const char *argv[]); + +__END_DECLS + + +int muorb_fastrpc_orb_initialize() +{ + int rc = 0; + PX4_WARN("Before calling dspal_entry() method..."); + // registere the fastrpc muorb with uORBManager. + uORB::Manager::get_instance()->set_uorb_communicator(uORB::FastRpcChannel::GetInstance()); + const char *argv[2] = { "dspal", "start" }; + int argc = 2; + dspal_main(argc, argv); + PX4_WARN("After calling dspal_entry"); + return rc; +} + +int muorb_fastrpc_add_subscriber(const char *name) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + channel->AddRemoteSubscriber(name); + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + + if (rxHandler != nullptr) { + rc = rxHandler->process_add_subscription(name, 0); + + if (rc != OK) { + channel->RemoveRemoteSubscriber(name); + } + + } else { + rc = -1; + } + + return rc; +} + +int muorb_fastrpc_remove_subscriber(const char *name) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + channel->RemoveRemoteSubscriber(name); + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + + if (rxHandler != nullptr) { + rc = rxHandler->process_remove_subscription(name); + + } else { + rc = -1; + } + + return rc; + +} + +int muorb_fastrpc_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + + if (rxHandler != nullptr) { + rc = rxHandler->process_received_message(name, data_len_in_bytes, (uint8_t *)data); + + } else { + rc = -1; + } + + return rc; +} + +int muorb_fastrpc_is_subscriber_present(const char *topic_name, int *status) +{ + int rc = 0; + int32_t local_status = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + rc = channel->is_subscriber_present(topic_name, &local_status); + + if (rc == 0) { + *status = (int)local_status; + } + + return rc; +} + +int muorb_fastrpc_receive_msg(int *msg_type, char *topic_name, int topic_name_len, uint8_t *data, int data_len_in_bytes, + int *bytes_returned) +{ + int rc = 0; + int32_t local_msg_type = 0; + int32_t local_bytes_returned = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + rc = channel->get_data(&local_msg_type, topic_name, topic_name_len, data, data_len_in_bytes, &local_bytes_returned); + *msg_type = (int)local_msg_type; + *bytes_returned = (int)local_bytes_returned; + return rc; +} + +int muorb_fastrpc_unblock_recieve_msg(void) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + rc = channel->unblock_get_data_method(); + return rc; +} diff --git a/src/modules/muorb/adsp/muorb_fastrpc.h b/src/modules/muorb/adsp/muorb_fastrpc.h new file mode 100644 index 0000000000..96eda20d35 --- /dev/null +++ b/src/modules/muorb/adsp/muorb_fastrpc.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#pragma once + +#include +#include + +__BEGIN_DECLS + +int muorb_fastrpc_orb_initialize() __EXPORT; + +int muorb_fastrpc_add_subscriber(const char *name) __EXPORT; + +int muorb_fastrpc_remove_subscriber(const char *name) __EXPORT; + +int muorb_fastrpc_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT; + +int muorb_fastrpc_is_subscriber_present(const char *topic_name, int *status) __EXPORT; + +int muorb_fastrpc_receive_msg(int *msg_type, char *topic_name, int topic_name_len, uint8_t *data, int data_len_in_bytes, + int *bytes_returned) __EXPORT; + +int muorb_fastrpc_unblock_recieve_msg(void) __EXPORT; + +__END_DECLS diff --git a/src/modules/muorb/adsp/uORBFastRpcChannel.cpp b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp new file mode 100644 index 0000000000..87c2bba57d --- /dev/null +++ b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp @@ -0,0 +1,318 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#include "uORBFastRpcChannel.hpp" +#include "px4_log.h" +#include + +// static intialization. +uORB::FastRpcChannel uORB::FastRpcChannel::_Instance; + +//============================================================================== +//============================================================================== +uORB::FastRpcChannel::FastRpcChannel() + : _RxHandler(0) + , _DataQInIndex(0) + , _DataQOutIndex(0) + , _ControlQInIndex(0) + , _ControlQOutIndex(0) +{ + for (int32_t i = 0; i < _MAX_MSG_QUEUE_SIZE; ++ i) { + _DataMsgQueue[i]._MaxBufferSize = 0; + _DataMsgQueue[i]._Length = 0; + _DataMsgQueue[i]._Buffer = 0; + } + + _RemoteSubscribers.clear(); +} + +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::add_subscription(const char *messageName, int32_t msgRateInHz) +{ + int16_t rc = 0; + _Subscribers.push_back(messageName); + PX4_DEBUG("Adding message[%s] to subscriber queue...", messageName); + return rc; +} + +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::remove_subscription(const char *messageName) +{ + int16_t rc = 0; + _Subscribers.remove(messageName); + + return rc; +} + +int16_t uORB::FastRpcChannel::is_subscriber_present(const char *messageName, int32_t *status) +{ + int16_t rc = 0; + + if (std::find(_Subscribers.begin(), _Subscribers.end(), messageName) != _Subscribers.end()) { + *status = 1; + PX4_DEBUG("******* Found subscriber for message[%s]....", messageName); + + } else { + *status = 0; + PX4_WARN("@@@@@ Subscriber not found for[%s]...numSubscribers[%d]", messageName, _Subscribers.size()); + int i = 0; + + for (std::list::iterator it = _Subscribers.begin(); it != _Subscribers.end(); ++it) { + if (*it == messageName) { + PX4_DEBUG("##### Found the message[%s] in the subscriber list-index[%d]", messageName, i); + } + + ++i; + } + } + + return rc; +} + +int16_t uORB::FastRpcChannel::unblock_get_data_method() +{ + PX4_DEBUG("[unblock_get_data_method] calling post method for _DataAvailableSemaphore()"); + _DataAvailableSemaphore.post(); + return 0; +} +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler) +{ + _RxHandler = handler; + return 0; +} + + +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::send_message(const char *messageName, int32_t length, uint8_t *data) +{ + int16_t rc = 0; + + if (_RemoteSubscribers.find(messageName) == _RemoteSubscribers.end()) { + //there is no-remote subscriber. So do not queue the message. + return rc; + } + + _QueueMutex.lock(); + bool overwriteData = false; + + if (IsDataQFull()) { + // queue is full. Overwrite the oldest data. + PX4_WARN("[send_message] Queue Full Overwrite the oldest data. in[%ld] out[%ld] max[%ld]", + _DataQInIndex, _DataQOutIndex, _MAX_MSG_QUEUE_SIZE); + _DataQOutIndex++; + + if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _DataQOutIndex = 0; + } + + overwriteData = true; + } + + // now check to see if the data queue's buffer size if large enough to memcpy the data. + // if not, delete the old buffer and re-create a new buffer of larger size. + check_and_expand_data_buffer(_DataQInIndex, length); + + // now memcpy the data to the buffer. + memcpy(_DataMsgQueue[ _DataQInIndex ]._Buffer, data, length); + _DataMsgQueue[ _DataQInIndex ]._Length = length; + _DataMsgQueue[ _DataQInIndex ]._MsgName = messageName; + + _DataQInIndex++; + + if (_DataQInIndex == _MAX_MSG_QUEUE_SIZE) { + _DataQInIndex = 0; + } + + // the assumption here is that each caller reads only one data from either control or data queue. + if (!overwriteData) { + _DataAvailableSemaphore.post(); + } + + _QueueMutex.unlock(); + return rc; +} + +//============================================================================== +//============================================================================== +void uORB::FastRpcChannel::check_and_expand_data_buffer(int32_t index, int32_t length) +{ + if (_DataMsgQueue[ index ]._MaxBufferSize < length) { + // create a new buffer of size length and delete old buffer. + if (_DataMsgQueue[ index ]._Buffer != 0) { + delete _DataMsgQueue[ index ]._Buffer; + } + + _DataMsgQueue[ index ]._Buffer = new uint8_t[ length ]; + + if (_DataMsgQueue[ index ]._Buffer == 0) { + PX4_ERR("Error[check_and_expand_data_buffer] Failed to allocate data queue buffer of size[%ld]", length); + _DataMsgQueue[ index ]._MaxBufferSize = 0; + return; + } + + _DataMsgQueue[ index ]._MaxBufferSize = length; + } +} + +int32_t uORB::FastRpcChannel::DataQSize() +{ + int32_t rc; + rc = (_DataQInIndex - _DataQOutIndex) + _MAX_MSG_QUEUE_SIZE; + rc %= _MAX_MSG_QUEUE_SIZE; + return rc; +} + +int32_t uORB::FastRpcChannel::ControlQSize() +{ + int32_t rc; + rc = (_ControlQInIndex - _ControlQOutIndex) + _MAX_MSG_QUEUE_SIZE; + rc %= _MAX_MSG_QUEUE_SIZE; + return rc; +} + +bool uORB::FastRpcChannel::IsControlQFull() +{ + return (ControlQSize() == (_MAX_MSG_QUEUE_SIZE - 1)); +} + +bool uORB::FastRpcChannel::IsControlQEmpty() +{ + return (ControlQSize() == 0); +} + +bool uORB::FastRpcChannel::IsDataQFull() +{ + return (DataQSize() == (_MAX_MSG_QUEUE_SIZE - 1)); +} + +bool uORB::FastRpcChannel::IsDataQEmpty() +{ + return (DataQSize() == 0); +} + +int16_t uORB::FastRpcChannel::get_data +( + int32_t *msg_type, + char *topic_name, + int32_t topic_name_len, + uint8_t *data, + int32_t data_len_in_bytes, + int32_t *bytes_returned +) +{ + int16_t rc = 0; + // wait for data availability + _DataAvailableSemaphore.wait(); + _QueueMutex.lock(); + + if (DataQSize() != 0 || ControlQSize() != 0) { + if (ControlQSize() > 0) { + // read the first element of the Control Queue. + *msg_type = _ControlMsgQueue[ _ControlQOutIndex ]._Type; + + if ((int)_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() < (int)topic_name_len) { + memcpy + ( + topic_name, + _ControlMsgQueue[ _ControlQOutIndex ]._MsgName.c_str(), + _ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() + ); + + topic_name[_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size()] = 0; + + *bytes_returned = 0; + + _ControlQOutIndex++; + + if (_ControlQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _ControlQOutIndex = 0; + } + + } else { + PX4_ERR("Error[get_data-CONTROL]: max topic_name_len[%ld] < controlMsgLen[%d]", + topic_name_len, + _ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() + ); + rc = -1; + } + + } else { + // read the first element of the Control Queue. + *msg_type = _DATA_MSG_TYPE; + + if (((int)_DataMsgQueue[ _DataQOutIndex ]._MsgName.size() < topic_name_len) || + (_DataMsgQueue[ _DataQOutIndex ]._Length < data_len_in_bytes)) { + memcpy + ( + topic_name, + _DataMsgQueue[ _DataQOutIndex ]._MsgName.c_str(), + _DataMsgQueue[ _DataQOutIndex ]._MsgName.size() + ); + + topic_name[_DataMsgQueue[ _DataQOutIndex ]._MsgName.size()] = 0; + + *bytes_returned = _DataMsgQueue[ _DataQOutIndex ]._Length; + memcpy(data, _DataMsgQueue[ _DataQOutIndex ]._Buffer, _DataMsgQueue[ _DataQOutIndex ]._Length); + + _DataQOutIndex++; + + if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _DataQOutIndex = 0; + } + + } else { + PX4_ERR("Error:[get_data-DATA] type msg max topic_name_len[%ld] > dataMsgLen[%d] ", + topic_name_len, + _DataMsgQueue[ _DataQOutIndex ]._MsgName.size() + ); + PX4_ERR("Error:[get_data-DATA] Or data_buffer_len[%ld] > message_size[%ld] ", + data_len_in_bytes, + _DataMsgQueue[ _DataQOutIndex ]._Length + ); + + rc = -1; + } + } + + } else { + PX4_ERR("[get_data] Error: Semaphore is up when there is no data on the control/data queues"); + rc = -1; + } + + _QueueMutex.unlock(); + return rc; +} diff --git a/src/modules/muorb/adsp/uORBFastRpcChannel.hpp b/src/modules/muorb/adsp/uORBFastRpcChannel.hpp new file mode 100644 index 0000000000..0446fb1772 --- /dev/null +++ b/src/modules/muorb/adsp/uORBFastRpcChannel.hpp @@ -0,0 +1,251 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#ifndef _uORBFastRpcChannel_hpp_ +#define _uORBFastRpcChannel_hpp_ + +#include +#include +#include +#include "uORB/uORBCommunicator.hpp" +#include +#include + +namespace uORB +{ +class FastRpcChannel; +} + +class uORB::FastRpcChannel : public uORBCommunicator::IChannel +{ +public: + /** + * static method to get the IChannel Implementor. + */ + static uORB::FastRpcChannel *GetInstance() + { + return &(_Instance); + } + + /** + * @brief Interface to notify the remote entity of interest of a + * subscription for a message. + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t add_subscription(const char *messageName, int32_t msgRateInHz); + + + /** + * @brief Interface to notify the remote entity of removal of a subscription + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not necessarily mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t remove_subscription(const char *messageName); + + /** + * Register Message Handler. This is internal for the IChannel implementer* + */ + virtual int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler); + + + //========================================================================= + // INTERFACES FOR Data messages + //========================================================================= + + /** + * @brief Sends the data message over the communication link. + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data); + + //Function to return the data to krait. + int16_t get_data + ( + int32_t *msg_type, + char *topic_name, + int32_t topic_name_len, + uint8_t *data, + int32_t data_len_in_bytes, + int32_t *bytes_returned + ); + + // function to check if there are subscribers for a topic on adsp. + int16_t is_subscriber_present(const char *messageName, int32_t *status); + + // function to release the blocking semaphore for get_data method. + int16_t unblock_get_data_method(); + + uORBCommunicator::IChannelRxHandler *GetRxHandler() + { + return _RxHandler; + } + + void AddRemoteSubscriber(const std::string &messageName) + { + _RemoteSubscribers.insert(messageName); + } + void RemoveRemoteSubscriber(const std::string &messageName) + { + _RemoteSubscribers.erase(messageName); + } + +private: // data members + static uORB::FastRpcChannel _Instance; + uORBCommunicator::IChannelRxHandler *_RxHandler; + + /// data structure to store the messages to be retrived by Krait. + static const int32_t _MAX_MSG_QUEUE_SIZE = 100; + static const int32_t _CONTROL_MSG_TYPE_ADD_SUBSCRIBER = 1; + static const int32_t _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER = 2; + static const int32_t _DATA_MSG_TYPE = 3; + + struct FastRpcDataMsg { + int32_t _MaxBufferSize; + int32_t _Length; + uint8_t *_Buffer; + std::string _MsgName; + }; + + struct FastRpcControlMsg { + int32_t _Type; + std::string _MsgName; + }; + + struct FastRpcDataMsg _DataMsgQueue[ _MAX_MSG_QUEUE_SIZE ]; + int32_t _DataQInIndex; + int32_t _DataQOutIndex; + + struct FastRpcControlMsg _ControlMsgQueue[ _MAX_MSG_QUEUE_SIZE ]; + int32_t _ControlQInIndex; + int32_t _ControlQOutIndex; + + std::list _Subscribers; + + //utility classes + class Mutex + { + public: + Mutex() + { + sem_init(&_Sem, 0, 1); + } + ~Mutex() + { + sem_destroy(&_Sem); + } + void lock() + { + sem_wait(&_Sem); + } + void unlock() + { + sem_post(&_Sem); + } + private: + sem_t _Sem; + + Mutex(const Mutex &); + + Mutex &operator=(const Mutex &); + }; + + class Semaphore + { + public: + Semaphore() + { + sem_init(&_Sem, 0, 0); + } + ~Semaphore() + { + sem_destroy(&_Sem); + } + void post() + { + sem_post(&_Sem); + } + void wait() + { + sem_wait(&_Sem); + } + private: + sem_t _Sem; + Semaphore(const Semaphore &); + Semaphore &operator=(const Semaphore &); + + }; + + Mutex _QueueMutex; + Semaphore _DataAvailableSemaphore; + +private://class members. + /// constructor. + FastRpcChannel(); + + void check_and_expand_data_buffer(int32_t index, int32_t length); + + bool IsControlQFull(); + bool IsControlQEmpty(); + bool IsDataQFull(); + bool IsDataQEmpty(); + int32_t DataQSize(); + int32_t ControlQSize(); + + std::set _RemoteSubscribers; +}; + +#endif /* _uORBFastRpcChannel_hpp_ */ diff --git a/src/modules/muorb/krait/module.mk b/src/modules/muorb/krait/module.mk new file mode 100644 index 0000000000..c628b53f96 --- /dev/null +++ b/src/modules/muorb/krait/module.mk @@ -0,0 +1,49 @@ +############################################################################ +# +# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build uORB +# + +MODULE_COMMAND = muorb + +ifeq ($(PX4_TARGET_OS),posix-arm) +SRCS = uORBKraitFastRpcChannel.cpp \ + muorb_main.cpp +INCLUDE_DIRS += $(EXT_MUORB_LIB_ROOT)/krait/include \ + $(PX4_BASE)/src/modules/uORB \ + $(PX4_BASE)/src/modules + +EXTRA_LIBS += $(EXT_MUORB_LIB_ROOT)/krait/libs/libmuorb.so +endif + diff --git a/src/modules/muorb/krait/muorb_main.cpp b/src/modules/muorb/krait/muorb_main.cpp new file mode 100644 index 0000000000..3a320e6134 --- /dev/null +++ b/src/modules/muorb/krait/muorb_main.cpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include "uORBManager.hpp" +#include "uORBKraitFastRpcChannel.hpp" + +extern "C" { __EXPORT int muorb_main(int argc, char *argv[]); } + +static void usage() +{ + warnx("Usage: muorb 'start', 'stop', 'status'"); +} + + +int +muorb_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return -EINVAL; + } + + /* + * Start/load the driver. + * + * XXX it would be nice to have a wrapper for this... + */ + if (!strcmp(argv[1], "start")) { + // register the fast rpc channel with UORB. + uORB::Manager::get_instance()->set_uorb_communicator(uORB::KraitFastRpcChannel::GetInstance()); + + // start the KaitFastRPC channel thread. + uORB::KraitFastRpcChannel::GetInstance()->Start(); + return OK; + + } + + if (!strcmp(argv[1], "stop")) { + + uORB::KraitFastRpcChannel::GetInstance()->Stop(); + return OK; + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "status")) { + return OK; + } + + usage(); + return -EINVAL; +} diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp new file mode 100644 index 0000000000..3900a38c94 --- /dev/null +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "uORBKraitFastRpcChannel.hpp" +#include "px4_log.h" + + +#define LOG_TAG "uORBKraitFastRpcChannel.cpp" + +// static intialization. +uORB::KraitFastRpcChannel uORB::KraitFastRpcChannel::_Instance; + +//============================================================================== +//============================================================================== +uORB::KraitFastRpcChannel::KraitFastRpcChannel() + : _RxHandler(nullptr) + , _ThreadStarted(false) + , _ShouldExit(false) +{ + _KraitWrapper.Initialize(); +} + +//============================================================================== +//============================================================================== +int16_t uORB::KraitFastRpcChannel::add_subscription(const char *messageName, int32_t msgRateInHz) +{ + int16_t rc = 0; + // invoke fast_rpc call. From Idl. + PX4_DEBUG("Before calling AddSubscriber for [%s]\n", messageName); + rc = _KraitWrapper.AddSubscriber(messageName); + PX4_DEBUG("Response for AddSubscriber for [%s], rc[%d]\n", messageName, rc); + return rc; +} + +//============================================================================== +//============================================================================== +int16_t uORB::KraitFastRpcChannel::remove_subscription(const char *messageName) +{ + int16_t rc = 0; + // invoke the fast_rpc call defined in idl. + PX4_DEBUG("Before calling RemoveSubscriber for [%s]\n", messageName); + rc = _KraitWrapper.RemoveSubscriber(messageName); + PX4_DEBUG("Response for RemoveSubscriber for [%s], rc[%d]\n", messageName, rc); + return rc; +} + +//============================================================================== +//============================================================================== +int16_t uORB::KraitFastRpcChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler) +{ + _RxHandler = handler; + return 0; +} + + +//============================================================================== +//============================================================================== +int16_t uORB::KraitFastRpcChannel::send_message(const char *messageName, int32_t length, uint8_t *data) +{ + int16_t rc = 0; + // invoke the fast rpc call to send data defined in idl. + //PX4_DEBUG( "Before calling send_data for [%s] len[%d]\n", messageName.c_str(), length ); + int32_t status = 0; + + if (_KraitWrapper.IsSubscriberPresent(messageName, &status) == 0) { + if (status > 0) { // there are remote subscribers + rc = _KraitWrapper.SendData(messageName, length, data); + //PX4_DEBUG( "***** SENDING[%s] topic to remote....\n", messageName.c_str() ); + + } else { + //PX4_DEBUG( "******* NO SUBSCRIBER PRESENT ON THE REMOTE FOR topic[%s] \n", messageName.c_str() ); + } + } else { + PX4_ERR("Error returned for KraitWrapper.IsSubscriberPresent(%s)\n", messageName); + } + + //PX4_DEBUG( "Response for SendMessage for [%s],len[%d] rc[%d]\n", messageName.c_str(), length, rc ); + return rc; +} + +void uORB::KraitFastRpcChannel::Start() +{ + _ThreadStarted = true; + pthread_create(&_RecvThread, NULL, thread_start, this); +} + +void uORB::KraitFastRpcChannel::Stop() +{ + _ShouldExit = true; + _KraitWrapper.UnblockReceiveData(); + PX4_DEBUG("After calling krait_wrapper_unlock_receive_Data...\n"); + pthread_join(_RecvThread, NULL); + PX4_DEBUG("*** After calling thread wait...\n"); + _ThreadStarted = false; + _ShouldExit = false; +} + + +void uORB::KraitFastRpcChannel::thread_start(void *handler) +{ + if (handler != nullptr) { + ((uORB::KraitFastRpcChannel *)handler)->fastrpc_recv_thread(); + } +} + +void uORB::KraitFastRpcChannel::fastrpc_recv_thread() +{ + // sit in while loop. + int32_t rc = 0; + int32_t type = 0; + char *name = nullptr; + int32_t data_length = 0; + uint8_t *data = nullptr; + + while (!_ShouldExit) { + // call the fastrpc recv data call. + //uorb_fastrpc_recieve( &type, &name_len, name, &data_length, data ); + rc = _KraitWrapper.ReceiveData(&type, &name, &data_length, &data); + + if (rc == 0) { + switch (type) { + case _CONTROL_MSG_TYPE_ADD_SUBSCRIBER: + if (_RxHandler != nullptr) { + _RxHandler->process_add_subscription(name, 1); + PX4_DEBUG("Received add subscriber control message for: [%s]\n", name); + } + + break; + + case _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER: + if (_RxHandler != nullptr) { + _RxHandler->process_remove_subscription(name); + PX4_DEBUG("Received remove subscriber control message for: [%s]\n", name); + } + + break; + + case _DATA_MSG_TYPE: + if (_RxHandler != nullptr) { + _RxHandler->process_received_message(name, + data_length, data); + //PX4_DEBUG( "Received topic data for control message for: [%s] len[%d]\n", name, data_length ); + } + + break; + + default: + // error condition. + break; + } + + } else { + PX4_DEBUG("Error: Getting data over fastRPC channel\n"); + break; + } + } + + PX4_DEBUG("[uORB::KraitFastRpcChannel::fastrpc_recv_thread] Exiting fastrpc_recv_thread\n"); +} + diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp new file mode 100644 index 0000000000..5c61f1e961 --- /dev/null +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef _uORBKraitFastRpcChannel_hpp_ +#define _uORBKraitFastRpcChannel_hpp_ + +#include +#include +#include +#include "uORB/uORBCommunicator.hpp" +#include "muorbKraitFastRpcWrapper.hpp" + +namespace uORB +{ +class KraitFastRpcChannel; +} + +class uORB::KraitFastRpcChannel : public uORBCommunicator::IChannel +{ +public: + /** + * static method to get the IChannel Implementor. + */ + static uORB::KraitFastRpcChannel *GetInstance() + { + return &(_Instance); + } + + /** + * @brief Interface to notify the remote entity of interest of a + * subscription for a message. + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t add_subscription(const char *messageName, int32_t msgRateInHz); + + + /** + * @brief Interface to notify the remote entity of removal of a subscription + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not necessarily mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t remove_subscription(const char *messageName); + + /** + * Register Message Handler. This is internal for the IChannel implementer* + */ + virtual int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler); + + + //========================================================================= + // INTERFACES FOR Data messages + //========================================================================= + + /** + * @brief Sends the data message over the communication link. + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data); + + + void Start(); + void Stop(); + +private: // data members + static uORB::KraitFastRpcChannel _Instance; + uORBCommunicator::IChannelRxHandler *_RxHandler; + pthread_t _RecvThread; + bool _ThreadStarted; + bool _ShouldExit; + + static const int32_t _CONTROL_MSG_TYPE_ADD_SUBSCRIBER = 1; + static const int32_t _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER = 2; + static const int32_t _DATA_MSG_TYPE = 3; + + muorb::KraitFastRpcWrapper _KraitWrapper; + + + +private://class members. + /// constructor. + KraitFastRpcChannel(); + + static void thread_start(void *handler); + + void fastrpc_recv_thread(); + +}; + +#endif /* _uORBKraitFastRpcChannel_hpp_ */