Copter: skeleton init call for all flight modes

ArduCopter/control_stabilize.pde

@@ -1,5 +1,11 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+// acro_init - initialise acro controller
+static bool acro_init()
+{
+    return true;
+}
+
 // acro_run - runs the acro controller
 // should be called at 100hz or more
 static void acro_run()
@@ -32,6 +38,12 @@ static void acro_run()
     // body-frame rate controller is run directly from 100hz loop
 }
 
+// stabilize_init - initialise stabilize controller
+static bool stabilize_init()
+{
+    return true;
+}
+
 // stabilize_run - runs the main stabilize controller
 // should be called at 100hz or more
 static void stabilize_run()
@@ -88,12 +100,24 @@ static void stabilize_run()
     // To-Do: add throttle control for stabilize mode here?
 }
 
+// althold_init - initialise althold controller
+static bool althold_init()
+{
+    return true;
+}
+
 // althold_run - runs the althold controller
 // should be called at 100hz or more
 static void althold_run()
 {
 }
 
+// auto_init - initialise auto controller
+static bool auto_init()
+{
+    return true;
+}
+
 // auto_run - runs the auto controller
 // should be called at 100hz or more
 static void auto_run()
@@ -127,24 +151,48 @@ static void auto_run()
     // body-frame rate controller is run directly from 100hz loop
 }
 
+// circle_init - initialise circle controller
+static bool circle_init()
+{
+    return true;
+}
+
 // circle_run - runs the circle controller
 // should be called at 100hz or more
 static void circle_run()
 {
 }
 
+// loiter_init - initialise loiter controller
+static bool loiter_init()
+{
+    return true;
+}
+
 // loiter_run - runs the loiter controller
 // should be called at 100hz or more
 static void loiter_run()
 {
 }
 
+// guided_init - initialise guided controller
+static bool guided_init()
+{
+    return true;
+}
+
 // guided_run - runs the guided controller
 // should be called at 100hz or more
 static void guided_run()
 {
 }
 
+// land_init - initialise land controller
+static bool land_init()
+{
+    return true;
+}
+
 // land_run - runs the land controller
 // should be called at 100hz or more
 static void land_run()
@@ -152,6 +200,12 @@ static void land_run()
     verify_land();
 }
 
+// rtl_init - initialise rtl controller
+static bool rtl_init()
+{
+    return true;
+}
+
 // rtl_run - runs the return-to-launch controller
 // should be called at 100hz or more
 static void rtl_run()
@@ -159,18 +213,36 @@ static void rtl_run()
     verify_RTL();
 }
 
+// ofloiter_init - initialise ofloiter controller
+static bool ofloiter_init()
+{
+    return true;
+}
+
 // ofloiter_run - runs the optical flow loiter controller
 // should be called at 100hz or more
 static void ofloiter_run()
 {
 }
 
+// drift_init - initialise drift controller
+static bool drift_init()
+{
+    return true;
+}
+
 // drift_run - runs the drift controller
 // should be called at 100hz or more
 static void drift_run()
 {
 }
 
+// sport_init - initialise sport controller
+static bool sport_init()
+{
+    return true;
+}
+
 // sport_run - runs the sport controller
 // should be called at 100hz or more
 static void sport_run()

ArduCopter/system.pde

@@ -384,7 +384,7 @@ static bool set_mode(uint8_t mode)
 
     switch(mode) {
         case ACRO:
-            success = true;
+            success = acro_init();
             set_yaw_mode(ACRO_YAW);
             set_roll_pitch_mode(ACRO_RP);
             set_throttle_mode(ACRO_THR);
@@ -392,7 +392,7 @@ static bool set_mode(uint8_t mode)
             break;
 
         case STABILIZE:
-            success = true;
+            success = stabilize_init();
             set_yaw_mode(STABILIZE_YAW);
             set_roll_pitch_mode(STABILIZE_RP);
             set_throttle_mode(STABILIZE_THR);
@@ -400,7 +400,7 @@ static bool set_mode(uint8_t mode)
             break;
 
         case ALT_HOLD:
-            success = true;
+            success = althold_init();
             set_yaw_mode(ALT_HOLD_YAW);
             set_roll_pitch_mode(ALT_HOLD_RP);
             set_throttle_mode(ALT_HOLD_THR);
@@ -410,15 +410,15 @@ static bool set_mode(uint8_t mode)
         case AUTO:
             // check we have a GPS and at least one mission command (note the home position is always command 0)
             if ((GPS_ok() && g.command_total > 1) || ignore_checks) {
-                success = true;
+                success = auto_init();
                 // roll-pitch, throttle and yaw modes will all be set by the first nav command
                 init_commands();            // clear the command queues. will be reloaded when "run_autopilot" calls "update_commands" function
-            }
+            }            
             break;
 
         case CIRCLE:
             if (GPS_ok() || ignore_checks) {
-                success = true;
+                success = circle_init();
                 set_roll_pitch_mode(CIRCLE_RP);
                 set_throttle_mode(CIRCLE_THR);
                 set_nav_mode(CIRCLE_NAV);
@@ -428,7 +428,7 @@ static bool set_mode(uint8_t mode)
 
         case LOITER:
             if (GPS_ok() || ignore_checks) {
-                success = true;
+                success = loiter_init();
                 set_yaw_mode(LOITER_YAW);
                 set_roll_pitch_mode(LOITER_RP);
                 set_throttle_mode(LOITER_THR);
@@ -448,7 +448,7 @@ static bool set_mode(uint8_t mode)
 
         case GUIDED:
             if (GPS_ok() || ignore_checks) {
-                success = true;
+                success = guided_init();
                 set_yaw_mode(get_wp_yaw_mode(false));
                 set_roll_pitch_mode(GUIDED_RP);
                 set_throttle_mode(GUIDED_THR);
@@ -457,20 +457,20 @@ static bool set_mode(uint8_t mode)
             break;
 
         case LAND:
-            success = true;
+            success = land_init();
             do_land(NULL);  // land at current location
             break;
 
         case RTL:
             if (GPS_ok() || ignore_checks) {
-                success = true;
+                success = rtl_init();
                 do_RTL();
             }
             break;
 
         case OF_LOITER:
             if (g.optflow_enabled || ignore_checks) {
-                success = true;
+                success = ofloiter_init();
                 set_yaw_mode(OF_LOITER_YAW);
                 set_roll_pitch_mode(OF_LOITER_RP);
                 set_throttle_mode(OF_LOITER_THR);
@@ -479,7 +479,7 @@ static bool set_mode(uint8_t mode)
             break;
 
         case DRIFT:
-            success = true;
+            success = drift_init();
             set_yaw_mode(YAW_DRIFT);
             set_roll_pitch_mode(ROLL_PITCH_DRIFT);
             set_nav_mode(NAV_NONE);
@@ -487,7 +487,7 @@ static bool set_mode(uint8_t mode)
             break;
 
         case SPORT:
-            success = true;
+            success = sport_init();
             set_yaw_mode(SPORT_YAW);
             set_roll_pitch_mode(SPORT_RP);
             set_throttle_mode(SPORT_THR);