From d2c37daa66225a589d3c845cf556ab5d97bcaf97 Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Fri, 29 Oct 2021 08:43:52 -0400 Subject: [PATCH] AP_Motors: change internal variable names and methods for mid collective to be more accurate --- libraries/AP_Motors/AP_MotorsHeli.cpp | 34 ++++++++++---------- libraries/AP_Motors/AP_MotorsHeli.h | 14 ++++---- libraries/AP_Motors/AP_MotorsHeli_Dual.cpp | 18 +++++------ libraries/AP_Motors/AP_MotorsHeli_Dual.h | 2 +- libraries/AP_Motors/AP_MotorsHeli_Quad.cpp | 16 ++++----- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 14 ++++---- 6 files changed, 49 insertions(+), 49 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index d92bb187d1..2c4d1477a6 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -110,36 +110,36 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = { // @User: Standard AP_GROUPINFO("OPTIONS", 28, AP_MotorsHeli, _heli_options, (uint8_t)HeliOption::USE_LEAKY_I), - // @Param: COL_MIN_DEG - // @DisplayName: Minimum Collective Blade Pitch Angle + // @Param: COL_ANG_MIN + // @DisplayName: Collective Blade Pitch Angle Minimum // @Description: Minimum collective blade pitch angle in deg that corresponds to the PWM set for minimum collective pitch (H_COL_MIN). // @Range: -20 0 // @Units: deg // @Increment: 0.1 // @User: Standard - AP_GROUPINFO("COL_MIN_DEG", 29, AP_MotorsHeli, _collective_min_deg, AP_MOTORS_HELI_COLLECTIVE_MIN_DEG), + AP_GROUPINFO("COL_ANG_MIN", 29, AP_MotorsHeli, _collective_min_deg, AP_MOTORS_HELI_COLLECTIVE_MIN_DEG), - // @Param: COL_MAX_DEG - // @DisplayName: Maximum Collective Blade Pitch Angle + // @Param: COL_ANG_MAX + // @DisplayName: Collective Blade Pitch Angle Maximum // @Description: Maximum collective blade pitch angle in deg that corresponds to the PWM set for maximum collective pitch (H_COL_MAX). // @Range: 5 20 // @Units: deg // @Increment: 0.1 // @User: Standard - AP_GROUPINFO("COL_MAX_DEG", 30, AP_MotorsHeli, _collective_max_deg, AP_MOTORS_HELI_COLLECTIVE_MAX_DEG), + AP_GROUPINFO("COL_ANG_MAX", 30, AP_MotorsHeli, _collective_max_deg, AP_MOTORS_HELI_COLLECTIVE_MAX_DEG), // @Param: COL_ZERO_THRST - // @DisplayName: Zero-Thrust Collective Pitch - // @Description: Zero thrust blade collective pitch in degrees. + // @DisplayName: Collective Blade Pitch at Zero Thrust + // @Description: Collective blade pitch angle at zero thrust in degrees. For symetric airfoil blades this value is zero deg. For chambered airfoil blades this value is typically negative. // @Range: -5 0 // @Units: deg // @Increment: 0.1 // @User: Standard - AP_GROUPINFO("COL_ZERO_THRST", 31, AP_MotorsHeli, _collective_zero_thrst_deg, 0.0f), + AP_GROUPINFO("COL_ZERO_THRST", 31, AP_MotorsHeli, _collective_zero_thrust_deg, 0.0f), // @Param: COL_LAND_MIN - // @DisplayName: Minimum Landed Collective Blade Pitch - // @Description: Minimum Landed collective blade pitch in degrees for non-manual collective modes (i.e. modes that use altitude hold). + // @DisplayName: Collective Blade Pitch Minimum when Landed + // @Description: Minimum collective blade pitch angle when landed in degrees for non-manual collective modes (i.e. modes that use altitude hold). // @Range: -5 0 // @Units: deg // @Increment: 0.1 @@ -282,7 +282,7 @@ void AP_MotorsHeli::output_disarmed() // fixate mid collective _roll_in = 0.0f; _pitch_in = 0.0f; - _throttle_filter.reset(_collective_zero_pct); + _throttle_filter.reset(_collective_zero_thrust_pct); _yaw_in = 0.0f; break; case SERVO_CONTROL_MODE_MANUAL_MAX: @@ -496,7 +496,7 @@ bool AP_MotorsHeli::parameter_check(bool display_msg) const // returns false if _collective_min_deg is not default value which indicates users set parameter if (is_equal((float)_collective_min_deg, (float)AP_MOTORS_HELI_COLLECTIVE_MIN_DEG)) { if (display_msg) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Set H_COL_MIN_DEG to actual min blade pitch in deg"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Set H_COL_ANG_MIN to measured min blade pitch in deg"); } return false; } @@ -504,7 +504,7 @@ bool AP_MotorsHeli::parameter_check(bool display_msg) const // returns false if _collective_max_deg is not default value which indicates users set parameter if (is_equal((float)_collective_max_deg, (float)AP_MOTORS_HELI_COLLECTIVE_MAX_DEG)) { if (display_msg) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Set H_COL_MAX_DEG to actual max blade pitch in deg"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Set H_COL_ANG_MAX to measured max blade pitch in deg"); } return false; } @@ -563,8 +563,8 @@ void AP_MotorsHeli::update_throttle_hover(float dt) // Don't let _collective_hover go below H_COLL_ZERO_THRST float curr_collective = get_throttle(); - if (curr_collective < _collective_zero_pct) { - curr_collective = _collective_zero_pct; + if (curr_collective < _collective_zero_thrust_pct) { + curr_collective = _collective_zero_thrust_pct; } // we have chosen to constrain the hover collective to be within the range reachable by the third order expo polynomial. @@ -584,7 +584,7 @@ void AP_MotorsHeli::save_params_on_disarm() // updates the takeoff collective flag void AP_MotorsHeli::update_takeoff_collective_flag(float coll_out) { - if (coll_out > _collective_zero_pct + 0.5f * (_collective_hover - _collective_zero_pct)) { + if (coll_out > _collective_zero_thrust_pct + 0.5f * (_collective_hover - _collective_zero_thrust_pct)) { _heliflags.takeoff_collective = true; } else { _heliflags.takeoff_collective = false; diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 38d00b7691..8c41e0be58 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -24,7 +24,7 @@ #define AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX 0.8f // maximum possible hover throttle #define AP_MOTORS_HELI_COLLECTIVE_MIN_DEG -90.0f // minimum collective blade pitch angle in deg #define AP_MOTORS_HELI_COLLECTIVE_MAX_DEG 90.0f // maximum collective blade pitch angle in deg -#define AP_MOTORS_HELI_COLLECTIVE_LAND_MIN -2.0f // minimum landed collective blade pitch angle in deg for modes using althold +#define AP_MOTORS_HELI_COLLECTIVE_LAND_MIN -1.0f // minimum landed collective blade pitch angle in deg for modes using althold // flybar types @@ -133,8 +133,8 @@ public: // accessor to get the takeoff collective flag signifying that current collective is greater than collective required to indicate takeoff bool get_takeoff_collective() const { return _heliflags.takeoff_collective; } - // accessor to get the takeoff collective flag signifying that current collective is greater than collective required to indicate takeoff - bool get_below_mid_collective() const { return _heliflags.below_mid_collective; } + // accessor to get the land min collective flag signifying that current collective is lower than collective required for landing + bool get_below_land_min_coll() const { return _heliflags.below_land_min_coll; } // support passing init_targets_on_arming flag to greater code bool init_targets_on_arming() const override { return _heliflags.init_targets_on_arming; } @@ -249,7 +249,7 @@ protected: uint8_t servo_test_running : 1; // true if servo_test is running uint8_t land_complete : 1; // true if aircraft is landed uint8_t takeoff_collective : 1; // true if collective is above 30% between H_COL_MID and H_COL_MAX - uint8_t below_mid_collective : 1; // true if collective is below H_COL_MID + uint8_t below_land_min_coll : 1; // true if collective is below H_COL_LAND_MIN } _heliflags; // parameters @@ -261,14 +261,14 @@ protected: AP_Float _collective_hover; // estimated collective required to hover throttle in the range 0 ~ 1 AP_Int8 _collective_hover_learn; // enable/disabled hover collective learning AP_Int8 _heli_options; // bitmask for optional features - AP_Float _collective_zero_thrst_deg; // Zero thrust blade collective pitch in degrees + AP_Float _collective_zero_thrust_deg;// Zero thrust blade collective pitch in degrees AP_Float _collective_land_min_deg; // Minimum Landed collective blade pitch in degrees for non-manual collective modes (i.e. modes that use altitude hold) AP_Float _collective_max_deg; // Maximum collective blade pitch angle in deg that corresponds to the PWM set for maximum collective pitch (H_COL_MAX) AP_Float _collective_min_deg; // Minimum collective blade pitch angle in deg that corresponds to the PWM set for minimum collective pitch (H_COL_MIN) // internal variables - float _collective_zero_pct; // collective mid parameter value converted to 0 ~ 1 range - float _collective_land_min_pct; // collective mid parameter value converted to 0 ~ 1 range + float _collective_zero_thrust_pct; // collective zero thrutst parameter value converted to 0 ~ 1 range + float _collective_land_min_pct; // collective land min parameter value converted to 0 ~ 1 range uint8_t _servo_test_cycle_counter = 0; // number of test cycles left to run after bootup motor_frame_type _frame_type; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index c3ec056455..6e9defdea2 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -375,22 +375,22 @@ void AP_MotorsHeli_Dual::calculate_scalars() _collective2_max = AP_MOTORS_HELI_DUAL_COLLECTIVE2_MAX; } - _collective_zero_thrst_deg = constrain_float(_collective_zero_thrst_deg, _collective_min_deg, _collective_max_deg); + _collective_zero_thrust_deg = constrain_float(_collective_zero_thrust_deg, _collective_min_deg, _collective_max_deg); _collective_land_min_deg = constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg); if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) { // calculate collective zero thrust point as a number from 0 to 1 - _collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); + _collective_zero_thrust_pct = (_collective_zero_thrust_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); // calculate collective land min point as a number from 0 to 1 _collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); } else { - _collective_zero_pct = 0.0f; + _collective_zero_thrust_pct = 0.0f; _collective_land_min_pct = 0.0f; } - _collective2_zero_pct = _collective_zero_pct; + _collective2_zero_thrst_pct = _collective_zero_thrust_pct; // configure swashplate 1 and update scalars _swashplate1.configure(); @@ -576,11 +576,11 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c limit.throttle_lower = true; } - // updates below mid collective flag + // updates below land min collective flag if (collective_out <= _collective_land_min_pct) { - _heliflags.below_mid_collective = true; + _heliflags.below_land_min_coll = true; } else { - _heliflags.below_mid_collective = false; + _heliflags.below_land_min_coll = false; } // updates takeoff collective flag based on 50% hover collective @@ -589,7 +589,7 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c // Set rear collective to midpoint if required float collective2_out = collective_out; if (_servo_mode == SERVO_CONTROL_MODE_MANUAL_CENTER) { - collective2_out = _collective2_zero_pct; + collective2_out = _collective2_zero_thrst_pct; } // if servo output not in manual mode, process pre-compensation factors @@ -601,7 +601,7 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c // for intermeshing, reverse yaw in negative collective region and smoothen transition near zero collective if (_yaw_rev_expo > 0.01f) { // yaw_compensation range: (-1,1) S-shaped curve (Logistic Model) 1/(1 + e^kt) - yaw_compensation = 1.0f - (2.0f / (1.0f + powf(2.7182818f , _yaw_rev_expo * (collective_out-_collective_zero_pct)))); + yaw_compensation = 1.0f - (2.0f / (1.0f + powf(2.7182818f , _yaw_rev_expo * (collective_out-_collective_zero_thrust_pct)))); yaw_out = yaw_out * yaw_compensation; } } else { diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.h b/libraries/AP_Motors/AP_MotorsHeli_Dual.h index aac99bcab9..23147d43d0 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.h @@ -137,5 +137,5 @@ protected: AP_Float _yaw_rev_expo; // yaw reverser smoothing exponent, for intermeshing mode only. // internal variables - float _collective2_zero_pct; + float _collective2_zero_thrst_pct; }; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp index 55da1c237c..babb71b514 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp @@ -147,18 +147,18 @@ void AP_MotorsHeli_Quad::calculate_scalars() _collective_max = AP_MOTORS_HELI_COLLECTIVE_MAX; } - _collective_zero_thrst_deg = constrain_float(_collective_zero_thrst_deg, _collective_min_deg, _collective_max_deg); + _collective_zero_thrust_deg = constrain_float(_collective_zero_thrust_deg, _collective_min_deg, _collective_max_deg); _collective_land_min_deg = constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg); if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) { // calculate collective zero thrust point as a number from 0 to 1 - _collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); + _collective_zero_thrust_pct = (_collective_zero_thrust_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); // calculate collective land min point as a number from 0 to 1 _collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); } else { - _collective_zero_pct = 0.0f; + _collective_zero_thrust_pct = 0.0f; _collective_land_min_pct = 0.0f; } @@ -253,11 +253,11 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c limit.throttle_lower = true; } - // updates below mid collective flag + // updates below land min collective flag if (collective_out <= _collective_land_min_pct) { - _heliflags.below_mid_collective = true; + _heliflags.below_land_min_coll = true; } else { - _heliflags.below_mid_collective = false; + _heliflags.below_land_min_coll = false; } // updates takeoff collective flag based on 50% hover collective @@ -273,7 +273,7 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c _main_rotor.set_collective(fabsf(collective_out)); // rescale collective for overhead calc - collective_out -= _collective_zero_pct; + collective_out -= _collective_zero_thrust_pct; // reserve some collective for attitude control collective_out *= collective_range; @@ -314,7 +314,7 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c for (uint8_t i=0; i