diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index f4a4f46dc3631..90b8be5fd7010 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -124,6 +124,12 @@ class AP_MotorsHeli : public AP_Motors { // support passing init_targets_on_arming flag to greater code bool init_targets_on_arming() const override { return _heliflags.init_targets_on_arming; } + // set_in_autorotation - allows main code to set when aircraft is in autorotation. + void set_in_autorotation(bool autorotation) { _heliflags.in_autorotation = autorotation; } + + // set_enable_bailout - allows main code to set when RSC can immediately ramp engine instantly + void set_enable_bailout(bool bailout) { _heliflags.enable_bailout = bailout; } + // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; @@ -191,6 +197,8 @@ class AP_MotorsHeli : public AP_Motors { uint8_t inverted_flight : 1; // true for inverted flight uint8_t init_targets_on_arming : 1; // 0 if targets were initialized, 1 if targets were not initialized after arming uint8_t save_rsc_mode : 1; // used to determine the rsc mode needs to be saved while disarmed + 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 } _heliflags; // parameters diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index 8ee045708abe7..3e32df7294504 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -332,6 +332,9 @@ void AP_MotorsHeli_Dual::calculate_armed_scalars() _main_rotor._rsc_mode.save(); _heliflags.save_rsc_mode = false; } + + // set bailout ramp time + _main_rotor.use_bailout_ramp_time(_heliflags.enable_bailout); } // calculate_scalars diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp index c69c5a5f14cb2..3af95f153719f 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp @@ -120,6 +120,9 @@ void AP_MotorsHeli_Quad::calculate_armed_scalars() _main_rotor._rsc_mode.save(); _heliflags.save_rsc_mode = false; } + + // set bailout ramp time + _main_rotor.use_bailout_ramp_time(_heliflags.enable_bailout); } // calculate_scalars diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp index 82949e73dbda7..50e3d9bbb6ef0 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp @@ -294,9 +294,12 @@ void AP_MotorsHeli_RSC::output(RotorControlState state) // update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input, float dt) { - // sanity check ramp time - if (_ramp_time <= 0) { - _ramp_time = 1; + int8_t ramp_time; + // sanity check ramp time and enable bailout if set + if (_use_bailout_ramp || _ramp_time <= 0) { + ramp_time = 1; + } else { + ramp_time = _ramp_time; } // ramp output upwards towards target @@ -306,7 +309,7 @@ void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input, float dt) _rotor_ramp_output = _rotor_runup_output; } // ramp up slowly to target - _rotor_ramp_output += (dt / _ramp_time); + _rotor_ramp_output += (dt / ramp_time); if (_rotor_ramp_output > rotor_ramp_input) { _rotor_ramp_output = rotor_ramp_input; } diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index bb96595da9c8e..e3e8ec3dd6f2b 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -17,6 +17,7 @@ // default main rotor ramp up time in seconds #define AP_MOTORS_HELI_RSC_RAMP_TIME 1 // 1 second to ramp output to main rotor ESC to setpoint #define AP_MOTORS_HELI_RSC_RUNUP_TIME 10 // 10 seconds for rotor to reach full speed +#define AP_MOTORS_HELI_RSC_BAILOUT_TIME 1 // time in seconds to ramp motors when bailing out of autorotation // Throttle Curve Defaults #define AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT 25 @@ -116,6 +117,9 @@ class AP_MotorsHeli_RSC { // set_collective. collective for throttle curve calculation void set_collective(float collective) { _collective_in = collective; } + // use bailout ramp time + void use_bailout_ramp_time(bool enable) { _use_bailout_ramp = enable; } + // output - update value to send to ESC/Servo void output(RotorControlState state); @@ -149,6 +153,7 @@ class AP_MotorsHeli_RSC { float _rotor_rpm; // rotor rpm from speed sensor for governor float _governor_output; // governor output for rotor speed control bool _governor_engage; // RSC governor status flag for soft-start + bool _use_bailout_ramp; // true if allowing RSC to quickly ramp up engine // update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output void update_rotor_ramp(float rotor_ramp_input, float dt); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 110847f025079..677cba5501cab 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -283,6 +283,10 @@ void AP_MotorsHeli_Single::calculate_armed_scalars() _main_rotor._rsc_mode.save(); _heliflags.save_rsc_mode = false; } + + // set bailout ramp time + _main_rotor.use_bailout_ramp_time(_heliflags.enable_bailout); + _tail_rotor.use_bailout_ramp_time(_heliflags.enable_bailout); } // calculate_scalars - recalculates various scalers used. @@ -413,13 +417,13 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float } // ensure not below landed/landing collective - if (_heliflags.landing_collective && collective_out < _collective_mid_pct) { + if (_heliflags.landing_collective && collective_out < _collective_mid_pct && !_heliflags.in_autorotation) { collective_out = _collective_mid_pct; limit.throttle_lower = true; } - // if servo output not in manual mode, process pre-compensation factors - if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED) { + // 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 // the feed-forward is not required when the motor is stopped or at idle, and thus not creating torque // also not required if we are using external gyro diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 0d6bca2dc3008..0987817b9b565 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -176,3 +176,10 @@ void AP_Motors::add_motor_num(int8_t motor_num) } } } + +namespace AP { + AP_Motors *motors() + { + return AP_Motors::get_singleton(); + } +} \ No newline at end of file diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index d7d484fbffaa6..9e18d0a1faff8 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -260,3 +260,7 @@ class AP_Motors { private: static AP_Motors *_singleton; }; + +namespace AP { + AP_Motors *motors(); +}; \ No newline at end of file