diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 506e4af320..7c07c35a96 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -152,6 +152,15 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = { // @Path: ../AC_Autorotation/RSC_Autorotation.cpp AP_SUBGROUPINFO(_main_rotor.autorotation, "RSC_AROT_", 33, AP_MotorsHeli, RSC_Autorotation), + // @Param: COL_LAND_OFSET + // @DisplayName: Offset of Collective Blade Pitch when Landed + // @Description: Offset of the collective blade pitch angle when landed in degrees for non-manual collective modes (i.e. modes that use altitude hold). + // @Range: -2 2 + // @Units: deg + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("COL_LAND_OFSET", 34, AP_MotorsHeli, _collective_land_offset_deg, 0.0), + AP_GROUPEND }; @@ -349,7 +358,7 @@ void AP_MotorsHeli::output_logic() case SpoolState::GROUND_IDLE: { // Motors should be stationary or at ground idle. // set limits flags - if (_heliflags.land_complete && !using_leaky_integrator()) { + if ((_heliflags.land_complete || _heliflags.land_complete_maybe) && !using_leaky_integrator()) { set_limit_flag_pitch_roll_yaw(true); } else { set_limit_flag_pitch_roll_yaw(false); @@ -371,7 +380,7 @@ void AP_MotorsHeli::output_logic() // Servos should exhibit normal flight behavior. // set limits flags - if (_heliflags.land_complete && !using_leaky_integrator()) { + if ((_heliflags.land_complete || _heliflags.land_complete_maybe) && !using_leaky_integrator()) { set_limit_flag_pitch_roll_yaw(true); } else { set_limit_flag_pitch_roll_yaw(false); @@ -393,7 +402,7 @@ void AP_MotorsHeli::output_logic() // Servos should exhibit normal flight behavior. // set limits flags - if (_heliflags.land_complete && !using_leaky_integrator()) { + if ((_heliflags.land_complete || _heliflags.land_complete_maybe) && !using_leaky_integrator()) { set_limit_flag_pitch_roll_yaw(true); } else { set_limit_flag_pitch_roll_yaw(false); @@ -413,7 +422,7 @@ void AP_MotorsHeli::output_logic() // Servos should exhibit normal flight behavior. // set limits flags - if (_heliflags.land_complete && !using_leaky_integrator()) { + if ((_heliflags.land_complete || _heliflags.land_complete_maybe) && !using_leaky_integrator()) { set_limit_flag_pitch_roll_yaw(true); } else { set_limit_flag_pitch_roll_yaw(false); diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index e606d8df15..fe5bc58a0a 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -25,6 +25,7 @@ #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_FILTER_FREQ 2.0f // time constant used to update estimated hover throttle, 0 ~ 1 // flybar types @@ -132,7 +133,10 @@ public: // set land complete flag void set_land_complete(bool landed) { _heliflags.land_complete = landed; } - + + // set land complete maybe flag + void set_land_complete_maybe(bool landed) { _heliflags.land_complete_maybe = landed; } + //return zero lift collective position float get_coll_mid() const { return _collective_zero_thrust_pct; } @@ -254,6 +258,7 @@ protected: uint8_t below_land_min_coll : 1; // true if collective is below H_COL_LAND_MIN uint8_t rotor_spooldown_complete : 1; // true if the rotors have spooled down completely uint8_t start_engine : 1; // true if turbine start RC option is initiated + uint8_t land_complete_maybe : 1; // true if aircraft is landed_maybe } _heliflags; // parameters @@ -269,11 +274,14 @@ protected: 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) + AP_Float _collective_land_offset_deg;// Offset of Landed collective blade pitch in degrees for non-manual collective modes (i.e. modes that use altitude hold) // internal variables 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 + float _collective_land_offset_pct; // collective land offset parameter value converted to fraction of collective range uint8_t _servo_test_cycle_counter = 0; // number of test cycles left to run after bootup + float _collective_offset_landed; // current value of collective offset motor_frame_type _frame_type; motor_frame_class _frame_class; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index d5f8b72b0b..1498ce8b3f 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -269,6 +269,7 @@ void AP_MotorsHeli_Dual::calculate_scalars() _collective_zero_thrust_deg.set(constrain_float(_collective_zero_thrust_deg, _collective_min_deg, _collective_max_deg)); _collective_land_min_deg.set(constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg)); + _collective_land_offset_deg.set(constrain_float(_collective_land_offset_deg, 0.0, _collective_zero_thrust_deg-_collective_land_min_deg)); if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) { // calculate collective zero thrust point as a number from 0 to 1 @@ -276,6 +277,7 @@ void AP_MotorsHeli_Dual::calculate_scalars() // 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); + _collective_land_offset_pct = _collective_land_offset_deg/(_collective_max_deg-_collective_min_deg); } else { _collective_zero_thrust_pct = 0.0f; _collective_land_min_pct = 0.0f; @@ -420,14 +422,27 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c limit.throttle_upper = true; } - // ensure not below landed/landing collective - if (_heliflags.landing_collective && collective_out < _collective_land_min_pct) { + // In non-manual collective modes, ensure not below landed/landing collective. If collective offset is used, don't allow negative + // collective offsets. Only allow positive collective offsets to keep collective above landed collective. + if (_heliflags.land_complete && _heliflags.landing_collective && collective_out <= _collective_land_min_pct + _collective_offset_landed) { + float alpha = _dt / (_dt + 1.0/(2.0 * M_PI * AP_MOTORS_HELI_COLLECTIVE_FILTER_FREQ)); //calculate LP filter alpha for 2 hz cutoff freq + _collective_offset_landed += alpha * (_collective_land_offset_pct - _collective_offset_landed); + // constrain collective_out so it never goes above collective zero thrust + collective_out = constrain_float((_collective_land_min_pct + _collective_offset_landed), _collective_land_min_pct, _collective_zero_thrust_pct); + limit.throttle_lower = true; + } else if (_heliflags.landing_collective && collective_out < _collective_land_min_pct && !_main_rotor.in_autorotation()) { collective_out = _collective_land_min_pct; limit.throttle_lower = true; + } else { + _collective_offset_landed = 0.0; } // updates below land min collective flag - if (collective_out <= _collective_land_min_pct) { + float collective_land_min = _collective_land_min_pct; + if (_heliflags.land_complete && _heliflags.landing_collective) { + collective_land_min += _collective_offset_landed; + } + if (collective_out <= collective_land_min) { _heliflags.below_land_min_coll = true; } else { _heliflags.below_land_min_coll = false; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 6372363e37..94f4d9ee47 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -284,6 +284,7 @@ void AP_MotorsHeli_Single::calculate_scalars() _collective_zero_thrust_deg.set(constrain_float(_collective_zero_thrust_deg, _collective_min_deg, _collective_max_deg)); _collective_land_min_deg.set(constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg)); + _collective_land_offset_deg.set(constrain_float(_collective_land_offset_deg, 0.0, _collective_zero_thrust_deg-_collective_land_min_deg)); if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) { // calculate collective zero thrust point as a number from 0 to 1 @@ -291,6 +292,7 @@ void AP_MotorsHeli_Single::calculate_scalars() // 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); + _collective_land_offset_pct = _collective_land_offset_deg/(_collective_max_deg-_collective_min_deg); } else { _collective_zero_thrust_pct = 0.0f; _collective_land_min_pct = 0.0f; @@ -387,14 +389,27 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float limit.throttle_upper = true; } - // ensure not below landed/landing collective - if (_heliflags.landing_collective && collective_out < _collective_land_min_pct && !_main_rotor.in_autorotation()) { + // In non-manual collective modes, ensure not below landed/landing collective. If collective offset is used, don't allow negative + // collective offsets. Only allow positive collective offsets to keep collective above landed collective. + if (_heliflags.land_complete && _heliflags.landing_collective && collective_out <= _collective_land_min_pct + _collective_offset_landed) { + float alpha = _dt / (_dt + 1.0/(2.0 * M_PI * AP_MOTORS_HELI_COLLECTIVE_FILTER_FREQ)); //calculate LP filter alpha for 2 hz cutoff freq + _collective_offset_landed += alpha * (_collective_land_offset_pct - _collective_offset_landed); + // constrain collective_out so it never goes above collective zero thrust + collective_out = constrain_float((_collective_land_min_pct + _collective_offset_landed), _collective_land_min_pct, _collective_zero_thrust_pct); + limit.throttle_lower = true; + } else if (_heliflags.landing_collective && collective_out < _collective_land_min_pct && !_main_rotor.in_autorotation()) { collective_out = _collective_land_min_pct; limit.throttle_lower = true; + } else { + _collective_offset_landed = 0.0; } // updates below land min collective flag - if (collective_out <= _collective_land_min_pct) { + float collective_land_min = _collective_land_min_pct; + if (_heliflags.land_complete && _heliflags.landing_collective) { + collective_land_min += _collective_offset_landed; + } + if (collective_out <= collective_land_min) { _heliflags.below_land_min_coll = true; } else { _heliflags.below_land_min_coll = false;