diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 5fb143bf3b..ccc972d68b 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -97,8 +97,8 @@ const AP_Param::Info Copter::var_info[] = { // @DisplayName: Throttle stick behavior // @Description: Bitmask containing various throttle stick options. Add up the values for options that you want. // @User: Standard - // @Values: 0:None,1:Feedback from mid stick,2:High throttle cancels landing - // @Bitmask: 0:Feedback from mid stick,1:High throttle cancels landing + // @Values: 0:None,1:Feedback from mid stick,2:High throttle cancels landing,4:Disarm on land detection + // @Bitmask: 0:Feedback from mid stick,1:High throttle cancels landing,2:Disarm on land detection GSCALAR(throttle_behavior, "PILOT_THR_BHV", 0), // @Group: SERIAL diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 2d460b0a2a..421220fa79 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -461,3 +461,4 @@ enum ThrowModeState { // for PILOT_THR_BHV parameter #define THR_BEHAVE_FEEDBACK_FROM_MID_STICK (1<<0) #define THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND (1<<1) +#define THR_BEHAVE_DISARM_ON_LAND_DETECT (1<<2) diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 8f7dd2461f..5164e1b026 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -93,6 +93,13 @@ void Copter::set_land_complete(bool b) Log_Write_Event(DATA_NOT_LANDED); } ap.land_complete = b; + + bool disarm_on_land_configured = (g.throttle_behavior & THR_BEHAVE_DISARM_ON_LAND_DETECT) != 0; + bool mode_disarms_on_land = mode_allows_arming(control_mode,false) && !mode_has_manual_throttle(control_mode); + + if (ap.land_complete && motors.armed() && disarm_on_land_configured && mode_disarms_on_land) { + init_disarm_motors(); + } } // set land complete maybe flag