Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_Control: fixed build and merge errors

Browse Source
This commit is contained in:
Andrew Tridgell 2013-05-05 14:34:33 +10:00
parent 10ecffce01
commit 23d9c31b0a
6 changed files with 13 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/APM_Control/AP_PitchController.cpp
View File

@ -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()

2
libraries/APM_Control/AP_PitchController.h
View File

@ -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;

15
libraries/APM_Control/AP_RollController.cpp
View File

@ -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()

2
libraries/APM_Control/AP_RollController.h
View File

@ -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;

19
libraries/APM_Control/AP_YawController.cpp
View File

@ -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()

2
libraries/APM_Control/AP_YawController.h
View File

@ -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;