diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 22d6939a27..b97b65ba5a 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -702,6 +702,7 @@ void AC_AttitudeControl::accel_limiting(bool enable_limits) // provide 0 to cut motors void AC_AttitudeControl::set_throttle_out(float throttle_out, bool apply_angle_boost, float filter_cutoff) { + _throttle_in_filt.apply(throttle_out, _dt); _motors.set_stabilizing(true); _motors.set_throttle_filter_cutoff(filter_cutoff); if (apply_angle_boost) { @@ -716,6 +717,7 @@ void AC_AttitudeControl::set_throttle_out(float throttle_out, bool apply_angle_b // outputs a throttle to all motors evenly with no attitude stabilization void AC_AttitudeControl::set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filter_cutoff) { + _throttle_in_filt.apply(throttle_in, _dt); if (reset_attitude_control) { relax_bf_rate_controller(); set_yaw_target_to_current_heading(); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index de06b021b6..fa72cc004f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -41,6 +41,8 @@ #define AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT 1 // body-frame rate feedforward enabled by default +#define AC_ATTITUDE_CONTROL_ALTHOLD_LEANANGLE_FILT_HZ 1.0f // filter (in hz) of throttle filter used to limit lean angle so that vehicle does not lose altitude + class AC_AttitudeControl { public: AC_AttitudeControl( AP_AHRS &ahrs, @@ -60,7 +62,8 @@ public: _pid_rate_yaw(pid_rate_yaw), _dt(AC_ATTITUDE_100HZ_DT), _angle_boost(0), - _acro_angle_switch(0) + _acro_angle_switch(0), + _throttle_in_filt(AC_ATTITUDE_CONTROL_ALTHOLD_LEANANGLE_FILT_HZ) { AP_Param::setup_object_defaults(this, var_info); @@ -211,6 +214,9 @@ public: // angle_boost - accessor for angle boost so it can be logged int16_t angle_boost() const { return _angle_boost; } + // get lean angle max for pilot input that prioritises altitude hold over lean angle + virtual float get_althold_lean_angle_max() const = 0; + // // helper functions // @@ -293,6 +299,9 @@ protected: Vector3f _rate_bf_desired; // body-frame feed forward rates int16_t _angle_boost; // used only for logging int16_t _acro_angle_switch; // used only for logging + + // throttle based angle limits + LowPassFilterFloat _throttle_in_filt; // throttle input from pilot or alt hold controller }; #define AC_ATTITUDE_CONTROL_LOG_FORMAT(msg) { msg, sizeof(AC_AttitudeControl::log_Attitude), \ diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 3552005c83..a7d8ac4919 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -94,6 +94,13 @@ void AC_AttitudeControl_Heli::rate_controller_run() } } +// get lean angle max for pilot input that prioritises altitude hold over lean angle +float AC_AttitudeControl_Heli::get_althold_lean_angle_max() const +{ + // calc maximum tilt angle based on throttle + return ToDeg(acos(constrain_float(_throttle_in_filt.get()/900.0f, 0.0f, 1000.0f) / 1000.0f)) * 100.0f; +} + // // private methods // diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index 5c739efd4c..92052dfab7 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -43,6 +43,9 @@ public: // should be called at 100hz or more virtual void rate_controller_run(); + // get lean angle max for pilot input that prioritises altitude hold over lean angle + float get_althold_lean_angle_max() const; + // use_leaky_i - controls whether we use leaky i term for body-frame to motor output stage void use_leaky_i(bool leaky_i) { _flags_heli.leaky_i = leaky_i; } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 223daa9cac..993df984a9 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -4,6 +4,14 @@ #include #include +// get lean angle max for pilot input that prioritises altitude hold over lean angle +float AC_AttitudeControl_Multi::get_althold_lean_angle_max() const +{ + // calc maximum tilt angle based on throttle + float thr_max = _motors_multi.throttle_max(); + return ToDeg(acos(constrain_float(_throttle_in_filt.get()/(0.9f * thr_max / 1000.0f), 0.0f, 1000.0f) / 1000.0f)) * 100.0f; +} + // returns a throttle including compensation for roll/pitch angle // throttle value should be 0 ~ 1000 float AC_AttitudeControl_Multi::get_boosted_throttle(float throttle_in) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index a78aaa48c0..dca7ed3bb3 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -24,6 +24,9 @@ public: // empty destructor to suppress compiler warning virtual ~AC_AttitudeControl_Multi() {} + // get lean angle max for pilot input that prioritises altitude hold over lean angle + float get_althold_lean_angle_max() const; + // calculate total body frame throttle required to produce the given earth frame throttle float get_boosted_throttle(float throttle_in);