From 9d0f40b432d7b6e7663b1d26bf31c018b414e344 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 16 May 2013 16:32:00 +0900
Subject: [PATCH] Copter: add pre_arm_rc_check

Motors are not be enabled unless we have passed rc checks which include
checking the radio in calibration has been performed.
---
 ArduCopter/ArduCopter.pde | 41 ++++++++++++++++++++-------------------
 ArduCopter/motors.pde     | 34 ++++++++++++++++++++++++++++----
 ArduCopter/radio.pde      | 16 +++++++--------
 ArduCopter/setup.pde      | 11 ++++++++++-
 ArduCopter/system.pde     |  3 ++-
 ArduCopter/test.pde       |  9 ++++++++-
 6 files changed, 78 insertions(+), 36 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index c0054897ea..b7d7007cb8 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -359,28 +359,29 @@ static AP_RangeFinder_MaxsonarXL *sonar;
 //Documentation of GLobals:
 static union {
     struct {
-        uint8_t home_is_set        : 1; // 0
-        uint8_t simple_mode        : 1; // 1    // This is the state of simple mode
-        uint8_t manual_attitude    : 1; // 2
-        uint8_t manual_throttle    : 1; // 3
+        uint8_t home_is_set         : 1; // 0
+        uint8_t simple_mode         : 1; // 1    // This is the state of simple mode
+        uint8_t manual_attitude     : 1; // 2
+        uint8_t manual_throttle     : 1; // 3
 
-        uint8_t low_battery        : 1; // 4    // Used to track if the battery is low - LED output flashes when the batt is low
-        uint8_t pre_arm_check      : 1; // 5    // true if the radio and accel calibration have been performed
-        uint8_t logging_started    : 1; // 6    // true if dataflash logging has started
-        uint8_t auto_armed         : 1; // 7    // stops auto missions from beginning until throttle is raised
+        uint8_t pre_arm_rc_check    : 1; // 5    // true if rc input pre-arm checks have been completed successfully
+        uint8_t pre_arm_check       : 1; // 6    // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
+        uint8_t auto_armed          : 1; // 7    // stops auto missions from beginning until throttle is raised
+        uint8_t logging_started     : 1; // 8    // true if dataflash logging has started
 
-        uint8_t failsafe_radio     : 1; // 8    // A status flag for the radio failsafe
-        uint8_t failsafe_batt      : 1; // 9    // A status flag for the battery failsafe
-        uint8_t failsafe_gps       : 1; // 10   // A status flag for the gps failsafe
-        uint8_t failsafe_gcs       : 1; // 11   // A status flag for the ground station failsafe
-        uint8_t rc_override_active : 1; // 12   // true if rc control are overwritten by ground station
-        uint8_t do_flip            : 1; // 13   // Used to enable flip code
-        uint8_t takeoff_complete   : 1; // 14
-        uint8_t land_complete      : 1; // 15
-        uint8_t compass_status     : 1; // 16
-        uint8_t gps_status         : 1; // 17
+        uint8_t low_battery         : 1; // 9    // Used to track if the battery is low - LED output flashes when the batt is low
+        uint8_t failsafe_radio      : 1; // 10   // A status flag for the radio failsafe
+        uint8_t failsafe_batt       : 1; // 11   // A status flag for the battery failsafe
+        uint8_t failsafe_gps        : 1; // 12   // A status flag for the gps failsafe
+        uint8_t failsafe_gcs        : 1; // 13   // A status flag for the ground station failsafe
+        uint8_t rc_override_active  : 1; // 14   // true if rc control are overwritten by ground station
+        uint8_t do_flip             : 1; // 15   // Used to enable flip code
+        uint8_t takeoff_complete    : 1; // 16
+        uint8_t land_complete       : 1; // 17
+        uint8_t compass_status      : 1; // 18
+        uint8_t gps_status          : 1; // 19
     };
-    uint16_t value;
+    uint32_t value;
 } ap;
 
 
@@ -1267,7 +1268,7 @@ static void slow_loop()
 }
 
 #define AUTO_DISARMING_DELAY 25
