diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 8353c64e57..8c39fac055 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -101,6 +101,20 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = { // @Path: AP_MotorsHeli_RSC.cpp AP_SUBGROUPINFO(_main_rotor, "RSC_", 25, AP_MotorsHeli, AP_MotorsHeli_RSC), + // @Param: COLL_HOVER + // @DisplayName: Collective Hover Value + // @Description: Collective needed to hover expressed as a number from 0 to 1 where 0 is H_COL_MIN and 1 is H_COL_MAX + // @Range: 0.3 0.8 + // @User: Advanced + AP_GROUPINFO("COLL_HOVER", 26, AP_MotorsHeli, _collective_hover, AP_MOTORS_HELI_COLLECTIVE_HOVER_DEFAULT), + + // @Param: HOVER_LEARN + // @DisplayName: Hover Value Learning + // @Description: Enable/Disable automatic learning of hover collective + // @Values: 0:Disabled, 1:Learn, 2:Learn and Save + // @User: Advanced + AP_GROUPINFO("HOVER_LEARN", 27, AP_MotorsHeli, _collective_hover_learn, HOVER_LEARN_AND_SAVE), + AP_GROUPEND }; @@ -307,6 +321,11 @@ void AP_MotorsHeli::output_logic() // Motors should be stationary. // Servos set to their trim values or in a test condition. + // set limits flags + limit.roll = true; + limit.pitch = true; + limit.yaw = true; + // make sure the motors are spooling in the correct direction if (_spool_desired != DesiredSpoolState::SHUT_DOWN) { _spool_state = SpoolState::GROUND_IDLE; @@ -317,6 +336,17 @@ void AP_MotorsHeli::output_logic() case SpoolState::GROUND_IDLE: { // Motors should be stationary or at ground idle. + // set limits flags + if (_heliflags.land_complete) { + limit.roll = true; + limit.pitch = true; + limit.yaw = true; + } else { + limit.roll = false; + limit.pitch = false; + limit.yaw = false; + } + // Servos should be moving to correct the current attitude. if (_spool_desired == DesiredSpoolState::SHUT_DOWN){ _spool_state = SpoolState::SHUT_DOWN; @@ -332,6 +362,17 @@ void AP_MotorsHeli::output_logic() // Maximum throttle should move from minimum to maximum. // Servos should exhibit normal flight behavior. + // set limits flags + if (_heliflags.land_complete) { + limit.roll = true; + limit.pitch = true; + limit.yaw = true; + } else { + limit.roll = false; + limit.pitch = false; + limit.yaw = false; + } + // make sure the motors are spooling in the correct direction if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED ){ _spool_state = SpoolState::SPOOLING_DOWN; @@ -347,6 +388,17 @@ void AP_MotorsHeli::output_logic() // Throttle should exhibit normal flight behavior. // Servos should exhibit normal flight behavior. + // set limits flags + if (_heliflags.land_complete) { + limit.roll = true; + limit.pitch = true; + limit.yaw = true; + } else { + limit.roll = false; + limit.pitch = false; + limit.yaw = false; + } + // make sure the motors are spooling in the correct direction if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED) { _spool_state = SpoolState::SPOOLING_DOWN; @@ -360,6 +412,17 @@ void AP_MotorsHeli::output_logic() // Maximum throttle should move from maximum to minimum. // Servos should exhibit normal flight behavior. + // set limits flags + if (_heliflags.land_complete) { + limit.roll = true; + limit.pitch = true; + limit.yaw = true; + } else { + limit.roll = false; + limit.pitch = false; + limit.yaw = false; + } + // make sure the motors are spooling in the correct direction if (_spool_desired == DesiredSpoolState::THROTTLE_UNLIMITED) { _spool_state = SpoolState::SPOOLING_UP; @@ -462,3 +525,27 @@ void AP_MotorsHeli::rc_write_swash(uint8_t chan, float swash_in) SRV_Channels::set_output_pwm_trimmed(function, pwm); } +// update the collective input filter. should be called at 100hz +void AP_MotorsHeli::update_throttle_hover(float dt) +{ + if (_collective_hover_learn != HOVER_LEARN_DISABLED) { + + // Don't let _collective_hover go below H_COLL_MID + float curr_collective = get_throttle(); + if (curr_collective < _collective_mid_pct) { + curr_collective = _collective_mid_pct; + } + + // we have chosen to constrain the hover throttle to be within the range reachable by the third order expo polynomial. + _collective_hover = constrain_float(_collective_hover + (dt / (dt + AP_MOTORS_HELI_COLLECTIVE_HOVER_TC)) * (curr_collective - _collective_hover), AP_MOTORS_HELI_COLLECTIVE_HOVER_MIN, AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX); + } +} + +// save parameters as part of disarming +void AP_MotorsHeli::save_params_on_disarm() +{ + // save hover throttle + if (_collective_hover_learn == HOVER_LEARN_AND_SAVE) { + _collective_hover.save(); + } +} diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index cdaa465547..ea1f69e1bd 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -19,6 +19,10 @@ #define AP_MOTORS_HELI_COLLECTIVE_MIN 1250 #define AP_MOTORS_HELI_COLLECTIVE_MAX 1750 #define AP_MOTORS_HELI_COLLECTIVE_MID 1500 +#define AP_MOTORS_HELI_COLLECTIVE_HOVER_DEFAULT 0.5f // the estimated hover throttle, 0 ~ 1 +#define AP_MOTORS_HELI_COLLECTIVE_HOVER_TC 10.0f // time constant used to update estimated hover throttle, 0 ~ 1 +#define AP_MOTORS_HELI_COLLECTIVE_HOVER_MIN 0.3f // minimum possible hover throttle +#define AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX 0.8f // maximum possible hover throttle // flybar types #define AP_MOTORS_HELI_NOFLYBAR 0 @@ -119,7 +123,11 @@ public: // supports_yaw_passthrough virtual bool supports_yaw_passthrough() const { return false; } - float get_throttle_hover() const override { return 0.5f; } + // update estimated throttle required to hover + void update_throttle_hover(float dt); + float get_throttle_hover() const override { return _collective_hover; } + + bool get_takeoff_collective() const { return _heliflags.takeoff_collective; } // support passing init_targets_on_arming flag to greater code bool init_targets_on_arming() const override { return _heliflags.init_targets_on_arming; } @@ -132,6 +140,9 @@ public: // return true if the servo test is still running/pending bool servo_test_running() const { return _heliflags.servo_test_running; } + + // set land complete flag + void set_land_complete(bool landed) { _heliflags.land_complete = landed; } // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; @@ -193,6 +204,16 @@ protected: // write to a swash servo. output value is pwm void rc_write_swash(uint8_t chan, float swash_in); + // save parameters as part of disarming + void save_params_on_disarm() override; + + // enum values for HOVER_LEARN parameter + enum HoverLearn { + HOVER_LEARN_DISABLED = 0, + HOVER_LEARN_ONLY = 1, + HOVER_LEARN_AND_SAVE = 2 + }; + // flags bitmask struct heliflags_type { uint8_t landing_collective : 1; // true if collective is setup for landing which has much higher minimum @@ -203,6 +224,8 @@ protected: uint8_t in_autorotation : 1; // true if aircraft is in autorotation uint8_t enable_bailout : 1; // true if allowing RSC to quickly ramp up engine 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 } _heliflags; // parameters @@ -212,6 +235,8 @@ protected: AP_Int16 _collective_mid; // Swash servo position corresponding to zero collective pitch (or zero lift for Asymmetrical blades) AP_Int8 _servo_mode; // Pass radio inputs directly to servos during set-up through mission planner AP_Int8 _servo_test; // sets number of cycles to test servo movement on bootup + 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 // internal variables float _collective_mid_pct = 0.0f; // collective mid parameter value converted to 0 ~ 1 range diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index 8729953d51..afbf494e62 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -483,9 +483,6 @@ void AP_MotorsHeli_Dual::update_motor_control(RotorControlState state) void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float collective_in, float yaw_out) { // initialize limits flag - limit.roll = false; - limit.pitch = false; - limit.yaw = false; limit.throttle_lower = false; limit.throttle_upper = false; @@ -555,6 +552,12 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c limit.throttle_lower = true; } + if (collective_out > _collective_mid_pct + 0.5f * (_collective_hover - _collective_mid_pct)) { + _heliflags.takeoff_collective = true; + } else { + _heliflags.takeoff_collective = false; + } + // Set rear collective to midpoint if required float collective2_out = collective_out; if (_servo_mode == SERVO_CONTROL_MODE_MANUAL_CENTER) { diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp index ff23e6dc9d..d71f899887 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp @@ -210,9 +210,6 @@ void AP_MotorsHeli_Quad::update_motor_control(RotorControlState state) void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float collective_in, float yaw_out) { // initialize limits flag - limit.roll = false; - limit.pitch = false; - limit.yaw = false; limit.throttle_lower = false; limit.throttle_upper = false; @@ -233,6 +230,12 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c limit.throttle_lower = true; } + if (collective_out > _collective_mid_pct + 0.5f * (_collective_hover - _collective_mid_pct)) { + _heliflags.takeoff_collective = true; + } else { + _heliflags.takeoff_collective = false; + } + float collective_range = (_collective_max - _collective_min) * 0.001f; if (_heliflags.inverted_flight) { diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 0b344f395d..f9f63d1080 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -381,9 +381,6 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float float yaw_offset = 0.0f; // initialize limits flag - limit.roll = false; - limit.pitch = false; - limit.yaw = false; limit.throttle_lower = false; limit.throttle_upper = false; @@ -422,6 +419,12 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float limit.throttle_lower = true; } + if (collective_out > _collective_mid_pct + 0.5f * (_collective_hover - _collective_mid_pct)) { + _heliflags.takeoff_collective = true; + } else { + _heliflags.takeoff_collective = false; + } + // if servo output not in manual mode and heli is not in autorotation, process pre-compensation factors if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED && !_heliflags.in_autorotation) { // rudder feed forward based on collective