diff --git a/libraries/AP_HAL_AVR/AP_HAL_AVR_Main.h b/libraries/AP_HAL_AVR/AP_HAL_AVR_Main.h
index 689e7e3634..b2d2949c7b 100644
--- a/libraries/AP_HAL_AVR/AP_HAL_AVR_Main.h
+++ b/libraries/AP_HAL_AVR/AP_HAL_AVR_Main.h
@@ -5,11 +5,9 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
 #define AP_HAL_MAIN() extern "C" {\
     int main (void) {\
-	hal.init(0, NULL);			\
-        setup();\
-        hal.scheduler->system_initialized(); \
-        for(;;) loop();\
-        return 0;\
+        AP_HAL::HAL::FunCallbacks callbacks(setup, loop); \
+        hal.run(0, NULL, &callbacks); \
+        return 0; \
     }\
     }
 #endif
diff --git a/libraries/AP_HAL_AVR/HAL_AVR_APM1_Class.cpp b/libraries/AP_HAL_AVR/HAL_AVR_APM1_Class.cpp
index b27b73b698..be5e8cd56e 100644
--- a/libraries/AP_HAL_AVR/HAL_AVR_APM1_Class.cpp
+++ b/libraries/AP_HAL_AVR/HAL_AVR_APM1_Class.cpp
@@ -6,6 +6,8 @@
  * wrap the whole HAL_AVR_APM1 class declaration and definition. */
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
 
+#include <assert.h>
+
 #include "AP_HAL_AVR.h"
 #include "AP_HAL_AVR_private.h"
 #include "HAL_AVR_APM1_Class.h"
@@ -86,6 +88,20 @@ void HAL_AVR_APM1::init(int argc, char * const argv[]) const {
     PORTJ |= _BV(0);
 };
 
+void HAL_AVR_APM1::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_AVR_APM1 hal;
     return hal;
diff --git a/libraries/AP_HAL_AVR/HAL_AVR_APM1_Class.h b/libraries/AP_HAL_AVR/HAL_AVR_APM1_Class.h
index 1c367f3e43..edff329c10 100644
--- a/libraries/AP_HAL_AVR/HAL_AVR_APM1_Class.h
+++ b/libraries/AP_HAL_AVR/HAL_AVR_APM1_Class.h
@@ -21,6 +21,7 @@ class HAL_AVR_APM1 : public AP_HAL::HAL {
 public:
     HAL_AVR_APM1();
     void init(int argc, char * const argv[]) const;
+    void run(int argc, char* const argv[], Callbacks* callbacks) const override;
 };
 
 /**
diff --git a/libraries/AP_HAL_AVR/HAL_AVR_APM2_Class.cpp b/libraries/AP_HAL_AVR/HAL_AVR_APM2_Class.cpp
index 638c06c8f8..1520403c78 100644
--- a/libraries/AP_HAL_AVR/HAL_AVR_APM2_Class.cpp
+++ b/libraries/AP_HAL_AVR/HAL_AVR_APM2_Class.cpp
@@ -6,6 +6,8 @@
  * wrap the whole HAL_AVR_APM2 class declaration and definition. */
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM2
 
+#include <assert.h>
+
 #include "AP_HAL_AVR.h"
 #include "AP_HAL_AVR_private.h"
 #include "HAL_AVR_APM2_Class.h"
@@ -85,6 +87,20 @@ void HAL_AVR_APM2::init(int argc, char * const argv[]) const {
     PORTH |= _BV(0);
 };
 
+void HAL_AVR_APM2::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_AVR_APM2 hal;
     return hal;
diff --git a/libraries/AP_HAL_AVR/HAL_AVR_APM2_Class.h b/libraries/AP_HAL_AVR/HAL_AVR_APM2_Class.h
index b8428b85df..b8cc539d3d 100644
--- a/libraries/AP_HAL_AVR/HAL_AVR_APM2_Class.h
+++ b/libraries/AP_HAL_AVR/HAL_AVR_APM2_Class.h
@@ -21,6 +21,7 @@ class HAL_AVR_APM2 : public AP_HAL::HAL {
 public:
     HAL_AVR_APM2();
     void init(int argc, char * const argv[]) const;
+    void run(int argc, char* const argv[], Callbacks* callbacks) const override;
 };
 
 /**