Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 22:35:23 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'digitalentity/betaflight-imu-fixes' into betaflight

Browse source
This commit is contained in:
borisbstyle 2015-11-06 12:06:43 +01:00
commit 91e6922b16
5 changed files with 58 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

10
src/main/blackbox/blackbox.c
View file

@ -206,6 +206,9 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
{"accSmooth", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"accSmooth", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"accSmooth", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"attitude", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"attitude", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"attitude", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
/* Motors only rarely drops under minthrottle (when stick falls below mincommand), so predict minthrottle for it and use *unsigned* encoding (which is large for negative numbers but more compact for positive ones): */
{"motor", 0, UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_1)},
/* Subsequent motors base their I-frame values on the first one, P-frame values on the average of last two frames: */
@ -275,6 +278,7 @@ typedef struct blackboxMainState_s {
int16_t rcCommand[4];
int16_t gyroADC[XYZ_AXIS_COUNT];
int16_t accSmooth[XYZ_AXIS_COUNT];
int16_t attitude[XYZ_AXIS_COUNT];
int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t servo[MAX_SUPPORTED_SERVOS];
@ -549,6 +553,7 @@ static void writeIntraframe(void)
blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT);
blackboxWriteSigned16VBArray(blackboxCurrent->accSmooth, XYZ_AXIS_COUNT);
blackboxWriteSigned16VBArray(blackboxCurrent->attitude, XYZ_AXIS_COUNT);
//Motors can be below minthrottle when disarmed, but that doesn't happen much
blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - masterConfig.escAndServoConfig.minthrottle);
@ -675,6 +680,7 @@ static void writeInterframe(void)
//Since gyros, accs and motors are noisy, base their predictions on the average of the history:
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, gyroADC), XYZ_AXIS_COUNT);
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accSmooth), XYZ_AXIS_COUNT);
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, attitude), XYZ_AXIS_COUNT);
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, motor), motorCount);
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) {
@ -923,6 +929,10 @@ static void loadMainState(void)
blackboxCurrent->accSmooth[i] = accSmooth[i];
}
blackboxCurrent->attitude[0] = attitude.values.roll;
blackboxCurrent->attitude[0] = attitude.values.pitch;
blackboxCurrent->attitude[0] = attitude.values.yaw;
for (i = 0; i < motorCount; i++) {
blackboxCurrent->motor[i] = motor[i];
}

2
src/main/config/config.c
View file

@ -396,7 +396,7 @@ static void resetConf(void)
// global settings
masterConfig.current_profile_index = 0; // default profile
masterConfig.dcm_kp = 10000; // 1.0 * 10000
masterConfig.dcm_ki = 30; // 0.003 * 10000
masterConfig.dcm_ki = 0; // 0.003 * 10000
resetAccelerometerTrims(&masterConfig.accZero);

60
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;
@ -342,13 +370,13 @@ static bool imuIsAccelerometerHealthy(void)
int32_t accMagnitude = 0;
for (axis = 0; axis < 3; axis++) {
accMagnitude += (int32_t)accADC[axis] * accADC[axis];
accMagnitude += (int32_t)accSmooth[axis] * accSmooth[axis];
}
accMagnitude = accMagnitude * 100 / ((int32_t)acc_1G * acc_1G);
// Accept accel readings only in range 0.85g - 1.15g
return (72 < accMagnitude) && (accMagnitude < 133);
// Accept accel readings only in range 0.90g - 1.10g
return (81 < accMagnitude) && (accMagnitude < 121);
}
static bool isMagnetometerHealthy(void)
@ -370,17 +398,17 @@ static void imuCalculateEstimatedAttitude(void)
uint32_t deltaT = currentTime - previousIMUUpdateTime;
previousIMUUpdateTime = currentTime;
// If reading is considered valid - apply filter
for (axis = 0; axis < 3; axis++) {
if (imuRuntimeConfig->acc_cut_hz > 0) {
accSmooth[axis] = filterApplyPt1(accADC[axis], &accLPFState[axis], imuRuntimeConfig->acc_cut_hz, deltaT * 1e-6);
} else {
accSmooth[axis] = accADC[axis];
}
}
// Smooth and use only valid accelerometer readings
if (imuIsAccelerometerHealthy()) {
// If reading is considered valid - apply filter
for (axis = 0; axis < 3; axis++) {
if (imuRuntimeConfig->acc_cut_hz > 0) {
accSmooth[axis] = filterApplyPt1(accADC[axis], &accLPFState[axis], imuRuntimeConfig->acc_cut_hz, deltaT * 1e-6);
} else {
accSmooth[axis] = accADC[axis];
}
}
useAcc = true;
}

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

4
src/main/io/serial_cli.c
View file

@ -416,8 +416,8 @@ const clivalue_t valueTable[] = {
{ "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, 100, 1200 },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, 0, 128 },
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, 0, 20000 },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, 0, 20000 },
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, 0, 50000 },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, 0, 50000 },
{ "alt_hold_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].rcControlsConfig.alt_hold_deadband, 1, 250 },
{ "alt_hold_fast_change", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].rcControlsConfig.alt_hold_fast_change, 0, 1 },