From 14cd32b991bc3e10b7e35b25234099140b5fdf69 Mon Sep 17 00:00:00 2001 From: Kalyn Doerr Date: Fri, 22 Apr 2016 15:14:56 -0500 Subject: [PATCH] RC11 changes. New anti-windup prevention that won't affect flight. (Activated with Always Stabilized) No need for idle up switch when Always Stabilized is used. The above two features only function if switch arming is used. Reduced SPI speed down from RC 10. I think 48 MHz was pushing it too far. Gyro overflow fix. Death rolls shouldn't be possible anymore. Note, setting your rates to greater than 1950 DPS or so is still not advised. --- src/main/drivers/accgyro_mpu.c | 64 ++++++++++++++------------ src/main/drivers/accgyro_spi_mpu6000.c | 57 +---------------------- src/main/drivers/accgyro_spi_mpu9250.c | 2 +- src/main/flight/pid.c | 7 ++- src/main/mw.c | 11 ++++- src/main/sensors/initialisation.c | 2 +- src/main/version.h | 4 +- 7 files changed, 56 insertions(+), 91 deletions(-) diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index da4d29ba7..a7d725756 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -311,45 +311,51 @@ bool mpuGyroRead(int16_t *gyroADC) return false; } - static int16_t last_data0[3] = {0, 0, 0}; - static int16_t last_data1[3] = {0, 0, 0}; - int16_t current_data[3] = {0, 0, 0}; + static int32_t last_data0[3] = {0, 0, 0}; + static int32_t last_data1[3] = {0, 0, 0}; + int32_t current_data[3] = {0, 0, 0}; current_data[0] = (int16_t)((data[0] << 8) | data[1]); current_data[1] = (int16_t)((data[2] << 8) | data[3]); current_data[2] = (int16_t)((data[4] << 8) | data[5]); - if ( (last_data0[0] - current_data[0]) >= 32767) { - current_data[0] = 32766; - } - if ( (last_data0[1] - current_data[1]) >= 32767) { - current_data[1] = 32766; - } - if ( (last_data0[2] - current_data[2]) >= 32767) { - current_data[2] = 32766; + if ( (last_data0[0] - current_data[0]) >= 32000) { + current_data[0] = 32677; + } else + if ( (last_data0[1] - current_data[1]) >= 32000) { + current_data[1] = 32677; + } else + if ( (last_data0[2] - current_data[2]) >= 32000) { + current_data[2] = 32677; + } else + if ( (-last_data0[0] + current_data[0]) >= 32000) { + current_data[0] = -32677; + } else + if ( (-last_data0[1] + current_data[1]) >= 32000) { + current_data[1] = -32677; + } else + if ( (-last_data0[2] + current_data[2]) >= 32000) { + current_data[2] = -32677; } - if ( (-last_data0[0] + current_data[0]) >= 32767) { - current_data[0] = -32766; - } - if ( (-last_data0[1] + current_data[1]) >= 32767) { - current_data[1] = -32766; - } - if ( (-last_data0[2] + current_data[2]) >= 32767) { - current_data[2] = -32766; - } + if (current_data[0] >= 32677) { current_data[0] = 32677; } + if (current_data[1] >= 32677) { current_data[1] = 32677; } + if (current_data[2] >= 32677) { current_data[2] = 32677; } + if (current_data[0] <= -32677) { current_data[0] = -32677; } + if (current_data[1] <= -32677) { current_data[1] = -32677; } + if (current_data[2] <= -32677) { current_data[2] = -32677; } - gyroADC[0] = ((int16_t)(current_data[0] + last_data0[0] + last_data1[0])) / 3; - gyroADC[1] = ((int16_t)(current_data[1] + last_data0[1] + last_data1[1])) / 3; - gyroADC[2] = ((int16_t)(current_data[2] + last_data0[2] + last_data1[2])) / 3; + gyroADC[0] = (int16_t)((current_data[0] + last_data0[0] + last_data1[0]) / 3); + gyroADC[1] = (int16_t)((current_data[1] + last_data0[1] + last_data1[1]) / 3); + gyroADC[2] = (int16_t)((current_data[2] + last_data0[2] + last_data1[2]) / 3); - last_data0[0] = last_data1[0]; - last_data0[1] = last_data1[1]; - last_data0[2] = last_data1[2]; + last_data1[0] = last_data0[0]; + last_data1[1] = last_data0[1]; + last_data1[2] = last_data0[2]; - last_data1[0] = current_data[0]; - last_data1[1] = current_data[1]; - last_data1[2] = current_data[2]; + last_data0[0] = current_data[0]; + last_data0[1] = current_data[1]; + last_data0[2] = current_data[2]; return true; } diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index 489baa541..32fce7ac5 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -151,7 +151,7 @@ void mpu6000SpiGyroInit(uint8_t lpf) spiResetErrorCounter(MPU6000_SPI_INSTANCE); - spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_FAST_CLOCK); //high speed now that we don't need to write to the slow registers + spiSetDivisor(MPU6000_SPI_INSTANCE, 5); //high speed now that we don't need to write to the slow registers int16_t data[3]; mpuGyroRead(data); @@ -294,61 +294,6 @@ bool mpu6000SpiDetect(void) return false; } -/* -static void mpu6000AccAndGyroInit(void) { - - if (mpuSpi6000InitDone) { - return; - } - - spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_SLOW_CLOCK); - - // Device Reset - mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET); - delay(150); - - mpu6000WriteRegister(MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP); - delay(150); - - // Clock Source PPL with Z axis gyro reference - mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); - delayMicroseconds(15); - - // Disable Primary I2C Interface - mpu6000WriteRegister(MPU_RA_USER_CTRL, BIT_I2C_IF_DIS); - delayMicroseconds(15); - - mpu6000WriteRegister(MPU_RA_PWR_MGMT_2, 0x00); - delayMicroseconds(15); - - // Accel Sample Rate 1kHz - // Gyroscope Output Rate = 1kHz when the DLPF is enabled - mpu6000WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); - delayMicroseconds(15); - - // Gyro +/- 1000 DPS Full Scale - mpu6000WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); - delayMicroseconds(15); - - // Accel +/- 8 G Full Scale - mpu6000WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); - delayMicroseconds(15); - - - mpu6000WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR - delayMicroseconds(15); - -#ifdef USE_MPU_DATA_READY_SIGNAL - mpu6000WriteRegister(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); - delayMicroseconds(15); -#endif - - spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_FAST_CLOCK); // 18 MHz SPI clock - delayMicroseconds(1); - - mpuSpi6000InitDone = true; -} -*/ bool mpu6000SpiAccDetect(acc_t *acc) { diff --git a/src/main/drivers/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro_spi_mpu9250.c index 5d3bff4f7..ece83d479 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro_spi_mpu9250.c @@ -105,7 +105,7 @@ void mpu9250SpiGyroInit(uint8_t lpf) spiResetErrorCounter(MPU9250_SPI_INSTANCE); - spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_ULTRAFAST_CLOCK); //high speed now that we don't need to write to the slow registers + spiSetDivisor(MPU9250_SPI_INSTANCE, 5); //high speed now that we don't need to write to the slow registers int16_t data[3]; mpuGyroRead(data); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d33f2f8c9..3475f2a01 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -50,6 +50,7 @@ extern float dT; float Throttle_p; +extern bool FullKiLatched; extern bool motorLimitReached; extern bool allowITermShrinkOnly; @@ -205,7 +206,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } // -----calculate I component. - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * (pidProfile->I_f[axis]/2) * 10, -250.0f, 250.0f); + if (FullKiLatched) { + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * (pidProfile->I_f[axis]/2) * 10, -250.0f, 250.0f); + } else { + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * (pidProfile->I_f[axis]/2) * 10, -10.0f, 10.0f); + } if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { diff --git a/src/main/mw.c b/src/main/mw.c index 384374efb..56a134c87 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -101,6 +101,8 @@ enum { #define GYRO_WATCHDOG_DELAY 100 // Watchdog delay for gyro sync +bool FullKiLatched = false; + // AIR MODE Reset timers #define ERROR_RESET_DEACTIVATE_DELAY (1 * 1000) // 1 sec delay to disable AIR MODE Iterm resetting bool allowITermShrinkOnly = false; @@ -302,6 +304,10 @@ void annexCode(void) rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] Throttle_p = constrainf( ((float)rcCommand[THROTTLE] - (float)masterConfig.rxConfig.mincheck) / ((float)masterConfig.rxConfig.maxcheck - (float)masterConfig.rxConfig.mincheck), 0, 100); + if ( (Throttle_p > 10) && (ARMING_FLAG(ARMED)) && (IS_RC_MODE_ACTIVE(BOXALWAYSSTABILIZED)) && (!isUsingSticksForArming()) ) { + FullKiLatched = true; + } + if (FLIGHT_MODE(HEADFREE_MODE)) { float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); float cosDiff = cos_approx(radDiff); @@ -355,6 +361,8 @@ void mwDisarm(void) if (ARMING_FLAG(ARMED)) { DISABLE_ARMING_FLAG(ARMED); + FullKiLatched = false; + #ifdef BLACKBOX if (feature(FEATURE_BLACKBOX)) { finishBlackbox(); @@ -386,6 +394,7 @@ void mwArm(void) } if (!ARMING_FLAG(PREVENT_ARMING)) { ENABLE_ARMING_FLAG(ARMED); + FullKiLatched = true; headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); #ifdef BLACKBOX @@ -753,7 +762,7 @@ void taskMainPidLoop(void) } #endif - if (ResetErrorActivated) { + if ( (ResetErrorActivated) && (!FullKiLatched) ) { pidResetErrorGyro(); } diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 0cb9761d6..9ef77e03d 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -661,7 +661,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a } #if defined(USE_GYRO_SPI_MPU6500) - spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_ULTRAFAST_CLOCK); + spiSetDivisor(MPU6500_SPI_INSTANCE, 6); #endif return true; diff --git a/src/main/version.h b/src/main/version.h index 572bfeb46..9cccb06a3 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -66,10 +66,10 @@ #define FC_VERSION_MAJOR 16 // build year #define FC_VERSION_MINOR 04 // build month -#define FC_VERSION_PATCH_LEVEL 17 // build day +#define FC_VERSION_PATCH_LEVEL 22 // build day #define FC_VERSION_LETTER "a" // build of day -#define FC_VERSION_COMMENT "RaceFlight Release 1 RC9 - Gyro Overflow Fix" +#define FC_VERSION_COMMENT "RaceFlight Release 1 RC11 - Anti-windup." #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x)