Copter: add "high throttle cancels landing" option

ArduCopter/Copter.cpp

@@ -70,6 +70,7 @@ Copter::Copter(void) :
     baro_alt(0),
     baro_climbrate(0.0f),
     land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),
+    rc_throttle_control_in_filter(1.0f),
     auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP),
     yaw_look_at_WP_bearing(0.0f),
     yaw_look_at_heading(0),

ArduCopter/Copter.h

@@ -395,6 +395,8 @@ private:
     float baro_climbrate;        // barometer climbrate in cm/s
     LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests
 
+    LowPassFilterFloat rc_throttle_control_in_filter;
+
     // 3D Location vectors
     // Current location of the copter (altitude is relative to home)
     struct Location current_loc;

ArduCopter/Parameters.cpp

@@ -93,9 +93,9 @@ const AP_Param::Info Copter::var_info[] = {
 
     // @Param: PILOT_THR_BHV
     // @DisplayName: Throttle stick behavior
-    // @Description: Bits for: Feedback starts from mid stick
+    // @Description: Bitmask containing various throttle stick options. Add up the values for options that you want.
     // @User: Standard
-    // @Values: 0:None,1:FeedbackFromMid
+    // @Values: 0:None,1:Feedback from mid stick,2:High throttle cancels landing
     GSCALAR(throttle_behavior, "PILOT_THR_BHV", 0),
 
     // @Group: SERIAL

ArduCopter/control_auto.cpp

@@ -324,6 +324,13 @@ void Copter::auto_land_run()
 
     // process pilot's input
     if (!failsafe.radio) {
+        if((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > 0.7f*THR_MAX){
+            // exit land if throttle is high
+            if (!set_mode(LOITER)) {
+                set_mode(ALT_HOLD);
+            }
+        }
+
         if (g.land_repositioning) {
             // apply SIMPLE mode transform to pilot inputs
             update_simple_mode();

ArduCopter/control_land.cpp

@@ -87,6 +87,13 @@ void Copter::land_gps_run()
 
     // process pilot inputs
     if (!failsafe.radio) {
+        if((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > 0.7f*THR_MAX){
+            // exit land if throttle is high
+            if (!set_mode(LOITER)) {
+                set_mode(ALT_HOLD);
+            }
+        }
+
         if (g.land_repositioning) {
             // apply SIMPLE mode transform to pilot inputs
             update_simple_mode();
@@ -148,6 +155,13 @@ void Copter::land_nogps_run()
 
     // process pilot inputs
     if (!failsafe.radio) {
+        if((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > 0.7f*THR_MAX){
+            // exit land if throttle is high
+            if (!set_mode(LOITER)) {
+                set_mode(ALT_HOLD);
+            }
+        }
+
         if (g.land_repositioning) {
             // apply SIMPLE mode transform to pilot inputs
             update_simple_mode();

ArduCopter/control_rtl.cpp

@@ -267,6 +267,13 @@ void Copter::rtl_descent_run()
 
     // process pilot's input
     if (!failsafe.radio) {
+        if((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > 0.7f*THR_MAX){
+            // exit land if throttle is high
+            if (!set_mode(LOITER)) {
+                set_mode(ALT_HOLD);
+            }
+        }
+
         if (g.land_repositioning) {
             // apply SIMPLE mode transform to pilot inputs
             update_simple_mode();
@@ -355,6 +362,13 @@ void Copter::rtl_land_run()
 
     // process pilot's input
     if (!failsafe.radio) {
+        if((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > 0.7f*THR_MAX){
+            // exit land if throttle is high
+            if (!set_mode(LOITER)) {
+                set_mode(ALT_HOLD);
+            }
+        }
+
         if (g.land_repositioning) {
             // apply SIMPLE mode transform to pilot inputs
             update_simple_mode();

ArduCopter/defines.h

@@ -423,5 +423,6 @@ enum FlipState {
 
 // for PILOT_THR_BHV parameter
 #define THR_BEHAVE_FEEDBACK_FROM_MID_STICK (1<<0)
+#define THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND (1<<1)
 
 #endif // _DEFINES_H

ArduCopter/radio.cpp

@@ -101,7 +101,6 @@ void Copter::read_radio()
     uint32_t tnow_ms = millis();
 
     if (hal.rcin->new_input()) {
-        last_update_ms = tnow_ms;
         ap.new_radio_frame = true;
         RC_Channel::set_pwm_all();
 
@@ -115,6 +114,10 @@ void Copter::read_radio()
 
         // update output on any aux channels, for manual passthru
         RC_Channel_aux::output_ch_all();
+
+        float dt = (tnow_ms - last_update_ms)*1.0e-3f;
+        rc_throttle_control_in_filter.apply(g.rc_3.control_in, dt);
+        last_update_ms = tnow_ms;
     }else{
         uint32_t elapsed = tnow_ms - last_update_ms;
         // turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE