Copter: Add support for Force Flying

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");
 

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()
 {

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;

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;

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();