diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index acdb74dd9c..090c8aa2ed 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -696,6 +696,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) { + _motors.set_stabilizing(true); if (apply_angle_boost) { _motors.set_throttle(get_angle_boost(throttle_out)); }else{ @@ -705,6 +706,18 @@ 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) +{ + if (reset_attitude_control) { + relax_bf_rate_controller(); + set_yaw_target_to_current_heading(); + } + _motors.set_stabilizing(false); + _motors.set_throttle(throttle_in); + _angle_boost = 0; +} + // get_angle_boost - returns a throttle including compensation for roll/pitch angle // throttle value should be 0 ~ 1000 float AC_AttitudeControl::get_angle_boost(float throttle_pwm) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 71f2ae945e..36aa64211a 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -179,9 +179,11 @@ public: // // set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors - // provide 0 to cut motors void set_throttle_out(float throttle_pwm, bool apply_angle_boost); + // outputs a throttle to all motors evenly with no stabilization + void set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control); + // angle_boost - accessor for angle boost so it can be logged int16_t angle_boost() const { return _angle_boost; }