diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index c26ff9e54f..2d47452d68 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -395,6 +395,7 @@ private: float baro_climbrate; // barometer climbrate in cm/s LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests + // filtered pilot's throttle input used to cancel landing if throttle held high LowPassFilterFloat rc_throttle_control_in_filter; // 3D Location vectors diff --git a/ArduCopter/config.h b/ArduCopter/config.h index a1a65fef86..01043765d4 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -401,6 +401,9 @@ #ifndef LAND_WITH_DELAY_MS # define LAND_WITH_DELAY_MS 4000 // default delay (in milliseconds) when a land-with-delay is triggered during a failsafe event #endif +#ifndef LAND_CANCEL_TRIGGER_THR + # define LAND_CANCEL_TRIGGER_THR 700 // land is cancelled by input throttle above 700 +#endif ////////////////////////////////////////////////////////////////////////////// // Landing Detector diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index a462baff68..0f47659340 100644 --- a/ArduCopter/control_auto.cpp +++ b/ArduCopter/control_auto.cpp @@ -324,7 +324,8 @@ 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){ + if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ + Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); // exit land if throttle is high if (!set_mode(LOITER)) { set_mode(ALT_HOLD); diff --git a/ArduCopter/control_land.cpp b/ArduCopter/control_land.cpp index 8a372df29a..bb96326377 100644 --- a/ArduCopter/control_land.cpp +++ b/ArduCopter/control_land.cpp @@ -87,7 +87,8 @@ 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){ + if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ + Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); // exit land if throttle is high if (!set_mode(LOITER)) { set_mode(ALT_HOLD); @@ -155,7 +156,8 @@ 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){ + if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ + Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); // exit land if throttle is high if (!set_mode(LOITER)) { set_mode(ALT_HOLD); diff --git a/ArduCopter/control_rtl.cpp b/ArduCopter/control_rtl.cpp index 39b72584aa..7056afa259 100644 --- a/ArduCopter/control_rtl.cpp +++ b/ArduCopter/control_rtl.cpp @@ -267,7 +267,8 @@ 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){ + if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ + Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); // exit land if throttle is high if (!set_mode(LOITER)) { set_mode(ALT_HOLD); @@ -362,7 +363,8 @@ 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){ + if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ + Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); // exit land if throttle is high if (!set_mode(LOITER)) { set_mode(ALT_HOLD); diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 946a27c28d..84b0819151 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -333,6 +333,7 @@ enum FlipState { #define DATA_ROTOR_RUNUP_COMPLETE 58 // Heli only #define DATA_ROTOR_SPEED_BELOW_CRITICAL 59 // Heli only #define DATA_EKF_ALT_RESET 60 +#define DATA_LAND_CANCELLED_BY_PILOT 61 // Centi-degrees to radians #define DEGX100 5729.57795f