Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_Control: Modified calculation of bank compensation rate offset to prevent climb if speed > max fbw speed

Browse Source 
The previous calculation constrained the speed used to calculate the
bank compensation rate offset between the min and max fbw speeds. This
would result in an unwanted climb if flown above the max fbw speed
(this could happen in fbw-a mode)
This commit is contained in:
priseborough 2013-04-29 17:37:34 +10:00 committed by Andrew Tridgell
parent e7736ed4e0
commit 205397d030
2 changed files with 18 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/APM_Control/AP_PitchController.cpp
View File

@ -39,9 +39,23 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab
if(_ahrs == NULL) return 0;
float delta_time = (float)dt / 1000.0f;
int8_t inv = 1;
if(abs(_ahrs->roll_sensor)>9000) inv = -1;
// Calculate offset to pitch rate demand required to maintain pitch angle whilst banking
// Calculate ideal turn rate from bank angle and airspeed assuming a level coordinated turn
// Pitch rate offset is the component of turn rate about the pitch axis
float aspeed;
float rate_offset;
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);
}
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 / 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;
angle_err *= inv;

2
libraries/APM_Control/AP_YawController.cpp
View File

@ -49,7 +49,7 @@ int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement)
}
}
}
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().z;