mirror of https://github.com/ArduPilot/ardupilot
AC_AttitudeControl: rework angle_boost
This commit is contained in:
parent
7abd02baf2
commit
518e798f53
|
@ -698,7 +698,7 @@ void AC_AttitudeControl::set_throttle_out(float throttle_out, bool apply_angle_b
|
|||
{
|
||||
_motors.set_stabilizing(true);
|
||||
if (apply_angle_boost) {
|
||||
_motors.set_throttle(get_angle_boost(throttle_out));
|
||||
_motors.set_throttle(get_boosted_throttle(throttle_out));
|
||||
}else{
|
||||
_motors.set_throttle(throttle_out);
|
||||
// clear angle_boost for logging purposes
|
||||
|
@ -718,25 +718,20 @@ void AC_AttitudeControl::set_throttle_out_unstabilized(float throttle_in, bool r
|
|||
_angle_boost = 0;
|
||||
}
|
||||
|
||||
// get_angle_boost - returns a throttle including compensation for roll/pitch angle
|
||||
// returns a throttle including compensation for roll/pitch angle
|
||||
// throttle value should be 0 ~ 1000
|
||||
float AC_AttitudeControl::get_angle_boost(float throttle_pwm)
|
||||
float AC_AttitudeControl::get_boosted_throttle(float throttle_in)
|
||||
{
|
||||
float temp = _ahrs.cos_pitch() * _ahrs.cos_roll();
|
||||
float throttle_out;
|
||||
|
||||
temp = constrain_float(temp, 0.5f, 1.0f);
|
||||
|
||||
// reduce throttle if we go inverted
|
||||
temp = constrain_float(9000-max(labs(_ahrs.roll_sensor),labs(_ahrs.pitch_sensor)), 0, 3000) / (3000 * temp);
|
||||
|
||||
// apply scale and constrain throttle
|
||||
// To-Do: move throttle_min and throttle_max into the AP_Vehicles class?
|
||||
throttle_out = constrain_float((float)(throttle_pwm-_motors.throttle_min()) * temp + _motors.throttle_min(), _motors.throttle_min(), 1000);
|
||||
|
||||
// record angle boost for logging
|
||||
_angle_boost = throttle_out - throttle_pwm;
|
||||
// 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 = throttle_out - throttle_in;
|
||||
return throttle_out;
|
||||
}
|
||||
|
||||
|
|
|
@ -238,8 +238,8 @@ protected:
|
|||
// throttle methods
|
||||
//
|
||||
|
||||
// get_angle_boost - calculate total body frame throttle required to produce the given earth frame throttle
|
||||
virtual float get_angle_boost(float throttle_pwm);
|
||||
// calculate total body frame throttle required to produce the given earth frame throttle
|
||||
virtual float get_boosted_throttle(float throttle_in);
|
||||
|
||||
// references to external libraries
|
||||
const AP_AHRS& _ahrs;
|
||||
|
|
|
@ -361,13 +361,13 @@ float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_cds)
|
|||
// throttle functions
|
||||
//
|
||||
|
||||
// get_angle_boost - returns a throttle including compensation for roll/pitch angle
|
||||
// returns a throttle including compensation for roll/pitch angle
|
||||
// throttle value should be 0 ~ 1000
|
||||
int16_t AC_AttitudeControl_Heli::get_angle_boost(int16_t throttle_pwm)
|
||||
float AC_AttitudeControl_Heli::get_boosted_throttle(float throttle_in)
|
||||
{
|
||||
// no angle boost for trad helis
|
||||
_angle_boost = 0;
|
||||
return throttle_pwm;
|
||||
return throttle_in;
|
||||
}
|
||||
|
||||
// update_feedforward_filter_rate - Sets LPF cutoff frequency
|
||||
|
|
|
@ -73,8 +73,8 @@ private:
|
|||
// throttle methods
|
||||
//
|
||||
|
||||
// get_angle_boost - calculate total body frame throttle required to produce the given earth frame throttle
|
||||
virtual int16_t get_angle_boost(int16_t throttle_pwm);
|
||||
// calculate total body frame throttle required to produce the given earth frame throttle
|
||||
float get_boosted_throttle(float throttle_in);
|
||||
|
||||
|
||||
// LPF filters to act on Rate Feedforward terms to linearize output.
|
||||
|
|
Loading…
Reference in New Issue