Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 01:05:27 +03:00
Code Issues Wiki Activity

Removed IMU kP gain scaling based on spin rate. Works better without it in real life

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-11-07 21:56:23 +10:00
parent 5c8970e41a
commit 621008cffc
1 changed files with 5 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

19
src/main/flight/imu.c
View file

@ -215,17 +215,14 @@ static bool imuUseFastGains(void)
return !ARMING_FLAG(ARMED) && millis() < 20000;
}
// Taken from http://gentlenav.googlecode.com/files/fastRotations.pdf
static float imuGetPGainScaleFactor(float spin_rate)
{
if (spin_rate < DEGREES_TO_RADIANS(50)) {
return 1.0f;
}
else if (spin_rate > DEGREES_TO_RADIANS(500)) {
if (imuUseFastGains()) {
return 10.0f;
}
return spin_rate / DEGREES_TO_RADIANS(50);
else {
return 1.0f;
}
}
static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
@ -309,13 +306,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
}
// Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster
float dcmKpGain = imuRuntimeConfig->dcm_kp;
if (imuUseFastGains()) {
dcmKpGain *= 10;
}
else {
dcmKpGain *= imuGetPGainScaleFactor(spin_rate);
}
float dcmKpGain = imuRuntimeConfig->dcm_kp * imuGetPGainScaleFactor(spin_rate);
// Apply proportional and integral feedback
gx += dcmKpGain * ex + integralFBx;