From 160c992548afecac9ff5c14cc19093fef8eeca47 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Sun, 20 Dec 2020 16:40:58 -0500 Subject: [PATCH] Copter: fix heli land detector and incoporate reset_I_smoothly --- ArduCopter/land_detector.cpp | 4 ++-- ArduCopter/mode.cpp | 2 +- ArduCopter/mode_acro.cpp | 2 +- ArduCopter/mode_acro_heli.cpp | 4 ++-- ArduCopter/mode_althold.cpp | 2 +- ArduCopter/mode_autotune.cpp | 2 +- ArduCopter/mode_drift.cpp | 2 +- ArduCopter/mode_flowhold.cpp | 2 +- ArduCopter/mode_loiter.cpp | 2 +- ArduCopter/mode_poshold.cpp | 2 +- ArduCopter/mode_sport.cpp | 2 +- ArduCopter/mode_stabilize.cpp | 2 +- ArduCopter/mode_stabilize_heli.cpp | 4 ++-- ArduCopter/mode_systemid.cpp | 2 +- ArduCopter/mode_zigzag.cpp | 2 +- 15 files changed, 18 insertions(+), 18 deletions(-) diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 4196fa4892..2a9111e1b2 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -60,8 +60,8 @@ void Copter::update_land_detector() } else { #if FRAME_CONFIG == HELI_FRAME - // check that collective pitch is on lower limit (should be constrained by LAND_COL_MIN) - bool motor_at_lower_limit = motors->limit.throttle_lower; + // check that collective pitch is below mid collective (zero thrust) position + bool motor_at_lower_limit = (motors->get_below_mid_collective() && fabsf(ahrs.get_roll()) < M_PI/2.0f); #else // check that the average throttle output is near minimum (less than 12.5% hover throttle) bool motor_at_lower_limit = motors->limit.throttle_lower && attitude_control->is_throttle_mix_min(); diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index a2a3da4b9c..840f15eb65 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -485,7 +485,7 @@ void Mode::make_safe_spool_down() case AP_Motors::SpoolState::SHUT_DOWN: case AP_Motors::SpoolState::GROUND_IDLE: // relax controllers during idle states - attitude_control->reset_rate_controller_I_terms(); + attitude_control->reset_rate_controller_I_terms_smoothly(); attitude_control->set_yaw_target_to_current_heading(); break; diff --git a/ArduCopter/mode_acro.cpp b/ArduCopter/mode_acro.cpp index 0ec88ebb76..b51c5c19e8 100644 --- a/ArduCopter/mode_acro.cpp +++ b/ArduCopter/mode_acro.cpp @@ -34,7 +34,7 @@ void ModeAcro::run() case AP_Motors::SpoolState::GROUND_IDLE: // Landed attitude_control->set_attitude_target_to_current_attitude(); - attitude_control->reset_rate_controller_I_terms(); + attitude_control->reset_rate_controller_I_terms_smoothly(); break; case AP_Motors::SpoolState::THROTTLE_UNLIMITED: diff --git a/ArduCopter/mode_acro_heli.cpp b/ArduCopter/mode_acro_heli.cpp index e5d2f24ffe..3b7b4f858f 100644 --- a/ArduCopter/mode_acro_heli.cpp +++ b/ArduCopter/mode_acro_heli.cpp @@ -53,12 +53,12 @@ void ModeAcro_Heli::run() // 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->set_attitude_target_to_current_attitude(); - attitude_control->reset_rate_controller_I_terms(); + attitude_control->reset_rate_controller_I_terms_smoothly(); } break; case AP_Motors::SpoolState::THROTTLE_UNLIMITED: if (copter.ap.land_complete && !motors->using_leaky_integrator()) { - attitude_control->reset_rate_controller_I_terms(); + attitude_control->reset_rate_controller_I_terms_smoothly(); } break; case AP_Motors::SpoolState::SPOOLING_UP: diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index 548a918c41..9ba943c462 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -58,7 +58,7 @@ void ModeAltHold::run() // FALLTHROUGH case AltHold_Landed_Pre_Takeoff: - attitude_control->reset_rate_controller_I_terms(); + attitude_control->reset_rate_controller_I_terms_smoothly(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero break; diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index 4b3323ff86..935a993d64 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -60,7 +60,7 @@ void AutoTune::run() } else { copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); } - copter.attitude_control->reset_rate_controller_I_terms(); + copter.attitude_control->reset_rate_controller_I_terms_smoothly(); copter.attitude_control->set_yaw_target_to_current_heading(); float target_roll, target_pitch, target_yaw_rate; diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index d7f542b889..ee9aed334f 100644 --- a/ArduCopter/mode_drift.cpp +++ b/ArduCopter/mode_drift.cpp @@ -98,7 +98,7 @@ void ModeDrift::run() case AP_Motors::SpoolState::GROUND_IDLE: // Landed attitude_control->set_yaw_target_to_current_heading(); - attitude_control->reset_rate_controller_I_terms(); + attitude_control->reset_rate_controller_I_terms_smoothly(); break; case AP_Motors::SpoolState::THROTTLE_UNLIMITED: diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index ab92a8242b..8aafb79125 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -286,7 +286,7 @@ void ModeFlowHold::run() // FALLTHROUGH case AltHold_Landed_Pre_Takeoff: - attitude_control->reset_rate_controller_I_terms(); + attitude_control->reset_rate_controller_I_terms_smoothly(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero break; diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 8597624c14..6e85b97172 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -154,7 +154,7 @@ void ModeLoiter::run() // FALLTHROUGH case AltHold_Landed_Pre_Takeoff: - attitude_control->reset_rate_controller_I_terms(); + attitude_control->reset_rate_controller_I_terms_smoothly(); loiter_nav->init_target(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index cc4e435e91..0e3d461d7c 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -153,7 +153,7 @@ void ModePosHold::run() // FALLTHROUGH case AltHold_Landed_Pre_Takeoff: - attitude_control->reset_rate_controller_I_terms(); + attitude_control->reset_rate_controller_I_terms_smoothly(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero // set poshold state to pilot override diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 0b0fa1f51d..99a73f4da2 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -106,7 +106,7 @@ void ModeSport::run() // FALLTHROUGH case AltHold_Landed_Pre_Takeoff: - attitude_control->reset_rate_controller_I_terms(); + attitude_control->reset_rate_controller_I_terms_smoothly(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero break; diff --git a/ArduCopter/mode_stabilize.cpp b/ArduCopter/mode_stabilize.cpp index 93edffc2c3..85d522ed5f 100644 --- a/ArduCopter/mode_stabilize.cpp +++ b/ArduCopter/mode_stabilize.cpp @@ -38,7 +38,7 @@ void ModeStabilize::run() case AP_Motors::SpoolState::GROUND_IDLE: // Landed attitude_control->set_yaw_target_to_current_heading(); - attitude_control->reset_rate_controller_I_terms(); + attitude_control->reset_rate_controller_I_terms_smoothly(); break; case AP_Motors::SpoolState::THROTTLE_UNLIMITED: diff --git a/ArduCopter/mode_stabilize_heli.cpp b/ArduCopter/mode_stabilize_heli.cpp index a216f9fb6d..3a3f908a10 100644 --- a/ArduCopter/mode_stabilize_heli.cpp +++ b/ArduCopter/mode_stabilize_heli.cpp @@ -58,12 +58,12 @@ void ModeStabilize_Heli::run() // 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->set_yaw_target_to_current_heading(); - attitude_control->reset_rate_controller_I_terms(); + attitude_control->reset_rate_controller_I_terms_smoothly(); } break; case AP_Motors::SpoolState::THROTTLE_UNLIMITED: if (copter.ap.land_complete && !motors->using_leaky_integrator()) { - attitude_control->reset_rate_controller_I_terms(); + attitude_control->reset_rate_controller_I_terms_smoothly(); } case AP_Motors::SpoolState::SPOOLING_UP: case AP_Motors::SpoolState::SPOOLING_DOWN: diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index 5b9734c04a..e3ea17f024 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -141,7 +141,7 @@ void ModeSystemId::run() // init_targets_on_arming is always set true for multicopter. if (motors->init_targets_on_arming()) { attitude_control->set_yaw_target_to_current_heading(); - attitude_control->reset_rate_controller_I_terms(); + attitude_control->reset_rate_controller_I_terms_smoothly(); } break; diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index c58ed7a326..740d53c316 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -253,7 +253,7 @@ void ModeZigZag::manual_control() FALLTHROUGH; case AltHold_Landed_Pre_Takeoff: - attitude_control->reset_rate_controller_I_terms(); + attitude_control->reset_rate_controller_I_terms_smoothly(); loiter_nav->init_target(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero