diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index d354440271..20e10ccf7c 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -365,7 +365,8 @@ private: ap_t ap; - AirMode air_mode; // air mode is 0 = not-configured ; 1 = disabled; 2 = enabled + AirMode air_mode; // air mode is 0 = not-configured ; 1 = disabled; 2 = enabled; + bool force_flying; // force flying is enabled when true; static_assert(sizeof(uint32_t) == sizeof(ap), "ap_t must be uint32_t"); diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index e0f6bc46ff..4d5965b9ea 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -118,6 +118,7 @@ void RC_Channel_Copter::init_aux_function(const aux_func_t ch_option, const AuxS case AUX_FUNC::SURFACE_TRACKING: case AUX_FUNC::WINCH_ENABLE: case AUX_FUNC::AIRMODE: + case AUX_FUNC::FORCEFLYING: run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT); break; default: @@ -565,6 +566,10 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi #endif break; + case AUX_FUNC::FORCEFLYING: + do_aux_function_change_force_flying(ch_flag); + break; + case AUX_FUNC::AUTO_RTL: #if MODE_AUTO_ENABLED == ENABLED do_aux_function_change_mode(Mode::Number::AUTO_RTL, ch_flag); @@ -612,6 +617,21 @@ void RC_Channel_Copter::do_aux_function_change_air_mode(const AuxSwitchPos ch_fl } } +// change force flying status +void RC_Channel_Copter::do_aux_function_change_force_flying(const AuxSwitchPos ch_flag) +{ + switch (ch_flag) { + case AuxSwitchPos::HIGH: + copter.force_flying = true; + break; + case AuxSwitchPos::MIDDLE: + break; + case AuxSwitchPos::LOW: + copter.force_flying = false; + break; + } +} + // save_trim - adds roll and pitch trims from the radio to ahrs void Copter::save_trim() { diff --git a/ArduCopter/RC_Channel.h b/ArduCopter/RC_Channel.h index 38e7f2a443..576135f91f 100644 --- a/ArduCopter/RC_Channel.h +++ b/ArduCopter/RC_Channel.h @@ -19,6 +19,7 @@ private: void do_aux_function_change_mode(const Mode::Number mode, const AuxSwitchPos ch_flag); void do_aux_function_change_air_mode(const AuxSwitchPos ch_flag); + void do_aux_function_change_force_flying(const AuxSwitchPos ch_flag); // called when the mode switch changes position: void mode_switch_changed(modeswitch_pos_t new_pos) override; diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index d5872e0239..b30a75fb2b 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -35,6 +35,12 @@ void Copter::crash_check() return; } + // exit immediately if in force flying + if (force_flying && !flightmode->is_landing()) { + crash_counter = 0; + return; + } + // return immediately if we are not in an angle stabilize flight mode or we are flipping if (flightmode->mode_number() == Mode::Number::ACRO || flightmode->mode_number() == Mode::Number::FLIP) { crash_counter = 0; diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index a1c536409e..4a4a672031 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -196,7 +196,7 @@ void Copter::update_throttle_mix() // check if landing const bool landing = flightmode->is_landing(); - if ((large_angle_request && !landing) || large_angle_error || accel_moving || descent_not_demanded) { + if (((large_angle_request || force_flying) && !landing) || large_angle_error || accel_moving || descent_not_demanded) { attitude_control->set_throttle_mix_max(pos_control->get_vel_z_control_ratio()); } else { attitude_control->set_throttle_mix_min();