Provide impelmentation of px4muorb_KraitRpcWrapper

The Terminate function is not properly implemented yet.

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

src/modules/commander/commander.cpp

@@ -2982,7 +2982,7 @@ set_control_mode()
 	/* set vehicle_control_mode according to set_navigation_state */
 	control_mode.flag_armed = armed.armed;
 	control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol);
-	control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON;
+	//control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON;
 	control_mode.flag_control_offboard_enabled = false;
 
 	switch (status.nav_state) {

src/modules/muorb/krait/CMakeLists.txt

@@ -30,12 +30,15 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+include_directories(${CMAKE_BINARY_DIR}/src/firmware/posix)
+include_directories(${HEXAGON_SDK_ROOT}/inc/stddef)
 
 px4_add_module(
 	MODULE modules__muorb__krait
 	MAIN muorb
 	SRCS
 		uORBKraitFastRpcChannel.cpp
+		px4muorb_KraitRpcWrapper.cpp
 		muorb_main.cpp
 	DEPENDS
 		platforms__common

src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp

@@ -30,47 +30,77 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-#ifndef _px4muorb_KraitRpcWrapper_hpp_
-#define _px4muorb_KraitRpcWrapper_hpp_
 #include <stdint.h>
+#include "px4muorb_KraitRpcWrapper.hpp"
+#include "px4muorb.h"
 
-namespace px4muorb
-{
-class KraitRpcWrapper;
+using namespace px4muorb;
+
+/**
+ * Constructor
+ */
+KraitRpcWrapper::KraitRpcWrapper() {}
+
+/**
+ * destructor
+ */
+KraitRpcWrapper::~KraitRpcWrapper() {}
+
+/**
+ * Initiatizes the rpc channel px4 muorb
+ */
+bool KraitRpcWrapper::Initialize()
+{ 
+	return (px4muorb_orb_initialize() == 0);
 }
 
-class px4muorb::KraitRpcWrapper
+/**
+ * Terminate to clean up the resources.  This should be called at program exit
+ */
+bool KraitRpcWrapper::Terminate()
 {
-public:
-	/**
-	 * Constructor
-	 */
-	KraitRpcWrapper() {}
+	// FIXME - TBD
+	return true;
+}
 
-	/**
-	 * destructor
-	 */
-	~KraitRpcWrapper() {}
+/**
+ * Muorb related functions to pub/sub of orb topic from krait to adsp
+ */
+int32_t KraitRpcWrapper::AddSubscriber(const char *topic)
+{
+	return px4muorb_add_subscriber(topic);
+}
 
-	/**
-	 * Initiatizes the rpc channel px4 muorb
-	 */
-	bool Initialize() { return true; }
+int32_t KraitRpcWrapper::RemoveSubscriber(const char *topic)
+{
+	return px4muorb_remove_subscriber(topic);
+}
 
-	/**
-	 * Terminate to clean up the resources.  This should be called at program exit
-	 */
-	bool Terminate() { return true; }
+int32_t KraitRpcWrapper::SendData(const char *topic, int32_t length_in_bytes, const uint8_t *data)
+{
+	return px4muorb_send_topic_data(topic, data, length_in_bytes);
+}
+
+int32_t KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic, int32_t *length_in_bytes, uint8_t **data)
+{
+	// FIXME ??
+	int topic_nameLen = 0;
+	int rv = px4muorb_receive_msg(msg_type, *topic, topic_nameLen, *data, *length_in_bytes, length_in_bytes);
+	return rv;
+}
+
+int32_t KraitRpcWrapper::IsSubscriberPresent(const char *topic, int32_t *status)
+{
+	return px4muorb_is_subscriber_present(topic, status);
+}
+
+int32_t KraitRpcWrapper::ReceiveBulkData(uint8_t **bulk_data, int32_t *length_in_bytes, int32_t *topic_count)
+{
+	return px4muorb_receive_bulk_data(*bulk_data, *length_in_bytes, length_in_bytes, topic_count);
+}
+
+int32_t KraitRpcWrapper::UnblockReceiveData()
+{
+	return px4muorb_unblock_recieve_msg();
+}
 
-	/**
-	 * Muorb related functions to pub/sub of orb topic from krait to adsp
-	 */
-	int32_t AddSubscriber(const char *topic) { return 1; }
-	int32_t RemoveSubscriber(const char *topic) { return 1; }
-	int32_t SendData(const char *topic, int32_t length_in_bytes, const uint8_t *data) { return 1; }
-	int32_t ReceiveData(int32_t *msg_type, char **topic, int32_t *length_in_bytes, uint8_t **data) { return 1; }
-	int32_t IsSubscriberPresent(const char *topic, int32_t *status) { return 1; }
-	int32_t ReceiveBulkData(uint8_t **bulk_data, int32_t *length_in_bytes, int32_t *topic_count) { return 1; }
-	int32_t UnblockReceiveData() { return 1; }
-};
-#endif // _px4muorb_KraitWrapper_hpp_

src/modules/muorb/krait/px4muorb_KraitRpcWrapper.hpp

@@ -0,0 +1,71 @@
+//=============================================================================
+// File: px4muorb_KraitRpcWrapper.hpp
+//
+//  @@-COPYRIGHT-START-@@
+//
+// Copyright 2015 Qualcomm Technologies, Inc. All rights reserved.
+// Confidential & Proprietary - Qualcomm Technologies, Inc. ("QTI")
+//
+// The party receiving this software directly from QTI (the "Recipient")
+// may use this software as reasonably necessary solely for the purposes
+// set forth in the agreement between the Recipient and QTI (the
+// "Agreement"). The software may be used in source code form solely by
+// the Recipient's employees (if any) authorized by the Agreement. Unless
+// expressly authorized in the Agreement, the Recipient may not sublicense,
+// assign, transfer or otherwise provide the source code to any third
+// party. Qualcomm Technologies, Inc. retains all ownership rights in and
+// to the software
+//
+// This notice supersedes any other QTI notices contained within the software
+// except copyright notices indicating different years of publication for
+// different portions of the software. This notice does not supersede the
+// application of any third party copyright notice to that third party's
+// code.
+//
+//  @@-COPYRIGHT-END-@@
+//
+//=============================================================================
+#ifndef _px4muorb_KraitRpcWrapper_hpp_
+#define _px4muorb_KraitRpcWrapper_hpp_
+#include <stdint.h>
+
+namespace px4muorb
+{
+   class KraitRpcWrapper;
+}
+
+class px4muorb::KraitRpcWrapper
+{
+public:
+   /**
+    * Constructor
+    */
+   KraitRpcWrapper();
+   
+   /**
+    * destructor
+    */
+   ~KraitRpcWrapper();
+
+   /**
+    * Initiatizes the rpc channel px4 muorb
+    */
+   bool Initialize();
+
+   /**
+    * Terminate to clean up the resources.  This should be called at program exit
+    */
+   bool Terminate();
+
+   /**
+    * Muorb related functions to pub/sub of orb topic from krait to adsp
+    */
+   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 );
+   int32_t ReceiveData( int32_t* msg_type, char** topic, int32_t* length_in_bytes, uint8_t** data );
+   int32_t IsSubscriberPresent( const char* topic, int32_t* status );
+   int32_t ReceiveBulkData( uint8_t** bulk_data, int32_t* length_in_bytes, int32_t* topic_count );
+   int32_t UnblockReceiveData();
+};
+#endif // _px4muorb_KraitWrapper_hpp_