From 626927f21e9a0818dea53d9c30abe967bfc30f16 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Tue, 19 Nov 2024 22:32:45 -0500 Subject: [PATCH 1/2] AP_Motors: support for making use of heading correction on ground selectable change heli rotors turning on ground yaw behavior Make use of heading correction on ground selectable for tradheli --- libraries/AP_Motors/AP_MotorsHeli.cpp | 4 ++-- libraries/AP_Motors/AP_MotorsHeli.h | 8 ++++++-- libraries/AP_Motors/AP_Motors_Class.h | 4 ++++ 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 506e4af32073b..a3115816d0561 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -108,9 +108,9 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = { // @Param: OPTIONS // @DisplayName: Heli_Options // @Description: Bitmask of heli options. Bit 0 changes how the pitch, roll, and yaw axis integrator term is managed for low speed and takeoff/landing. In AC 4.0 and earlier, scheme uses a leaky integrator for ground speeds less than 5 m/s and won't let the steady state integrator build above ILMI. The integrator is allowed to build to the ILMI value when it is landed. The other integrator management scheme bases integrator limiting on takeoff and landing. Whenever the aircraft is landed the integrator is set to zero. When the aicraft is airborne, the integrator is only limited by IMAX. - // @Bitmask: 0:Use Leaky I + // @Bitmask: 0:Use Leaky I, 1:Heading Error Correction On-Ground // @User: Standard - AP_GROUPINFO("OPTIONS", 28, AP_MotorsHeli, _heli_options, (uint8_t)HeliOption::USE_LEAKY_I), + AP_GROUPINFO("OPTIONS", 28, AP_MotorsHeli, _heli_options, (uint8_t)HeliOption::USE_LEAKY_I + (uint8_t)HeliOption::USE_HDG_CORRECTION), // @Param: COL_ANG_MIN // @DisplayName: Collective Blade Pitch Angle Minimum diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index e606d8df15b2e..431f10805abc7 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -138,11 +138,15 @@ class AP_MotorsHeli : public AP_Motors { // enum for heli optional features enum class HeliOption { - USE_LEAKY_I = (1<<0), // 1 + USE_LEAKY_I = (1<<0), + USE_HDG_CORRECTION = (1<<1), }; // use leaking integrator management scheme - bool using_leaky_integrator() const { return heli_option(HeliOption::USE_LEAKY_I); } + bool using_leaky_integrator() const override { return heli_option(HeliOption::USE_LEAKY_I); } + + // use heading error correction + bool using_hdg_error_correction() const override { return heli_option(HeliOption::USE_HDG_CORRECTION); } // Run arming checks bool arming_checks(size_t buflen, char *buffer) const override; diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 76e4928cb3f66..906b3d6fc74bb 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -258,6 +258,10 @@ class AP_Motors { // This function required for tradheli. Tradheli initializes targets when going from unarmed to armed state. // This function is overriden in motors_heli class. Always true for multicopters. virtual bool init_targets_on_arming() const { return true; } + // use leaking integrator management scheme is always false for multirotors + virtual bool using_leaky_integrator() const { return false; } + // use heading error correction which is always true for multirotors + virtual bool using_hdg_error_correction() const { return true; } // returns true if the configured PWM type is digital and should have fixed endpoints bool is_digital_pwm_type() const; From a0df9fe15d12c26a13cc5c3f236c9f998e184114 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Sun, 17 Nov 2024 23:49:44 -0500 Subject: [PATCH 2/2] 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 069de0c6c6a1d..246f073805834 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 357d701f9593f..24e79e3bdccab 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 985e3ba23e717..d114f95df58cc 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 467ef545cca21..d1947b6326543 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 752f4f6115739..339d597c94757 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 5474aaa84151f..9fa7d434520fc 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 249fa06ee507d..ec4bbb8152fdd 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 4260e4f9aa6af..26c9fa26d956e 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 7c39fb7380793..58023aa7a4c4c 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);