-// 1Hz loop
+// super_slow_loop - runs at 1Hz
 static void super_slow_loop()
 {
     if (g.log_bitmask != 0) {
diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde
index f862546df4..5112178ffe 100644
--- a/ArduCopter/motors.pde
+++ b/ArduCopter/motors.pde
@@ -11,7 +11,7 @@ static void arm_motors()
 {
     static int16_t arming_counter;
 
-    // don't allow arming/disarming in anything but manual
+    // ensure throttle is down
     if (g.rc_3.control_in > 0) {
         arming_counter = 0;
         return;
@@ -147,6 +147,9 @@ static void init_arm_motors()
     // enable gps velocity based centrefugal force compensation
     ahrs.set_correct_centrifugal(true);
 
+    // enable output to motors
+    output_min();
+
     // finally actually arm the motors
     motors.armed(true);
 
@@ -157,7 +160,7 @@ static void init_arm_motors()
     failsafe_enable();
 }
 
-// perform pre-arm checks and set 
+// perform pre-arm checks and set ap.pre_arm_check flag
 static void pre_arm_checks()
 {
     // exit immediately if we've already successfully performed the pre-arm check
@@ -165,8 +168,9 @@ static void pre_arm_checks()
         return;
     }
 
-    // check if radio has been calibrated
-    if(!g.rc_3.radio_min.load()) {
+    // pre-arm rc checks a prerequisite
+    pre_arm_rc_checks();
+    if(!ap.pre_arm_rc_check) {
         return;
     }
 
@@ -191,6 +195,28 @@ static void pre_arm_checks()
     ap.pre_arm_check = true;
 }
 
+// perform pre_arm_rc_checks checks and set ap.pre_arm_rc_check flag
+static void pre_arm_rc_checks()
+{
+    // exit immediately if we've already successfully performed the pre-arm rc check
+    if( ap.pre_arm_rc_check ) {
+        return;
+    }
+
+    // check if radio has been calibrated
+    if(!g.rc_3.radio_min.load()) {
+        return;
+    }
+
+    // check if throttle min is reasonable
+    if(g.rc_3.radio_min > 1300) {
+        return;
+    }
+
+    // if we've gotten this far rc is ok
+    ap.pre_arm_rc_check = true;
+}
+
 static void init_disarm_motors()
 {
 #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde
index 3db0611f2e..78cf3c94fc 100644
--- a/ArduCopter/radio.pde
+++ b/ArduCopter/radio.pde
@@ -61,6 +61,7 @@ static void init_rc_in()
 #endif
 }
 
+ // init_rc_out -- initialise motors and check if pilot wants to perform ESC calibration
 static void init_rc_out()
 {
     motors.set_update_rate(g.rc_speed);
@@ -77,18 +78,11 @@ static void init_rc_out()
     // we want the input to be scaled correctly
     g.rc_3.set_range_out(0,1000);
 
-    // sanity check - prevent unconfigured radios from outputting
-    if(g.rc_3.radio_min >= 1300) {
-        g.rc_3.radio_min = g.rc_3.radio_in;
-    }
-
-    // we are full throttle
+    // full throttle means to enter ESC calibration
     if(g.rc_3.control_in >= (MAXIMUM_THROTTLE - 50)) {
         if(g.esc_calibrate == 0) {
             // we will enter esc_calibrate mode on next reboot
             g.esc_calibrate.set_and_save(1);
-            // send minimum throttle out to ESC
-            motors.output_min();
             // display message on console
             cliSerial->printf_P(PSTR("Entering ESC Calibration: please restart APM.\n"));
             // block until we restart
@@ -107,8 +101,11 @@ static void init_rc_out()
         // did we abort the calibration?
         if(g.esc_calibrate == 1)
             g.esc_calibrate.set_and_save(0);
+    }
 
-        // send miinimum throttle out to ESC
+    // enable output to motors
+    pre_arm_rc_checks();
+    if (ap.pre_arm_rc_check) {
         output_min();
     }
 
@@ -119,6 +116,7 @@ static void init_rc_out()
 #endif
 }
 
+// output_min - enable and output lowest possible value to motors
 void output_min()
 {
     // enable motors
diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde
index d26454d30a..95143b7788 100644
--- a/ArduCopter/setup.pde
+++ b/ArduCopter/setup.pde
@@ -472,6 +472,13 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
         comp_type = AP_COMPASS_MOT_COMP_CURRENT;
     }
 
+    // check if radio is calibration
+    pre_arm_rc_checks();
+    if(!ap.pre_arm_rc_check) {
+        cliSerial->print_P(PSTR("radio not calibrated, exiting"));
+        return 0;
+    }
+
     // check compass is enabled
     if( !g.compass_enabled ) {
         cliSerial->print_P(PSTR("compass disabled, exiting"));
@@ -549,8 +556,8 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
 
     // enable motors and pass through throttle
     init_rc_out();
+    output_min();
     motors.armed(true);
-    motors.output_min();
 
     // initialise run time
     last_run_time = millis();
@@ -1370,6 +1377,8 @@ init_esc()
 {
     // reduce update rate to motors to 50Hz
     motors.set_update_rate(50);
+
+    // we enable the motors directly here instead of calling output_min because output_min would send a low signal to the ESC and disrupt the calibration process
     motors.enable();
     motors.armed(true);
     while(1) {
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index cb6f367f6e..d9e2280f94 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -184,7 +184,8 @@ static void init_ardupilot()
 #endif
 
     init_rc_in();               // sets up rc channels from radio
-    init_rc_out();              // sets up the timer libs
+    init_rc_out();              // sets up motors and output to escs
+
     /*
      *  setup the 'main loop is dead' check. Note that this relies on
      *  the RC library being initialised.
diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde
index 75ac025c35..60f47ce70c 100644
--- a/ArduCopter/test.pde
+++ b/ArduCopter/test.pde
@@ -320,6 +320,13 @@ test_tuning(uint8_t argc, const Menu::arg *argv)
 static int8_t
 test_battery(uint8_t argc, const Menu::arg *argv)
 {
+    // check if radio is calibration
+    pre_arm_rc_checks();
+    if(!ap.pre_arm_rc_check) {
+        cliSerial->print_P(PSTR("radio not calibrated, exiting"));
+        return(0);
+    }
+
     cliSerial->printf_P(PSTR("\nCareful! Motors will spin! Press Enter to start.\n"));
     while (cliSerial->read() != -1); /* flush */
     while(!cliSerial->available()) { /* wait for input */
@@ -329,7 +336,7 @@ test_battery(uint8_t argc, const Menu::arg *argv)
     print_hit_enter();
 
     // allow motors to spin
-    motors.enable();
+    output_min();
     motors.armed(true);
 
     while(1) {