From a0df9fe15d12c26a13cc5c3f236c9f998e184114 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Sun, 17 Nov 2024 23:49:44 -0500 Subject: [PATCH] Copter: support for making use of heading error correction selectable --- ArduCopter/mode_acro_heli.cpp | 15 +++++++------ ArduCopter/mode_althold.cpp | 11 +++++++--- ArduCopter/mode_drift.cpp | 34 ++++++++++++++++++++---------- ArduCopter/mode_flowhold.cpp | 9 ++++++-- ArduCopter/mode_loiter.cpp | 11 ++++++++-- ArduCopter/mode_poshold.cpp | 15 ++++++++++--- ArduCopter/mode_sport.cpp | 11 +++++++--- ArduCopter/mode_stabilize_heli.cpp | 15 +++++++------ ArduCopter/mode_zigzag.cpp | 11 ++++++++-- 9 files changed, 94 insertions(+), 38 deletions(-) diff --git a/ArduCopter/mode_acro_heli.cpp b/ArduCopter/mode_acro_heli.cpp index 069de0c6c6..246f073805 100644 --- a/ArduCopter/mode_acro_heli.cpp +++ b/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()){ diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index 357d701f95..24e79e3bdc 100644 --- a/ArduCopter/mode_althold.cpp +++ b/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(); - FALLTHROUGH; + 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 + 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; diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index 985e3ba23e..d114f95df5 100644 --- a/ArduCopter/mode_drift.cpp +++ b/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 - attitude_control->reset_yaw_target_and_rate(); - attitude_control->reset_rate_controller_I_terms_smoothly(); - break; - - case AP_Motors::SpoolState::THROTTLE_UNLIMITED: - // clear landing flag above zero throttle - if (!motors->limit.throttle_lower) { - set_land_complete(false); + // For helicopters, 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 a multirotor then it is always done + if (!copter.is_tradheli() || (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: - // 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; } diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 467ef545cc..d1947b6326 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -288,10 +288,15 @@ void ModeFlowHold::run() break; case AltHoldModeState::Landed_Ground_Idle: - attitude_control->reset_yaw_target_and_rate(); - FALLTHROUGH; + 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 + 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; diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 752f4f6115..339d597c94 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -132,10 +132,17 @@ void ModeLoiter::run() break; case AltHoldModeState::Landed_Ground_Idle: - attitude_control->reset_yaw_target_and_rate(); - FALLTHROUGH; + attitude_control->reset_yaw_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); + 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); diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 5474aaa841..9fa7d43452 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/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 diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 249fa06ee5..ec4bbb8152 100644 --- a/ArduCopter/mode_sport.cpp +++ b/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(); - FALLTHROUGH; + 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 + 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; diff --git a/ArduCopter/mode_stabilize_heli.cpp b/ArduCopter/mode_stabilize_heli.cpp index 4260e4f9aa..26c9fa26d9 100644 --- a/ArduCopter/mode_stabilize_heli.cpp +++ b/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 diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 7c39fb7380..58023aa7a4 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/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(); - FALLTHROUGH; + attitude_control->reset_yaw_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); + 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);