Skip to content

Commit

Permalink
RC11 changes.
Browse files Browse the repository at this point in the history 
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.
  • Loading branch information
@rs2k
rs2k committed Apr 22, 2016
1 parent 171a8d0 commit 14cd32b
Show file tree
Hide file tree
Showing 7 changed files with 56 additions and 91 deletions.
64 changes: 35 additions & 29 deletions src/main/drivers/accgyro_mpu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
57 changes: 1 addition & 56 deletions src/main/drivers/accgyro_spi_mpu6000.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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)
{
Expand Down
2 changes: 1 addition & 1 deletion src/main/drivers/accgyro_spi_mpu9250.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
7 changes: 6 additions & 1 deletion src/main/flight/pid.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@

extern float dT;
float Throttle_p;
extern bool FullKiLatched;
extern bool motorLimitReached;
extern bool allowITermShrinkOnly;

Expand Down Expand Up @@ -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)) {
Expand Down
11 changes: 10 additions & 1 deletion src/main/mw.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -355,6 +361,8 @@ void mwDisarm(void)
if (ARMING_FLAG(ARMED)) {
DISABLE_ARMING_FLAG(ARMED);

FullKiLatched = false;

#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
finishBlackbox();
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -753,7 +762,7 @@ void taskMainPidLoop(void)
}
#endif

if (ResetErrorActivated) {
if ( (ResetErrorActivated) && (!FullKiLatched) ) {
pidResetErrorGyro();
}

Expand Down
2 changes: 1 addition & 1 deletion src/main/sensors/initialisation.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
4 changes: 2 additions & 2 deletions src/main/version.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down

0 comments on commit 14cd32b

Please sign in to comment.