diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index d5ea0c6857..06a4f9c406 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -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*airspeed.get_airspeed_min()) { + if (dt > 0 && aspeed > 0.5f*float(aparm.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 > airspeed.get_airspeed_min()) { + if (autotune.running && aspeed > aparm.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*(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); } @@ -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*(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) { // 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()) , 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) { rate_offset = -rate_offset; diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index f25d14c136..7cdac2d63f 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -37,7 +37,6 @@ public: private: const AP_Vehicle::FixedWing &aparm; - AP_Airspeed airspeed; AP_AutoTune::ATGains gains; AP_AutoTune autotune; AP_Int16 _max_rate_neg; diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index 2d9fcd5470..a770df83b9 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -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 > airspeed.get_airspeed_min()) { + if (dt > 0 && aspeed > float(aparm.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 > airspeed.get_airspeed_min()) { + if (autotune.running && aspeed > aparm.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 diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index 7212dae9e9..aea62ba053 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -44,7 +44,6 @@ public: private: const AP_Vehicle::FixedWing &aparm; - AP_Airspeed airspeed; AP_AutoTune::ATGains gains; AP_AutoTune autotune; uint32_t _last_t; diff --git a/libraries/APM_Control/AP_YawController.cpp b/libraries/APM_Control/AP_YawController.cpp index 79214df62f..df7add4b23 100644 --- a/libraries/APM_Control/AP_YawController.cpp +++ b/libraries/APM_Control/AP_YawController.cpp @@ -79,7 +79,11 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator) } _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; @@ -93,9 +97,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*(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) 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 if (!disable_integrator && _K_D > 0) { //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 if (_last_out < -45) { diff --git a/libraries/APM_Control/AP_YawController.h b/libraries/APM_Control/AP_YawController.h index 23727e911e..d5e11e9143 100644 --- a/libraries/APM_Control/AP_YawController.h +++ b/libraries/APM_Control/AP_YawController.h @@ -29,7 +29,6 @@ public: private: const AP_Vehicle::FixedWing &aparm; - AP_Airspeed airspeed; AP_Float _K_A; AP_Float _K_I; AP_Float _K_D;