mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-23 08:13:56 -04:00
AC_AttControl: get_boosted_throttle purely virtual
This commit is contained in:
parent
4843be49de
commit
93dd7dd970
@ -725,23 +725,6 @@ void AC_AttitudeControl::set_throttle_out_unstabilized(float throttle_in, bool r
|
|||||||
_angle_boost = 0;
|
_angle_boost = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// returns a throttle including compensation for roll/pitch angle
|
|
||||||
// throttle value should be 0 ~ 1000
|
|
||||||
float AC_AttitudeControl::get_boosted_throttle(float throttle_in)
|
|
||||||
{
|
|
||||||
// inverted_factor is 1 for tilt angles below 60 degrees
|
|
||||||
// reduces as a function of angle beyond 60 degrees
|
|
||||||
// becomes zero at 90 degrees
|
|
||||||
float min_throttle = _motors.throttle_min();
|
|
||||||
float cos_tilt = _ahrs.cos_pitch() * _ahrs.cos_roll();
|
|
||||||
float inverted_factor = constrain_float(2.0f*cos_tilt, 0.0f, 1.0f);
|
|
||||||
float boost_factor = 1.0f/constrain_float(cos_tilt, 0.5f, 1.0f);
|
|
||||||
|
|
||||||
float throttle_out = (throttle_in-min_throttle)*inverted_factor*boost_factor + min_throttle;
|
|
||||||
_angle_boost = constrain_float(throttle_out - throttle_in,-32000,32000);
|
|
||||||
return throttle_out;
|
|
||||||
}
|
|
||||||
|
|
||||||
// sqrt_controller - response based on the sqrt of the error instead of the more common linear response
|
// sqrt_controller - response based on the sqrt of the error instead of the more common linear response
|
||||||
float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord_lim)
|
float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord_lim)
|
||||||
{
|
{
|
||||||
|
@ -263,7 +263,7 @@ protected:
|
|||||||
//
|
//
|
||||||
|
|
||||||
// calculate total body frame throttle required to produce the given earth frame throttle
|
// calculate total body frame throttle required to produce the given earth frame throttle
|
||||||
virtual float get_boosted_throttle(float throttle_in);
|
virtual float get_boosted_throttle(float throttle_in) = 0;
|
||||||
|
|
||||||
// references to external libraries
|
// references to external libraries
|
||||||
const AP_AHRS& _ahrs;
|
const AP_AHRS& _ahrs;
|
||||||
|
Loading…
Reference in New Issue
Block a user