Copter: support for making use of heading error correction selectable

ArduCopter/mode_acro_heli.cpp

@@ -45,26 +45,29 @@ void ModeAcro_Heli::run()
     switch (motors->get_spool_state()) {
     case AP_Motors::SpoolState::SHUT_DOWN:
         // Motors Stopped
-        attitude_control->reset_target_and_rate(false);
+        attitude_control->reset_target_and_rate();
         attitude_control->reset_rate_controller_I_terms();
         break;
     case AP_Motors::SpoolState::GROUND_IDLE:
-        // If aircraft is landed, set target heading to current and reset the integrator
+        // IF aircraft is landed and not using leaky integrator 
+        // OR initializing targets on arming and using leaky integrator, 
+        // THEN set target heading to current and reset the integrator
         // Otherwise motors could be at ground idle for practice autorotation
         if ((motors->init_targets_on_arming() && motors->using_leaky_integrator()) || (copter.ap.land_complete && !motors->using_leaky_integrator())) {
             attitude_control->reset_target_and_rate(false);
             attitude_control->reset_rate_controller_I_terms_smoothly();
         }
         break;
+    case AP_Motors::SpoolState::SPOOLING_UP:
     case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
+    case AP_Motors::SpoolState::SPOOLING_DOWN:
         if (copter.ap.land_complete && !motors->using_leaky_integrator()) {
+            if (!motors->using_hdg_error_correction()) {
+                attitude_control->reset_yaw_target_and_rate(false);
+            }
             attitude_control->reset_rate_controller_I_terms_smoothly();
         }
         break;
-    case AP_Motors::SpoolState::SPOOLING_UP:
-    case AP_Motors::SpoolState::SPOOLING_DOWN:
-        // do nothing
-        break;
     }
 
     if (!motors->has_flybar()){

ArduCopter/mode_althold.cpp

@@ -50,15 +50,20 @@ void ModeAltHold::run()
 
     case AltHoldModeState::MotorStopped:
         attitude_control->reset_rate_controller_I_terms();
-        attitude_control->reset_yaw_target_and_rate(false);
+        attitude_control->reset_yaw_target_and_rate();
         pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
         break;
 
     case AltHoldModeState::Landed_Ground_Idle:
-        attitude_control->reset_yaw_target_and_rate();
+        attitude_control->reset_yaw_target_and_rate(false);
-        FALLTHROUGH;
+        attitude_control->reset_rate_controller_I_terms_smoothly();
+        pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
+        break;
 
     case AltHoldModeState::Landed_Pre_Takeoff:
+        if (!motors->using_hdg_error_correction()) {
+            attitude_control->reset_target_and_rate(false);
+        }
         attitude_control->reset_rate_controller_I_terms_smoothly();
         pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
         break;

ArduCopter/mode_drift.cpp

@@ -91,26 +91,38 @@ void ModeDrift::run()
     switch (motors->get_spool_state()) {
     case AP_Motors::SpoolState::SHUT_DOWN:
         // Motors Stopped
-        attitude_control->reset_yaw_target_and_rate(false);
+        attitude_control->reset_yaw_target_and_rate();
         attitude_control->reset_rate_controller_I_terms();
         break;
 
     case AP_Motors::SpoolState::GROUND_IDLE:
-        // Landed
+        // For helicopters, IF aircraft is landed and not using leaky integrator 
-        attitude_control->reset_yaw_target_and_rate();
+        // OR initializing targets on arming and using leaky integrator, 
-        attitude_control->reset_rate_controller_I_terms_smoothly();
+        // THEN set target heading to current and reset the integrator
-        break;
+        // Otherwise motors could be at ground idle for practice autorotation
-
+        // If a multirotor then it is always done
-    case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
+        if (!copter.is_tradheli() || (motors->init_targets_on_arming() && motors->using_leaky_integrator()) || (copter.ap.land_complete && !motors->using_leaky_integrator())) {
-        // clear landing flag above zero throttle
+            attitude_control->reset_yaw_target_and_rate(false);
-        if (!motors->limit.throttle_lower) {
+            attitude_control->reset_rate_controller_I_terms_smoothly();
-            set_land_complete(false);
         }
         break;
 
     case AP_Motors::SpoolState::SPOOLING_UP:
+    case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
     case AP_Motors::SpoolState::SPOOLING_DOWN:
-        // do nothing
+        // clear landing flag above zero throttle
+        if (!copter.is_tradheli()) {
+            if (!motors->limit.throttle_lower) {
+                set_land_complete(false);
+            }
+        } else {
+            if (copter.ap.land_complete && !motors->using_leaky_integrator()) {
+                if (!motors->using_hdg_error_correction()) {
+                    attitude_control->reset_yaw_target_and_rate(false);
+                }
+                attitude_control->reset_rate_controller_I_terms_smoothly();
+            }
+        }
         break;
     }
 

ArduCopter/mode_flowhold.cpp

@@ -288,10 +288,15 @@ void ModeFlowHold::run()
         break;
 
     case AltHoldModeState::Landed_Ground_Idle:
-        attitude_control->reset_yaw_target_and_rate();
+        attitude_control->reset_yaw_target_and_rate(false);
-        FALLTHROUGH;
+        attitude_control->reset_rate_controller_I_terms_smoothly();
+        pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
+        break;
 
     case AltHoldModeState::Landed_Pre_Takeoff:
+        if (!motors->using_hdg_error_correction()) {
+            attitude_control->reset_target_and_rate(false);
+        }
         attitude_control->reset_rate_controller_I_terms_smoothly();
         pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
         break;

ArduCopter/mode_loiter.cpp

@@ -132,10 +132,17 @@ void ModeLoiter::run()
         break;
 
     case AltHoldModeState::Landed_Ground_Idle:
-        attitude_control->reset_yaw_target_and_rate();
+        attitude_control->reset_yaw_target_and_rate(false);
-        FALLTHROUGH;
+        attitude_control->reset_rate_controller_I_terms_smoothly();
+        loiter_nav->init_target();
+        attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false);
+        pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
+        break;
 
     case AltHoldModeState::Landed_Pre_Takeoff:
+        if (!motors->using_hdg_error_correction()) {
+            attitude_control->reset_target_and_rate(false);
+        }
         attitude_control->reset_rate_controller_I_terms_smoothly();
         loiter_nav->init_target();
         attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false);

