Copter: move esc calibration to separate file

Restructured into case statement
Replaced use of g.throttle_max with definition
Added more comments
Send message to ground station instead of printing on console (although
probably both are unlikely to be read)

ArduCopter/esc_calibration.pde

@@ -0,0 +1,90 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*****************************************************************************
+*  esc_calibration.pde : functions to check and perform ESC calibration
+*****************************************************************************/
+
+#define ESC_CALIBRATION_HIGH_THROTTLE   950
+
+// enum for ESC CALIBRATION
+enum ESCCalibrationModes {
+    ESCCAL_NONE = 0,
+    ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH = 1,
+    ESCCAL_PASSTHROUGH_ALWAYS = 2
+};
+
+// check if we should enter esc calibration mode
+static void esc_calibration_startup_check()
+{
+    // exit immediately if pre-arm rc checks fail
+    pre_arm_rc_checks();
+    if (!ap.pre_arm_rc_check) {
+        // clear esc flag for next time
+        if (g.esc_calibrate != ESCCAL_NONE) {
+            g.esc_calibrate.set_and_save(ESCCAL_NONE);
+        }
+        return;
+    }
+
+    // check ESC parameter
+    switch (g.esc_calibrate) {
+        case ESCCAL_NONE:
+            // check if throttle is high
+            if (g.rc_3.control_in >= ESC_CALIBRATION_HIGH_THROTTLE) {
+                // we will enter esc_calibrate mode on next reboot
+                g.esc_calibrate.set_and_save(ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH);
+                // send message to gcs
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("ESC Cal: restart board"));
+                // turn on esc calibration notification
+                AP_Notify::flags.esc_calibration = true;
+                // block until we restart
+                while(1) { delay(5); }
+            }
+            break;
+        case ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH:
+            // check if throttle is high
+            if (g.rc_3.control_in >= ESC_CALIBRATION_HIGH_THROTTLE) {
+                // pass through pilot throttle to escs
+                esc_calibration_passthrough();
+            }
+            break;
+        case ESCCAL_PASSTHROUGH_ALWAYS:
+            // pass through pilot throttle to escs
+            esc_calibration_passthrough();
+            break;
+        default:
+            // do nothing
+            break;
+    }
+    // clear esc flag for next time
+    g.esc_calibrate.set_and_save(ESCCAL_NONE);
+}
+
+// esc_calibration_passthrough - pass through pilot throttle to escs
+static void esc_calibration_passthrough()
+{
+    // clear esc flag for next time
+    g.esc_calibrate.set_and_save(ESCCAL_NONE);
+
+    // reduce update rate to motors to 50Hz
+    motors.set_update_rate(50);
+
+    // send message to GCS
+    gcs_send_text_P(SEVERITY_HIGH,PSTR("ESC Cal: passing pilot thr to ESCs"));
+
+    while(1) {
+        // arm motors
+        motors.armed(true);
+        motors.enable();
+
+        // flash LEDS
+        AP_Notify::flags.esc_calibration = true;
+
+        // read pilot input
+        read_radio();
+        delay(10);
+
+        // pass through to motors
+        motors.throttle_pass_through(g.rc_3.radio_in);
+    }
+}

ArduCopter/radio.pde

@@ -56,29 +56,8 @@ static void init_rc_out()
     // we want the input to be scaled correctly
     g.rc_3.set_range_out(0,1000);
 
-    // full throttle means to enter ESC calibration
-    if(g.rc_3.control_in >= (g.throttle_max - 50) || (g.esc_calibrate == 2)) {
-        if(g.esc_calibrate == 0) {
-            // we will enter esc_calibrate mode on next reboot
-            g.esc_calibrate.set_and_save(1);
-            // display message on console
-            cliSerial->printf_P(PSTR("Entering ESC Calibration: please restart APM.\n"));
-            // turn on esc calibration notification
-            AP_Notify::flags.esc_calibration = true;
-            // block until we restart
-            while(1) { delay(5); }
-        }else{
-            cliSerial->printf_P(PSTR("ESC Calibration active: passing throttle through to ESCs.\n"));
-            // clear esc flag
-            g.esc_calibrate.set_and_save(0);
-            // pass through user throttle to escs
-            init_esc();
-        }
-    }else{
-        // did we abort the calibration?
-        if(g.esc_calibrate == 1)
-            g.esc_calibrate.set_and_save(0);
-    }
+    // check if we should enter esc calibration mode
+    esc_calibration_startup_check();
 
     // enable output to motors
     pre_arm_rc_checks();

ArduCopter/setup.pde

@@ -522,34 +522,6 @@ static void print_enabled(bool b)
     cliSerial->print_P(PSTR("abled\n"));
 }
 
-
-static void
-init_esc()
-{
-    // reduce update rate to motors to 50Hz
-    motors.set_update_rate(50);
-
-    uint32_t last_print_ms = 0;
-    while(1) {
-        motors.armed(true);
-        motors.enable();
-        read_radio();
-        delay(10);
-        AP_Notify::flags.esc_calibration = true;
-        motors.throttle_pass_through();
-        
-        uint32_t now = hal.scheduler->millis();
-        if (now - last_print_ms > 1000) {
-            hal.console->printf_P(PSTR("ESC cal input: %u %u %u %u  output: %u %u %u %u\n"),
-                                  (unsigned)hal.rcin->read(0), (unsigned)hal.rcin->read(1), 
-                                  (unsigned)hal.rcin->read(2), (unsigned)hal.rcin->read(3),
-                                  (unsigned)hal.rcout->read(0), (unsigned)hal.rcout->read(1), 
-                                  (unsigned)hal.rcout->read(2), (unsigned)hal.rcout->read(3));
-            last_print_ms = now;
-        }
-    }
-}
-
 static void report_version()
 {
     cliSerial->printf_P(PSTR("FW Ver: %d\n"),(int)g.k_format_version);