From 84fb3afbcb519e9892d90b71291542b75c55b202 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Sun, 15 Nov 2020 18:32:31 -0500 Subject: [PATCH] AP_Motors: Tradheli support for integrator management and hover collective learning --- libraries/AP_Motors/AP_MotorsHeli.cpp | 87 ++++++++++++++++++++ libraries/AP_Motors/AP_MotorsHeli.h | 27 +++++- libraries/AP_Motors/AP_MotorsHeli_Dual.cpp | 9 +- libraries/AP_Motors/AP_MotorsHeli_Quad.cpp | 9 +- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 9 +- 5 files changed, 131 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 783ba83460..f9dd9f2485 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -96,6 +96,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 }; @@ -301,6 +315,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; @@ -311,6 +330,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; @@ -326,6 +356,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; @@ -341,6 +382,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; @@ -354,6 +406,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; @@ -456,3 +519,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 e281416d87..79285365dc 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -527,9 +527,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; @@ -576,6 +573,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 d8be36c1f1..c86b004d84 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp @@ -223,9 +223,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; @@ -246,6 +243,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 2275dc0699..70c3679ffb 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -408,9 +408,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; @@ -449,6 +446,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