diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index 0a2df28d30..4a66039008 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -55,13 +55,13 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab float bank_angle = _ahrs->roll; // limit bank angle between +- 80 deg if right way up if (fabsf(bank_angle) < 1.5707964f) { - bank_angle = constrain(bank_angle,-1.3962634,1.3962634f); + bank_angle = constrain_float(bank_angle,-1.3962634f,1.3962634f); } if (!_ahrs->airspeed_estimate(&aspeed)) { // If no airspeed available use average of min and max aspeed = 0.5f*(float(aspd_min) + float(aspd_max)); } - rate_offset = fabsf(ToDeg((9.807f / constrain(aspeed , float(aspd_min), float(aspd_max))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff; + rate_offset = fabsf(ToDeg((9.807f / max(aspeed , float(aspd_min))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff; //Calculate pitch angle error in centi-degrees int32_t angle_err = angle - _ahrs->pitch_sensor; @@ -82,16 +82,8 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab // Get body rate vector (radians/sec) float omega_y = _ahrs->get_gyro().y; - // Apply a first order lowpass filter with a 20Hz cut-off - // Coefficients derived using a first order hold discretisation method - // Use of FOH discretisation increases high frequency noise rejection - // and reduces phase loss compared to other methods - float rate = 0.0810026f * _last_rate_out + 0.6343426f * omega_y + 0.2846549f * _last_rate_in; - _last_rate_out = rate; - _last_rate_in = omega_y; - // Calculate the pitch rate error (deg/sec) and scale - float rate_error = (desired_rate - ToDeg(rate)) * scaler; + float rate_error = (desired_rate - ToDeg(omega_y)) * scaler; // Multiply pitch rate error by _ki_rate and integrate // Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs @@ -114,11 +106,10 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab // Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward // path, but want a 1/speed^2 scaler applied to the rate error path. // This is because acceleration scales with speed^2, but rate scales with speed. - float _last_out = ( (rate_error * _kp_rate) + _integrator + (desired_rate * _kp_ff) ) * scaler; + _last_out = ( (rate_error * _kp_rate) + _integrator + (desired_rate * _kp_ff) ) * scaler; // Convert to centi-degrees and constrain - float out = constrain(_last_out * 100, -4500, 4500); - return out; + return constrain_float(_last_out * 100, -4500, 4500); } void AP_PitchController::reset_I() diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index 2e1a4c98ef..07113e7410 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -30,8 +30,6 @@ private: AP_Int16 _max_rate_neg; AP_Float _roll_ff; uint32_t _last_t; - float _last_rate_in; - float _last_rate_out; float _last_out; float _integrator; diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index 655f8ad615..84b922f25c 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -57,16 +57,8 @@ int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabi // Get body rate vector (radians/sec) float omega_x = _ahrs->get_gyro().x; - // Apply a first order lowpass filter with a 20Hz cut-off - // Coefficients derived using a first order hold discretisation method - // Use of FOH discretisation increases high frequency noise rejection - // and reduces phase loss compared to other methods - float rate = 0.0810026f * _last_rate_out + 0.6343426f * omega_x + 0.2846549f * _last_rate_in; - _last_rate_out = rate; - _last_rate_in = omega_x; - // Calculate the roll rate error (deg/sec) and apply gain scaler - float rate_error = (desired_rate - ToDeg(rate)) * scaler; + float rate_error = (desired_rate - ToDeg(omega_x)) * scaler; // Get an airspeed estimate - default to zero if none available float aspeed; @@ -93,11 +85,10 @@ int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabi // Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward // path, but want a 1/speed^2 scaler applied to the rate error path. // This is because acceleration scales with speed^2, but rate scales with speed. - float _last_out = ( (rate_error * _kp_rate) + _integrator + (desired_rate * _kp_ff) ) * scaler; + _last_out = ( (rate_error * _kp_rate) + _integrator + (desired_rate * _kp_ff) ) * scaler; // Convert to centi-degrees and constrain - float out = constrain(_last_out * 100, -4500, 4500); - return out; + return constrain_float(_last_out * 100, -4500, 4500); } void AP_RollController::reset_I() diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index 4a1c7a679e..9dbb70929d 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -28,8 +28,6 @@ private: AP_Float _ki_rate; AP_Int16 _max_rate; uint32_t _last_t; - float _last_rate_in; - float _last_rate_out; float _last_out; float _integrator; diff --git a/libraries/APM_Control/AP_YawController.cpp b/libraries/APM_Control/AP_YawController.cpp index 7c478179df..6d6a67156c 100644 --- a/libraries/APM_Control/AP_YawController.cpp +++ b/libraries/APM_Control/AP_YawController.cpp @@ -44,31 +44,23 @@ int32_t AP_YawController::get_servo_out(float scaler, bool stabilize, int16_t as float bank_angle = _ahrs->roll; // limit bank angle between +- 80 deg if right way up if (fabsf(bank_angle) < 1.5707964f) { - bank_angle = constrain(bank_angle,-1.3962634,1.3962634f); + bank_angle = constrain_float(bank_angle,-1.3962634f,1.3962634f); } if (!_ahrs->airspeed_estimate(&aspeed)) { // If no airspeed available use average of min and max aspeed = 0.5f*(float(aspd_min) + float(aspd_max)); } - rate_offset = (9.807f / constrain(aspeed , float(aspd_min), float(aspd_max))) * tanf(bank_angle) * cosf(bank_angle) * _K_FF; + rate_offset = (9.807f / 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(); - - // Apply a first order lowpass filter with a 20Hz cut-off - // Coefficients derived using a first order hold discretisation method - // Use of FOH discretisation increases high frequency noise rejection - // and reduces phase loss compared to other methods - float rate = 0.0810026f * _last_rate_out + 0.6343426f * omega_z + 0.2846549f * _last_rate_in; - _last_rate_out = rate; - _last_rate_in = omega_z; + float omega_z = _ahrs->get_gyro().z; // Get the accln vector (m/s^2) float accel_y = _ins->get_accel().y; // Subtract the steady turn component of rate from the measured rate // to calculate the rate relative to the turn requirement in degrees/sec - float rate_hp_in = ToDeg(rate - rate_offset); + float rate_hp_in = ToDeg(omega_z - rate_offset); // Apply a high-pass filter to the rate to washout any steady state error // due to bias errors in rate_offset @@ -112,8 +104,7 @@ int32_t AP_YawController::get_servo_out(float scaler, bool stabilize, int16_t as _last_out = _K_D * (_integrator - rate_hp_out) * scaler * scaler; // Convert to centi-degrees and constrain - float out = constrain(_last_out * 100, -4500, 4500); - return out; + return constrain_float(_last_out * 100, -4500, 4500); } void AP_YawController::reset_I() diff --git a/libraries/APM_Control/AP_YawController.h b/libraries/APM_Control/AP_YawController.h index 531573598f..ede3df312c 100644 --- a/libraries/APM_Control/AP_YawController.h +++ b/libraries/APM_Control/AP_YawController.h @@ -32,8 +32,6 @@ private: AP_Float _K_FF; uint32_t _last_t; float _last_error; - float _last_rate_in; - float _last_rate_out; float _last_out; float _last_rate_hp_out; float _last_rate_hp_in;