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

Revert "APM_Control: Convert references to AP_Airspeed." 

This reverts commit cc5a2417a6.
This commit is contained in:
Tom Pittenger 2016-08-07 17:48:25 -07:00
parent e5bd23e34a
commit 82777873af
6 changed files with 15 additions and 14 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; float ki_rate = k_I * gains.tau;
//only integrate if gain and time step are positive and airspeed above min value. //only integrate if gain and time step are positive and airspeed above min value.
if (dt > 0 && aspeed > 0.5f*airspeed.get_airspeed_min()) { if (dt > 0 && aspeed > 0.5f*float(aparm.airspeed_min)) {
float integrator_delta = rate_error * ki_rate * delta_time * scaler; float integrator_delta = rate_error * ki_rate * delta_time * scaler;
if (_last_out < -45) { if (_last_out < -45) {
// prevent the integrator from increasing if surface defln demand is above the upper limit // 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; _last_out = _pid_info.D + _pid_info.FF + _pid_info.P;
_pid_info.desired = desired_rate; _pid_info.desired = desired_rate;
if (autotune.running && aspeed > airspeed.get_airspeed_min()) { if (autotune.running && aspeed > aparm.airspeed_min) {
// let autotune have a go at the values // let autotune have a go at the values
// Note that we don't pass the integrator component so we get // Note that we don't pass the integrator component so we get
// a better idea of how much the base PD controller // 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; float aspeed;
if (!_ahrs.airspeed_estimate(&aspeed)) { if (!_ahrs.airspeed_estimate(&aspeed)) {
// If no airspeed available use average of min and max // If no airspeed available use average of min and max
aspeed = 0.5f*(airspeed.get_airspeed_min() + airspeed.get_airspeed_max()); aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_max));
} }
return _get_rate_out(desired_rate, scaler, false, aspeed); 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 (!_ahrs.airspeed_estimate(&aspeed)) {
// If no airspeed available use average of min and max // If no airspeed available use average of min and max
aspeed = 0.5f*(airspeed.get_airspeed_min() + airspeed.get_airspeed_max()); aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_max));
} }
if (abs(_ahrs.pitch_sensor) > 7000) { if (abs(_ahrs.pitch_sensor) > 7000) {
// don't do turn coordination handling when at very high pitch angles // don't do turn coordination handling when at very high pitch angles
rate_offset = 0; rate_offset = 0;
} else { } else {
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; 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;
} }
if (inverted) { if (inverted) {
rate_offset = -rate_offset; rate_offset = -rate_offset;

1
libraries/APM_Control/AP_PitchController.h
View File

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

1
libraries/APM_Control/AP_RollController.h
View File

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

12
libraries/APM_Control/AP_YawController.cpp
View File

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

1
libraries/APM_Control/AP_YawController.h
View File

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