Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APM_Control: Convert references to AP_Airspeed.

This commit is contained in:
AndersonRayner 2016-05-29 01:08:04 +10:00 committed by Tom Pittenger
parent da1b18d918
commit cc5a2417a6
6 changed files with 14 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/APM_Control/AP_PitchController.cpp
View File

@ -152,7 +152,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
}
float ki_rate = k_I * gains.tau;
//only integrate if gain and time step are positive and airspeed above min value.
if (dt > 0 && aspeed > 0.5f*float(aparm.airspeed_min)) {
if (dt > 0 && aspeed > 0.5f*airspeed.get_airspeed_min()) {
float integrator_delta = rate_error * ki_rate * delta_time * scaler;
if (_last_out < -45) {
// prevent the integrator from increasing if surface defln demand is above the upper limit
@ -189,7 +189,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
_last_out = _pid_info.D + _pid_info.FF + _pid_info.P;
_pid_info.desired = desired_rate;
if (autotune.running && aspeed > aparm.airspeed_min) {
if (autotune.running && aspeed > airspeed.get_airspeed_min()) {
// let autotune have a go at the values
// Note that we don't pass the integrator component so we get
// a better idea of how much the base PD controller
@ -241,7 +241,7 @@ int32_t AP_PitchController::get_rate_out(float desired_rate, float scaler)
float aspeed;
if (!_ahrs.airspeed_estimate(&aspeed)) {
// If no airspeed available use average of min and max
aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_max));
aspeed = 0.5f*(airspeed.get_airspeed_min() + airspeed.get_airspeed_max());
}
return _get_rate_out(desired_rate, scaler, false, aspeed);
}
@ -272,13 +272,13 @@ float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inv
}
if (!_ahrs.airspeed_estimate(&aspeed)) {
// If no airspeed available use average of min and max
aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_max));
aspeed = 0.5f*(airspeed.get_airspeed_min() + airspeed.get_airspeed_max());
}
if (abs(_ahrs.pitch_sensor) > 7000) {
// don't do turn coordination handling when at very high pitch angles
rate_offset = 0;
} else {
rate_offset = cosf(_ahrs.pitch)*fabsf(ToDeg((GRAVITY_MSS / MAX((aspeed * _ahrs.get_EAS2TAS()) , float(aparm.airspeed_min))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff;
rate_offset = cosf(_ahrs.pitch)*fabsf(ToDeg((GRAVITY_MSS / MAX((aspeed * _ahrs.get_EAS2TAS()) , airspeed.get_airspeed_min())) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff;
}
if (inverted) {
rate_offset = -rate_offset;

1
libraries/APM_Control/AP_PitchController.h
View File

@ -37,6 +37,7 @@ public:
private:
const AP_Vehicle::FixedWing &aparm;
AP_Airspeed airspeed;
AP_AutoTune::ATGains gains;
AP_AutoTune autotune;
AP_Int16 _max_rate_neg;

4
libraries/APM_Control/AP_RollController.cpp
View File

@ -133,7 +133,7 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
if (!disable_integrator && ki_rate > 0) {
//only integrate if gain and time step are positive and airspeed above min value.
if (dt > 0 && aspeed > float(aparm.airspeed_min)) {
if (dt > 0 && aspeed > airspeed.get_airspeed_min()) {
float integrator_delta = rate_error * ki_rate * delta_time * scaler;
// prevent the integrator from increasing if surface defln demand is above the upper limit
if (_last_out < -45) {
@ -165,7 +165,7 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
_last_out = _pid_info.FF + _pid_info.P + _pid_info.D;
if (autotune.running && aspeed > aparm.airspeed_min) {
if (autotune.running && aspeed > airspeed.get_airspeed_min()) {
// let autotune have a go at the values
// Note that we don't pass the integrator component so we get
// a better idea of how much the base PD controller

1
libraries/APM_Control/AP_RollController.h
View File

@ -44,6 +44,7 @@ public:
private:
const AP_Vehicle::FixedWing &aparm;
AP_Airspeed airspeed;
AP_AutoTune::ATGains gains;
AP_AutoTune autotune;
uint32_t _last_t;

12
libraries/APM_Control/AP_YawController.cpp
View File

@ -79,11 +79,7 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator)
}
_last_t = tnow;
int16_t aspd_min = aparm.airspeed_min;
if (aspd_min < 1) {
aspd_min = 1;
}
float aspd_min = MAX(airspeed.get_airspeed_min(),1.0f);
float delta_time = (float) dt / 1000.0f;
@ -97,9 +93,9 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator)
}
if (!_ahrs.airspeed_estimate(&aspeed)) {
// If no airspeed available use average of min and max
aspeed = 0.5f*(float(aspd_min) + float(aparm.airspeed_max));
aspeed = 0.5f*(aspd_min + airspeed.get_airspeed_max());
}
rate_offset = (GRAVITY_MSS / MAX(aspeed , float(aspd_min))) * tanf(bank_angle) * cosf(bank_angle) * _K_FF;
rate_offset = (GRAVITY_MSS / MAX(aspeed , aspd_min)) * tanf(bank_angle) * cosf(bank_angle) * _K_FF;
// Get body rate vector (radians/sec)
float omega_z = _ahrs.get_gyro().z;
@ -127,7 +123,7 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator)
// Don't integrate if _K_D is zero as integrator will keep winding up
if (!disable_integrator && _K_D > 0) {
//only integrate if airspeed above min value
if (aspeed > float(aspd_min))
if (aspeed > aspd_min)
{
// prevent the integrator from increasing if surface defln demand is above the upper limit
if (_last_out < -45) {

1
libraries/APM_Control/AP_YawController.h
View File

@ -29,6 +29,7 @@ public:
private:
const AP_Vehicle::FixedWing &aparm;
AP_Airspeed airspeed;
AP_Float _K_A;
AP_Float _K_I;
AP_Float _K_D;