From c6292ff097d3be3bfa553159b1d3c268bb5462d0 Mon Sep 17 00:00:00 2001
From: Caio Marcelo de Oliveira Filho <caio.oliveira@intel.com>
Date: Mon, 19 Oct 2015 12:26:18 -0200
Subject: [PATCH] AP_HAL_SITL: implement HAL::run()

---
 libraries/AP_HAL_SITL/AP_HAL_SITL_Main.h | 12 ++++--------
 libraries/AP_HAL_SITL/HAL_SITL_Class.cpp | 16 ++++++++++++++++
 libraries/AP_HAL_SITL/HAL_SITL_Class.h   |  1 +
 3 files changed, 21 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_HAL_SITL/AP_HAL_SITL_Main.h b/libraries/AP_HAL_SITL/AP_HAL_SITL_Main.h
index 979ffcb2c3..518238a982 100644
--- a/libraries/AP_HAL_SITL/AP_HAL_SITL_Main.h
+++ b/libraries/AP_HAL_SITL/AP_HAL_SITL_Main.h
@@ -4,14 +4,10 @@
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 #define AP_HAL_MAIN() extern "C" {\
-    int main (int argc, char * const argv[]) {	\
-	hal.init(argc, argv); \
-        setup(); \
-        hal.scheduler->system_initialized(); \
-        for(;;) { \
-		loop(); \
-	} \
-        return 0;\
+    int main (int argc, char * const argv[]) { \
+        AP_HAL::HAL::FunCallbacks callbacks(setup, loop); \
+        hal.run(argc, argv, &callbacks); \
+        return 0; \
     }\
     }
 #endif
diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
index efd9e8043e..b28ecbcdc8 100644
--- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
+++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
@@ -4,6 +4,8 @@
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 
+#include <assert.h>
+
 #include "AP_HAL_SITL.h"
 #include "AP_HAL_SITL_Namespace.h"
 #include "HAL_SITL_Class.h"
@@ -79,6 +81,20 @@ void HAL_SITL::init(int argc, char * const argv[]) const
     analogin->init(NULL);
 }
 
+void HAL_SITL::run(int argc, char * const argv[], Callbacks* callbacks) const
+{
+    assert(callbacks);
+
+    init(argc, argv);
+
+    callbacks->setup();
+    scheduler->system_initialized();
+
+    for (;;) {
+        callbacks->loop();
+    }
+}
+
 const AP_HAL::HAL& AP_HAL::get_HAL() {
     static const HAL_SITL hal;
     return hal;
diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.h b/libraries/AP_HAL_SITL/HAL_SITL_Class.h
index 1a5bbe3e70..0749f47c9e 100644
--- a/libraries/AP_HAL_SITL/HAL_SITL_Class.h
+++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.h
@@ -14,6 +14,7 @@ class HAL_SITL : public AP_HAL::HAL {
 public:
     HAL_SITL();
     void init(int argc, char * const argv[]) const;
+    void run(int argc, char * const argv[], Callbacks* callbacks) const override;
 
 private:
     HALSITL::SITL_State *_sitl_state;