orb_exists support for muorb

Also added builtin command wait_for_topic

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp

@@ -278,6 +278,16 @@ bool px4muorb::KraitRpcWrapper::Terminate()
 	return true;
 }
 
+int32_t px4muorb::KraitRpcWrapper::TopicAdvertised(const char *topic)
+{
+	return ((_Initialized) ? px4muorb_topic_advertised(topic) : -1);
+}
+
+int32_t px4muorb::KraitRpcWrapper::TopicUnadvertised(const char *topic)
+{
+	return ((_Initialized) ? px4muorb_topic_unadvertised(topic) : -1);
+}
+
 int32_t px4muorb::KraitRpcWrapper::AddSubscriber(const char *topic)
 {
 	return ((_Initialized) ? px4muorb_add_subscriber(topic) : -1);

src/modules/muorb/krait/px4muorb_KraitRpcWrapper.hpp

@@ -65,6 +65,8 @@ public:
 	/**
 	 * Muorb related functions to pub/sub of orb topic from krait to adsp
 	 */
+	int32_t TopicAdvertised(const char *topic);
+	int32_t TopicUnadvertised(const char *topic);
 	int32_t AddSubscriber(const char *topic);
 	int32_t RemoveSubscriber(const char *topic);
 	int32_t SendData(const char *topic, int32_t length_in_bytes, const uint8_t *data);

src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp

@@ -67,6 +67,27 @@ uORB::KraitFastRpcChannel::KraitFastRpcChannel() :
 	_KraitWrapper.Initialize();
 }
 
+int16_t uORB::KraitFastRpcChannel::topic_advertised(const char *messageName)
+{
+	int16_t rc = 0;
+	PX4_DEBUG("Before calling TopicAdvertised for [%s]\n", messageName);
+	rc = _KraitWrapper.TopicAdvertised(messageName);
+	PX4_DEBUG("Response for TopicAdvertised for [%s], rc[%d]\n", messageName, rc);
+	return rc;
+}
+
+/*
+//TODO: verify if needed
+int16_t uORB::KraitFastRpcChannel::topic_unadvertised(const char *messageName)
+{
+	int16_t rc = 0;
+	PX4_DEBUG("Before calling TopicUnadvertised for [%s]\n", messageName);
+	rc = _KraitWrapper.TopicUnadvertised(messageName);
+	PX4_DEBUG("Response for TopicUnadvertised for [%s], rc[%d]\n", messageName, rc);
+	return rc;
+}
+*/
+
 int16_t uORB::KraitFastRpcChannel::add_subscription(const char *messageName, int32_t msgRateInHz)
 {
 	int16_t rc = 0;
@@ -254,9 +275,14 @@ void uORB::KraitFastRpcChannel::fastrpc_recv_thread()
 				uint8_t *topic_data = (uint8_t *)(messageName + strlen(messageName) + 1);
 
 				if (_RxHandler != nullptr) {
-					_RxHandler->process_received_message(messageName,
-									     header->_DataLen, topic_data);
-					//PX4_DEBUG( "Received topic data for control message for: [%s] len[%d]\n", name, data_length );
+					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);
+					} else if (header->_MsgType == _CONTROL_MSG_TYPE_ADVERTISE) {
+						PX4_DEBUG( "Received topic for control message for: [%s] len[%d]\n", messageName, data_length );
+						_RxHandler->process_remote_topic(messageName, true);
+					}
 				}
 
 				bytes_processed += header->_MsgNameLen + header->_DataLen + sizeof(struct BulkTransferHeader);

src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp

@@ -70,6 +70,32 @@ public:
 		return (_InstancePtr != nullptr);
 	}
 
+	/**
+	 * @brief Interface to notify the remote entity of a topic being advertised.
+	 *
+	 * @param messageName
+	 * 	This represents the uORB message name(aka topic); This message name should be
+	 * 	globally unique.
+	 * @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 topic_advertised(const char *messageName);
+
+	/**
+	 * @brief Interface to notify the remote entity of a topic being unadvertised
+	 * and is no longer publishing messages.
+	 *
+	 * @param messageName
+	 * 	This represents the uORB message name(aka topic); This message name should be
+	 * 	globally unique.
+	 * @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 topic_unadvertised(const char *messageName);
 
 	/**
 	 * @brief Interface to notify the remote entity of interest of a
@@ -141,8 +167,10 @@ private: // data members
 	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;
+	static const int32_t _CONTROL_MSG_TYPE_ADVERTISE = 4;
 
 	struct BulkTransferHeader {
+		uint16_t _MsgType;
 		uint16_t _MsgNameLen;
 		uint16_t _DataLen;
 	};