ArduCopter/mode_poshold.cpp

@@ -102,7 +102,7 @@ void ModePosHold::run()
 
     case AltHoldModeState::MotorStopped:
         attitude_control->reset_rate_controller_I_terms();
-        attitude_control->reset_yaw_target_and_rate(false);
+        attitude_control->reset_yaw_target_and_rate();
         pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
         loiter_nav->clear_pilot_desired_acceleration();
         loiter_nav->init_target();
@@ -116,13 +116,22 @@ void ModePosHold::run()
         break;
 
     case AltHoldModeState::Landed_Ground_Idle:
+        attitude_control->reset_yaw_target_and_rate(false);
+        attitude_control->reset_rate_controller_I_terms_smoothly();
+        pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
         loiter_nav->clear_pilot_desired_acceleration();
         loiter_nav->init_target();
-        attitude_control->reset_yaw_target_and_rate();
         init_wind_comp_estimate();
-        FALLTHROUGH;
+
+        // set poshold state to pilot override
+        roll_mode = RPMode::PILOT_OVERRIDE;
+        pitch_mode = RPMode::PILOT_OVERRIDE;
+        break;
 
     case AltHoldModeState::Landed_Pre_Takeoff:
+        if (!motors->using_hdg_error_correction()) {
+            attitude_control->reset_target_and_rate(false);
+        }
         attitude_control->reset_rate_controller_I_terms_smoothly();
         pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
 

ArduCopter/mode_sport.cpp

@@ -76,15 +76,20 @@ void ModeSport::run()
 
     case AltHoldModeState::MotorStopped:
         attitude_control->reset_rate_controller_I_terms();
-        attitude_control->reset_yaw_target_and_rate(false);
+        attitude_control->reset_yaw_target_and_rate();
         pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
         break;
 
     case AltHoldModeState::Landed_Ground_Idle:
-        attitude_control->reset_yaw_target_and_rate();
+        attitude_control->reset_yaw_target_and_rate(false);
-        FALLTHROUGH;
+        attitude_control->reset_rate_controller_I_terms_smoothly();
+        pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
+        break;
 
     case AltHoldModeState::Landed_Pre_Takeoff:
+        if (!motors->using_hdg_error_correction()) {
+            attitude_control->reset_target_and_rate(false);
+        }
         attitude_control->reset_rate_controller_I_terms_smoothly();
         pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
         break;

ArduCopter/mode_stabilize_heli.cpp

@@ -52,26 +52,29 @@ void ModeStabilize_Heli::run()
     switch (motors->get_spool_state()) {
     case AP_Motors::SpoolState::SHUT_DOWN:
         // Motors Stopped
-        attitude_control->reset_yaw_target_and_rate(false);
+        attitude_control->reset_yaw_target_and_rate();
         attitude_control->reset_rate_controller_I_terms();
         break;
     case AP_Motors::SpoolState::GROUND_IDLE:
-        // If aircraft is landed, set target heading to current and reset the integrator
+        // IF aircraft is landed and not using leaky integrator 
+        // OR initializing targets on arming and using leaky integrator, 
+        // THEN set target heading to current and reset the integrator
         // Otherwise motors could be at ground idle for practice autorotation
         if ((motors->init_targets_on_arming() && motors->using_leaky_integrator()) || (copter.ap.land_complete && !motors->using_leaky_integrator())) {
             attitude_control->reset_yaw_target_and_rate(false);
             attitude_control->reset_rate_controller_I_terms_smoothly();
         }
         break;
+    case AP_Motors::SpoolState::SPOOLING_UP:
     case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
+    case AP_Motors::SpoolState::SPOOLING_DOWN:
         if (copter.ap.land_complete && !motors->using_leaky_integrator()) {
+            if (!motors->using_hdg_error_correction()) {
+                attitude_control->reset_target_and_rate(false);
+            }
             attitude_control->reset_rate_controller_I_terms_smoothly();
         }
         break;
-    case AP_Motors::SpoolState::SPOOLING_UP:
-    case AP_Motors::SpoolState::SPOOLING_DOWN:
-        // do nothing
-        break;
     }
 
     // call attitude controller

ArduCopter/mode_zigzag.cpp

@@ -356,10 +356,17 @@ void ModeZigZag::manual_control()
         break;
 
     case AltHoldModeState::Landed_Ground_Idle:
-        attitude_control->reset_yaw_target_and_rate();
+        attitude_control->reset_yaw_target_and_rate(false);
-        FALLTHROUGH;
+        attitude_control->reset_rate_controller_I_terms_smoothly();
+        loiter_nav->init_target();
+        attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate);
+        pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
+        break;
 
     case AltHoldModeState::Landed_Pre_Takeoff:
+        if (!motors->using_hdg_error_correction()) {
+            attitude_control->reset_target_and_rate(false);
+        }
         attitude_control->reset_rate_controller_I_terms_smoothly();
         loiter_nav->init_target();
         attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate);