From d219076d52dd11d6b790c682c58d4311b478f889 Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Wed, 1 Jul 2015 21:33:21 -0700
Subject: [PATCH] POSIX: added muorb tests

Unit tests for muorb on posix build. These run on the Krait processor.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 src/platforms/posix/tests/muorb/module.mk     |  6 +-
 .../posix/tests/muorb/muorb_test_example.cpp  | 77 +++++++++++++++++--
 .../posix/tests/muorb/muorb_test_example.h    |  2 +
 .../posix/tests/muorb/muorb_test_main.cpp     |  7 --
 4 files changed, 76 insertions(+), 16 deletions(-)

diff --git a/src/platforms/posix/tests/muorb/module.mk b/src/platforms/posix/tests/muorb/module.mk
index 15bf4824e9..880fe82007 100644
--- a/src/platforms/posix/tests/muorb/module.mk
+++ b/src/platforms/posix/tests/muorb/module.mk
@@ -37,9 +37,9 @@
 
 MODULE_COMMAND	= muorb_test
 
-INCLUDE_DIRS           += ${PX4_BASE}../muorb_krait \
-                          ${PX4_BASE}../muorb_krait/lib/include \
-                          ${PX4_BASE}../muorb_krait/Pal/lib
+INCLUDE_DIRS           += \
+                          $(EXT_MUORB_LIB_ROOT)/krait/include \
+                          $(PX4_BASE)src/modules/muorb/krait 
 
 SRCS		= muorb_test_main.cpp \
 		  muorb_test_start_posix.cpp \
diff --git a/src/platforms/posix/tests/muorb/muorb_test_example.cpp b/src/platforms/posix/tests/muorb/muorb_test_example.cpp
index 37dfec5ed3..3bae353f2f 100644
--- a/src/platforms/posix/tests/muorb/muorb_test_example.cpp
+++ b/src/platforms/posix/tests/muorb/muorb_test_example.cpp
@@ -48,9 +48,16 @@ px4::AppState MuorbTestExample::appState;
 
 int MuorbTestExample::main()
 {
+	int rc;
 	appState.setRunning(true);
+	rc =  PingPongTest();
+	appState.setRunning(false);
+	return rc;
+}
 
-	int i=0;
+int  MuorbTestExample::DefaultTest()
+{
+        int i=0;
         orb_advert_t pub_id = orb_advertise( ORB_ID( esc_status ), & m_esc_status );
         if( pub_id == 0 )
         {
@@ -75,9 +82,9 @@ int MuorbTestExample::main()
            return -1;
         }
 
-	while (!appState.exitRequested() && i<100) {
+        while (!appState.exitRequested() && i<100) {
 
-		PX4_DEBUG("[%d]  Doing work...", i );
+                PX4_DEBUG("[%d]  Doing work...", i );
                 if( orb_publish( ORB_ID( esc_status ), pub_id, &m_esc_status ) == PX4_ERROR )
                 {
                    PX4_ERR( "[%d]Error publishing the esc status message for iter", i );
@@ -111,8 +118,66 @@ int MuorbTestExample::main()
                     break;
                 } 
                 
-		++i;
-	}
+         ++i;
+        }
+        return 0;
+}
+
+int MuorbTestExample::PingPongTest()
+{
+        int i=0;
+        orb_advert_t pub_id_vc = orb_advertise( ORB_ID( vehicle_command ), & m_vc );
+        if( pub_id_vc == 0 )
+        {
+            PX4_ERR( "error publishing vehicle_command" );
+            return -1;
+        }
+        if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR )
+        {
+           PX4_ERR( "[%d]Error publishing the vechile command message", i );
+           return -1;
+        }
+        int sub_esc_status = orb_subscribe( ORB_ID( esc_status ) );
+        if ( sub_esc_status == PX4_ERROR )
+        {
+           PX4_ERR( "Error subscribing to esc_status topic" );
+           return -1;
+        }
+
+        while (!appState.exitRequested() ) {
+
+                PX4_INFO("[%d]  Doing work...", i );
+                bool updated = false;
+                if( orb_check( sub_esc_status, &updated ) == 0 )
+                {
+                    if( updated )
+                    {
+                       PX4_INFO( "[%d]ESC status is updated... reading new value", i );
+                       if( orb_copy( ORB_ID( esc_status ), sub_esc_status, &m_esc_status ) != 0 )
+                       {
+                           PX4_ERR( "[%d]Error calling orb copy for esc status... ", i );
+                           break;
+                       }
+                       if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR )
+                       {
+                          PX4_ERR( "[%d]Error publishing the vechile command message", i );
+                          break;
+                       }
+                    }
+                    else
+                    {
+                       PX4_INFO( "[%d] esc status topic is not updated", i );
+                    }
+                }
+                else
+                {
+                    PX4_ERR( "[%d]Error checking the updated status for esc status... ", i );
+                    break;
+                }
+                // sleep for 1 sec.
+                usleep( 1000000 ); 
 
-	return 0;
+                ++i;
+        }
+        return 0;
 }
diff --git a/src/platforms/posix/tests/muorb/muorb_test_example.h b/src/platforms/posix/tests/muorb/muorb_test_example.h
index a3625167c7..c5d699ae7d 100644
--- a/src/platforms/posix/tests/muorb/muorb_test_example.h
+++ b/src/platforms/posix/tests/muorb/muorb_test_example.h
@@ -53,6 +53,8 @@ public:
 
 	static px4::AppState appState; /* track requests to terminate app */
 private:
+   int DefaultTest();
+   int PingPongTest();
    struct esc_status_s m_esc_status;
    struct vehicle_command_s m_vc;
    
diff --git a/src/platforms/posix/tests/muorb/muorb_test_main.cpp b/src/platforms/posix/tests/muorb/muorb_test_main.cpp
index 6ebc0ae92c..effa9ff88b 100644
--- a/src/platforms/posix/tests/muorb/muorb_test_main.cpp
+++ b/src/platforms/posix/tests/muorb/muorb_test_main.cpp
@@ -51,16 +51,9 @@ int PX4_MAIN(int argc, char **argv)
 
 	PX4_DEBUG("muorb_test");
        
-        // 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(); 
-
 	MuorbTestExample hello;
 	hello.main();
 
-        uORB::KraitFastRpcChannel::GetInstance()->Stop(); 
 	PX4_DEBUG("goodbye");
 	return 0;
 }