From a393bd26d79350b5f894f4ed7c4db8d2052af870 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Tue, 27 Sep 2016 13:33:56 -0700
Subject: [PATCH] Copter: add stick gesture to begin compass calibration

---
 ArduCopter/config.h    |  7 +++++++
 ArduCopter/sensors.cpp | 19 +++++++++++++++++++
 2 files changed, 26 insertions(+)

diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 10d3b7f578..5c4f362eac 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -259,6 +259,13 @@
  #endif
 #endif
 
+#ifndef COMPASS_CAL_STICK_GESTURE_TIME
+ #define COMPASS_CAL_STICK_GESTURE_TIME 2.0f // 2 seconds
+#endif
+#ifndef COMPASS_CAL_STICK_DELAY
+ #define COMPASS_CAL_STICK_DELAY 5.0f
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 //  OPTICAL_FLOW
 #ifndef OPTFLOW
diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp
index c33939f580..c4a005ee29 100644
--- a/ArduCopter/sensors.cpp
+++ b/ArduCopter/sensors.cpp
@@ -188,15 +188,34 @@ void Copter::read_receiver_rssi(void)
 
 void Copter::compass_cal_update()
 {
+    static uint32_t compass_cal_stick_gesture_begin = 0;
+
     if (!hal.util->get_soft_armed()) {
         compass.compass_cal_update();
     }
+
 #ifdef CAL_ALWAYS_REBOOT
     if (compass.compass_cal_requires_reboot()) {
         hal.scheduler->delay(1000);
         hal.scheduler->reboot(false);
+        return;
     }
 #endif
+
+    if (compass.is_calibrating()) {
+        if (channel_yaw->get_control_in() < -4000 && channel_throttle->get_control_in() > 900) {
+            compass.cancel_calibration_all();
+        }
+    } else {
+        bool stick_gesture_detected = compass_cal_stick_gesture_begin != 0 && !motors.armed() && channel_yaw->get_control_in() > 4000 && channel_throttle->get_control_in() > 900;
+        uint32_t tnow = millis();
+
+        if (!stick_gesture_detected) {
+            compass_cal_stick_gesture_begin = tnow;
+        } else if (tnow-compass_cal_stick_gesture_begin > 1000*COMPASS_CAL_STICK_GESTURE_TIME) {
+            compass.start_calibration_all(true,true,COMPASS_CAL_STICK_DELAY,false);
+        }
+    }
 }
 
 void Copter::accel_cal_update()