diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 4f7db7c1fd..e98732932b 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -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]; } diff --git a/src/main/config/config.c b/src/main/config/config.c index b1ff075a67..aba7cf2cfd 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -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); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index d566a0081b..e577a4be0f 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -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; } diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 9d4a2d07c1..2fb0f425f4 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -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]; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index b0f1fd4915..64824c6fd6 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -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 },