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

Dynamic kP gain for IMU calculations

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-11-06 20:25:17 +10:00
parent 0f8e4460bf
commit 831ac83f2b
3 changed files with 33 additions and 6965 deletions
Download patch file Download diff file Expand all files Collapse all files

6961
obj/betaflight_IRCFUSIONF3.hex
View file

File diff suppressed because it is too large Load diff

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

@ -50,6 +50,12 @@
#include "config/runtime_config.h"
// the limit (in degrees/second) beyond which we stop integrating
// omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
// which results in false gyro drift. See
// http://gentlenav.googlecode.com/files/fastRotations.pdf
#define SPIN_RATE_LIMIT 20
int16_t accSmooth[XYZ_AXIS_COUNT];
int32_t accSum[XYZ_AXIS_COUNT];
@ -209,6 +215,19 @@ 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)) {
return 10.0f;
}
return spin_rate / DEGREES_TO_RADIANS(50);
}
static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
bool useAcc, float ax, float ay, float az,
bool useMag, float mx, float my, float mz,
@ -220,6 +239,9 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
float ex = 0, ey = 0, ez = 0;
float qa, qb, qc;
// Calculate general spin rate (rad/s)
float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
// Use raw heading error (from GPS or whatever else)
if (useYaw) {
while (yawError > M_PIf) yawError -= (2.0f * M_PIf);
@ -272,10 +294,13 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
// Compute and apply integral feedback if enabled
if(imuRuntimeConfig->dcm_ki > 0.0f) {
float dcmKiGain = imuRuntimeConfig->dcm_ki;
integralFBx += dcmKiGain * ex * dt; // integral error scaled by Ki
integralFBy += dcmKiGain * ey * dt;
integralFBz += dcmKiGain * ez * dt;
// Stop integrating if spinning beyond the certain limit
if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
float dcmKiGain = imuRuntimeConfig->dcm_ki;
integralFBx += dcmKiGain * ex * dt; // integral error scaled by Ki
integralFBy += dcmKiGain * ey * dt;
integralFBz += dcmKiGain * ez * dt;
}
}
else {
integralFBx = 0.0f; // prevent integral windup
@ -288,6 +313,9 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
if (imuUseFastGains()) {
dcmKpGain *= 10;
}
else {
dcmKpGain *= imuGetPGainScaleFactor(spin_rate);
}
// Apply proportional and integral feedback
gx += dcmKpGain * ex + integralFBx;

1
src/main/flight/imu.h
View file

@ -27,6 +27,7 @@ extern int32_t accSum[XYZ_AXIS_COUNT];
#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
#define DECIDEGREES_TO_DEGREES(angle) (angle / 10)
#define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f)
#define DEGREES_TO_RADIANS(angle) ((angle) * 0.0174532925f)
typedef union {
int16_t raw[XYZ_AXIS_COUNT];