diff --git a/docs/development/PID Internals.md b/docs/development/PID Internals.md index 8e9e55af69..6cb81db68f 100644 --- a/docs/development/PID Internals.md +++ b/docs/development/PID Internals.md @@ -1,8 +1,8 @@ ### IO variables -`gyroData/8192*2000 = deg/s` +`gyroADC/8192*2000 = deg/s` -`gyroData/4 ~ deg/s` +`gyroADC/4 ~ deg/s` `rcCommand` - `<-500 - 500>` nominal, but is scaled with `rcRate/100`, max +-1250 @@ -23,7 +23,7 @@ Iacc = intergrate(error, limit +-10000) * I8[PIDLEVEL] / 4096 #### Gyro term ``` Pgyro = rcCommand[axis]; -error = rcCommand[axis] * 10 * 8 / pidProfile->P8[axis] - gyroData[axis] / 4; (conversion so that error is in deg/s ?) +error = rcCommand[axis] * 10 * 8 / pidProfile->P8[axis] - gyroADC[axis] / 4; (conversion so that error is in deg/s ?) Igyro = integrate(error, limit +-16000) / 10 / 8 * I8[axis] / 100 (conversion back to mixer units ?) ``` @@ -52,12 +52,12 @@ reset I term if #### Gyro stabilization ``` -P -= gyroData[axis] / 4 * dynP8 / 10 / 8 -D = -mean(diff(gyroData[axis] / 4), over 3 samples) * 3 * dynD8 / 32 +P -= gyroADC[axis] / 4 * dynP8 / 10 / 8 +D = -mean(diff(gyroADC[axis] / 4), over 3 samples) * 3 * dynD8 / 32 [equivalent to :] -D = - (gyroData[axis]/4 - (<3 loops old>gyroData[axis]/4)) * dynD8 / 32 +D = - (gyroADC[axis]/4 - (<3 loops old>gyroADC[axis]/4)) * dynD8 / 32 ``` This can be seen as sum of - - PI controller (handles rcCommand, HORIZON/ANGLE); `Igyro` is only output based on gyroData - - PD controller(parameters dynP8/dynD8) with zero setpoint acting on gyroData + - PI controller (handles rcCommand, HORIZON/ANGLE); `Igyro` is only output based on gyroADC + - PD controller(parameters dynP8/dynD8) with zero setpoint acting on gyroADC diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 18282c54ae..5b2dc2cefa 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -202,9 +202,9 @@ static const blackboxMainFieldDefinition_t blackboxMainFields[] = { #endif /* Gyros and accelerometers base their P-predictions on the average of the previous 2 frames to reduce noise impact */ - {"gyroData", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, - {"gyroData", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, - {"gyroData", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"gyroADC", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"gyroADC", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"gyroADC", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"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)}, @@ -486,7 +486,7 @@ static void writeIntraframe(void) #endif for (x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxCurrent->gyroData[x]); + blackboxWriteSignedVB(blackboxCurrent->gyroADC[x]); } for (x = 0; x < XYZ_AXIS_COUNT; x++) { @@ -602,7 +602,7 @@ static void writeInterframe(void) //Since gyros, accs and motors are noisy, base the prediction on the average of the history: for (x = 0; x < XYZ_AXIS_COUNT; x++) { - blackboxWriteSignedVB(blackboxHistory[0]->gyroData[x] - (blackboxHistory[1]->gyroData[x] + blackboxHistory[2]->gyroData[x]) / 2); + blackboxWriteSignedVB(blackboxHistory[0]->gyroADC[x] - (blackboxHistory[1]->gyroADC[x] + blackboxHistory[2]->gyroADC[x]) / 2); } for (x = 0; x < XYZ_AXIS_COUNT; x++) { @@ -781,7 +781,7 @@ static void loadBlackboxState(void) } for (i = 0; i < XYZ_AXIS_COUNT; i++) { - blackboxCurrent->gyroData[i] = gyroData[i]; + blackboxCurrent->gyroADC[i] = gyroADC[i]; } for (i = 0; i < XYZ_AXIS_COUNT; i++) { diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index fe6954713d..7f11008e1b 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -29,7 +29,7 @@ typedef struct blackboxValues_t { int32_t axisPID_P[XYZ_AXIS_COUNT], axisPID_I[XYZ_AXIS_COUNT], axisPID_D[XYZ_AXIS_COUNT]; int16_t rcCommand[4]; - int16_t gyroData[XYZ_AXIS_COUNT]; + int16_t gyroADC[XYZ_AXIS_COUNT]; int16_t accSmooth[XYZ_AXIS_COUNT]; int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t servo[MAX_SUPPORTED_SERVOS]; diff --git a/src/main/drivers/accgyro_l3g4200d.c b/src/main/drivers/accgyro_l3g4200d.c index bece471715..dd083ea735 100644 --- a/src/main/drivers/accgyro_l3g4200d.c +++ b/src/main/drivers/accgyro_l3g4200d.c @@ -57,7 +57,7 @@ static uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ; static void l3g4200dInit(void); -static void l3g4200dRead(int16_t *gyroData); +static void l3g4200dRead(int16_t *gyroADC); bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf) { @@ -110,13 +110,13 @@ static void l3g4200dInit(void) } // Read 3 gyro values into user-provided buffer. No overrun checking is done. -static void l3g4200dRead(int16_t *gyroData) +static void l3g4200dRead(int16_t *gyroADC) { uint8_t buf[6]; i2cRead(L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf); - gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]); - gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]); - gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]); + gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]); + gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]); + gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]); } diff --git a/src/main/drivers/accgyro_l3gd20.c b/src/main/drivers/accgyro_l3gd20.c index a50e3d34f2..2253548d31 100644 --- a/src/main/drivers/accgyro_l3gd20.c +++ b/src/main/drivers/accgyro_l3gd20.c @@ -120,7 +120,7 @@ void l3gd20GyroInit(void) delay(100); } -static void l3gd20GyroRead(int16_t *gyroData) +static void l3gd20GyroRead(int16_t *gyroADC) { uint8_t buf[6]; @@ -134,9 +134,9 @@ static void l3gd20GyroRead(int16_t *gyroData) GPIO_SetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN); - gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]); - gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]); - gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]); + gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]); + gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]); + gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]); #if 0 debug[0] = (int16_t)((buf[1] << 8) | buf[0]); diff --git a/src/main/drivers/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro_lsm303dlhc.c index c750e9c792..2217c2257b 100644 --- a/src/main/drivers/accgyro_lsm303dlhc.c +++ b/src/main/drivers/accgyro_lsm303dlhc.c @@ -106,7 +106,7 @@ void lsm303dlhcAccInit(void) } // Read 3 gyro values into user-provided buffer. No overrun checking is done. -static void lsm303dlhcAccRead(int16_t *gyroData) +static void lsm303dlhcAccRead(int16_t *gyroADC) { uint8_t buf[6]; @@ -116,9 +116,9 @@ static void lsm303dlhcAccRead(int16_t *gyroData) return; // the values range from -8192 to +8191 - gyroData[X] = (int16_t)((buf[1] << 8) | buf[0]) / 2; - gyroData[Y] = (int16_t)((buf[3] << 8) | buf[2]) / 2; - gyroData[Z] = (int16_t)((buf[5] << 8) | buf[4]) / 2; + gyroADC[X] = (int16_t)((buf[1] << 8) | buf[0]) / 2; + gyroADC[Y] = (int16_t)((buf[3] << 8) | buf[2]) / 2; + gyroADC[Z] = (int16_t)((buf[5] << 8) | buf[4]) / 2; #if 0 debug[0] = (int16_t)((buf[1] << 8) | buf[0]); diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c index fbf8be1a9c..ff90dd06c1 100644 --- a/src/main/drivers/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro_mpu3050.c @@ -58,7 +58,7 @@ static uint8_t mpuLowPassFilter = MPU3050_DLPF_42HZ; static void mpu3050Init(void); -static void mpu3050Read(int16_t *gyroData); +static void mpu3050Read(int16_t *gyroADC); static void mpu3050ReadTemp(int16_t *tempData); bool mpu3050Detect(gyro_t *gyro, uint16_t lpf) @@ -121,15 +121,15 @@ static void mpu3050Init(void) } // Read 3 gyro values into user-provided buffer. No overrun checking is done. -static void mpu3050Read(int16_t *gyroData) +static void mpu3050Read(int16_t *gyroADC) { uint8_t buf[6]; i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf); - gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]); - gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]); - gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]); + gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]); + gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]); + gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]); } static void mpu3050ReadTemp(int16_t *tempData) diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index 9656502238..8c02ed7c5a 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -175,7 +175,7 @@ static uint8_t mpuLowPassFilter = INV_FILTER_42HZ; static void mpu6050AccInit(void); static void mpu6050AccRead(int16_t *accData); static void mpu6050GyroInit(void); -static void mpu6050GyroRead(int16_t *gyroData); +static void mpu6050GyroRead(int16_t *gyroADC); typedef enum { MPU_6050_HALF_RESOLUTION, @@ -439,7 +439,7 @@ static void mpu6050GyroInit(void) #endif } -static void mpu6050GyroRead(int16_t *gyroData) +static void mpu6050GyroRead(int16_t *gyroADC) { uint8_t buf[6]; @@ -447,7 +447,7 @@ static void mpu6050GyroRead(int16_t *gyroData) return; } - gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]); - gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]); - gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]); + gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]); + gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]); + gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]); } diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index 8795bc54da..e8f38548b2 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -125,8 +125,8 @@ static bool mpuSpi6000InitDone = false; #define DISABLE_MPU6000 GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN) #define ENABLE_MPU6000 GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN) -void mpu6000SpiGyroRead(int16_t *gyroData); -void mpu6000SpiAccRead(int16_t *gyroData); +void mpu6000SpiGyroRead(int16_t *gyroADC); +void mpu6000SpiAccRead(int16_t *gyroADC); static void mpu6000WriteRegister(uint8_t reg, uint8_t data) { diff --git a/src/main/drivers/accgyro_spi_mpu6000.h b/src/main/drivers/accgyro_spi_mpu6000.h index ad4fd855d7..d296b04272 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.h +++ b/src/main/drivers/accgyro_spi_mpu6000.h @@ -16,5 +16,5 @@ bool mpu6000SpiAccDetect(acc_t *acc); bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf); -void mpu6000SpiGyroRead(int16_t *gyroData); -void mpu6000SpiAccRead(int16_t *gyroData); +void mpu6000SpiGyroRead(int16_t *gyroADC); +void mpu6000SpiAccRead(int16_t *gyroADC); diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 7b3893cc1d..f293d71040 100644 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -74,7 +74,7 @@ static uint8_t mpuLowPassFilter = INV_FILTER_42HZ; static void mpu6500AccInit(void); static void mpu6500AccRead(int16_t *accData); static void mpu6500GyroInit(void); -static void mpu6500GyroRead(int16_t *gyroData); +static void mpu6500GyroRead(int16_t *gyroADC); extern uint16_t acc_1G; @@ -188,13 +188,13 @@ static void mpu6500GyroInit(void) mpu6500WriteRegister(MPU6500_RA_RATE_DIV, 0); // 1kHz S/R } -static void mpu6500GyroRead(int16_t *gyroData) +static void mpu6500GyroRead(int16_t *gyroADC) { uint8_t buf[6]; mpu6500ReadRegister(MPU6500_RA_GYRO_XOUT_H, buf, 6); - gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]); - gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]); - gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]); + gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]); + gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]); + gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]); } diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 63158f451c..50b009abbd 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -48,7 +48,6 @@ int16_t accSmooth[XYZ_AXIS_COUNT]; int32_t accSum[XYZ_AXIS_COUNT]; -int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; uint32_t accTimeSum = 0; // keep track for integration of acc int accSumCount = 0; @@ -298,11 +297,10 @@ static void imuCalculateEstimatedAttitude(void) imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame } -void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) +void imuUpdate(rollAndPitchTrims_t *accelerometerTrims) { - static int16_t gyroYawSmooth = 0; - gyroUpdate(); + if (sensors(SENSOR_ACC)) { updateAccelerationReadings(accelerometerTrims); // TODO rename to accelerometerUpdate and rename many other 'Acceleration' references to be 'Accelerometer' imuCalculateEstimatedAttitude(); @@ -311,16 +309,6 @@ void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) accADC[Y] = 0; accADC[Z] = 0; } - - gyroData[FD_ROLL] = gyroADC[FD_ROLL]; - gyroData[FD_PITCH] = gyroADC[FD_PITCH]; - - if (mixerMode == MIXER_TRI) { - gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3; - gyroYawSmooth = gyroData[FD_YAW]; - } else { - gyroData[FD_YAW] = gyroADC[FD_YAW]; - } } int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index afd34566d4..c97ebd39ba 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -24,7 +24,6 @@ extern float accVelScale; extern t_fp_vector EstG; extern int16_t accSmooth[XYZ_AXIS_COUNT]; extern int32_t accSum[XYZ_AXIS_COUNT]; -extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT]; extern int16_t smallAngle; typedef struct rollAndPitchInclination_s { @@ -62,7 +61,7 @@ void imuConfigure( ); void calculateEstimatedAltitude(uint32_t currentTime); -void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode); +void imuUpdate(rollAndPitchTrims_t *accelerometerTrims); float calculateThrottleAngleScale(uint16_t throttle_correction_angle); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 50151cbd21..105de8bdca 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -174,7 +174,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } } - gyroRate = gyroData[axis] * gyro.scale; // gyro output scaled to dps + gyroRate = gyroADC[axis] * gyro.scale; // gyro output scaled to dps // --------low-level gyro-based PID. ---------- // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes @@ -258,12 +258,12 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } if (!FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || axis == FD_YAW) { // MODE relying on GYRO or YAW axis error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis]; - error -= gyroData[axis] / 4; + error -= gyroADC[axis] / 4; PTermGYRO = rcCommand[axis]; errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp - if ((ABS(gyroData[axis]) > (640 * 4)) || (axis == FD_YAW && ABS(rcCommand[axis]) > 100)) + if ((ABS(gyroADC[axis]) > (640 * 4)) || (axis == FD_YAW && ABS(rcCommand[axis]) > 100)) errorGyroI[axis] = 0; ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64; @@ -281,9 +281,9 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } } - PTerm -= ((int32_t)gyroData[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation - delta = (gyroData[axis] - lastGyro[axis]) / 4; - lastGyro[axis] = gyroData[axis]; + PTerm -= ((int32_t)gyroADC[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation + delta = (gyroADC[axis] - lastGyro[axis]) / 4; + lastGyro[axis] = gyroADC[axis]; deltaSum = delta1[axis] + delta2[axis] + delta; delta2[axis] = delta1[axis]; delta1[axis] = delta; @@ -319,10 +319,10 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control rc = rcCommand[axis] << 1; - error = rc - (gyroData[axis] / 4); + error = rc - (gyroADC[axis] / 4); errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp 16 bits is ok here - if (ABS(gyroData[axis]) > (640 * 4)) { + if (ABS(gyroADC[axis]) > (640 * 4)) { errorGyroI[axis] = 0; } @@ -359,10 +359,10 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control PTerm = PTermACC + ((PTerm - PTermACC) * prop >> 9); } - PTerm -= ((int32_t)(gyroData[axis] / 4) * dynP8[axis]) >> 6; // 32 bits is needed for calculation + PTerm -= ((int32_t)(gyroADC[axis] / 4) * dynP8[axis]) >> 6; // 32 bits is needed for calculation - delta = (gyroData[axis] - lastGyro[axis]) / 4; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastGyro[axis] = gyroData[axis]; + delta = (gyroADC[axis] - lastGyro[axis]) / 4; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 + lastGyro[axis] = gyroADC[axis]; DTerm = delta1[axis] + delta2[axis] + delta; delta2[axis] = delta1[axis]; delta1[axis] = delta; @@ -381,9 +381,9 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control //YAW rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->rates[FD_YAW] + 30) >> 5; #ifdef ALIENWII32 - error = rc - gyroData[FD_YAW]; + error = rc - gyroADC[FD_YAW]; #else - error = rc - (gyroData[FD_YAW] / 4); + error = rc - (gyroADC[FD_YAW] / 4); #endif errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW]; errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); @@ -450,12 +450,12 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con } if (!FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { // MODE relying on GYRO error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis]; - error -= gyroData[axis] / 4; + error -= gyroADC[axis] / 4; PTermGYRO = rcCommand[axis]; errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp - if (ABS(gyroData[axis]) > (640 * 4)) + if (ABS(gyroADC[axis]) > (640 * 4)) errorGyroI[axis] = 0; ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64; @@ -473,9 +473,9 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con } } - PTerm -= ((int32_t)gyroData[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation - delta = (gyroData[axis] - lastGyro[axis]) / 4; - lastGyro[axis] = gyroData[axis]; + PTerm -= ((int32_t)gyroADC[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation + delta = (gyroADC[axis] - lastGyro[axis]) / 4; + lastGyro[axis] = gyroADC[axis]; deltaSum = delta1[axis] + delta2[axis] + delta; delta2[axis] = delta1[axis]; delta1[axis] = delta; @@ -491,9 +491,9 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con //YAW rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->rates[FD_YAW] + 30) >> 5; #ifdef ALIENWII32 - error = rc - gyroData[FD_YAW]; + error = rc - gyroADC[FD_YAW]; #else - error = rc - (gyroData[FD_YAW] / 4); + error = rc - (gyroADC[FD_YAW] / 4); #endif errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW]; errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); @@ -523,7 +523,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { UNUSED(rxConfig); - float delta, RCfactor, rcCommandAxis, MainDptCut, gyroDataQuant; + float delta, RCfactor, rcCommandAxis, MainDptCut, gyroADCQuant; float PTerm, ITerm, DTerm, PTermACC = 0.0f, ITermACC = 0.0f, ITermGYRO, error, prop = 0.0f; static float lastGyro[2] = { 0.0f, 0.0f }, lastDTerm[2] = { 0.0f, 0.0f }; uint8_t axis; @@ -540,8 +540,8 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) } for (axis = 0; axis < 2; axis++) { - int32_t tmp = (int32_t)((float)gyroData[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea - gyroDataQuant = (float)tmp * 3.2f; // but delivers more accuracy and also reduces jittery flight + int32_t tmp = (int32_t)((float)gyroADC[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea + gyroADCQuant = (float)tmp * 3.2f; // but delivers more accuracy and also reduces jittery flight rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { #ifdef GPS @@ -563,10 +563,10 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) } if (!FLIGHT_MODE(ANGLE_MODE)) { - if (ABS((int16_t)gyroData[axis]) > 2560) { + if (ABS((int16_t)gyroADC[axis]) > 2560) { errorGyroIf[axis] = 0.0f; } else { - error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroDataQuant; + error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroADCQuant; errorGyroIf[axis] = constrainf(errorGyroIf[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f); } @@ -584,10 +584,10 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) ITerm = ITermACC; } - PTerm -= gyroDataQuant * dynP8[axis] * 0.003f; - delta = (gyroDataQuant - lastGyro[axis]) / ACCDeltaTimeINS; + PTerm -= gyroADCQuant * dynP8[axis] * 0.003f; + delta = (gyroADCQuant - lastGyro[axis]) / ACCDeltaTimeINS; - lastGyro[axis] = gyroDataQuant; + lastGyro[axis] = gyroADCQuant; lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]); DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f; @@ -605,7 +605,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->rates[FD_YAW] * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100; - int32_t tmp = lrintf(gyroData[FD_YAW] * 0.25f); + int32_t tmp = lrintf(gyroADC[FD_YAW] * 0.25f); PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80; if ((ABS(tmp) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) { errorGyroI[FD_YAW] = 0; @@ -616,7 +616,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) } } else { int32_t tmp = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->rates[FD_YAW] << 1) + 40)) >> 5; - error = tmp - lrintf(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better + error = tmp - lrintf(gyroADC[FD_YAW] * 0.25f); // Less Gyrojitter works actually better if (ABS(tmp) > 50) { errorGyroI[FD_YAW] = 0; @@ -716,7 +716,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes // -----calculate scaled error.AngleRates // multiplication of rcCommand corresponds to changing the sticks scaling here - RateError = AngleRateTmp - (gyroData[axis] / 4); + RateError = AngleRateTmp - (gyroADC[axis] / 4); // -----calculate P component PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 1061523dce..11bb5810ed 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -817,7 +817,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(accSmooth[i]); } for (i = 0; i < 3; i++) - serialize16(gyroData[i]); + serialize16(gyroADC[i]); for (i = 0; i < 3; i++) serialize16(magADC[i]); break; diff --git a/src/main/mw.c b/src/main/mw.c index 471b4294e0..a9a28c02ba 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -728,7 +728,7 @@ void loop(void) if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) { loopTime = currentTime + masterConfig.looptime; - imuUpdate(¤tProfile->accelerometerTrims, masterConfig.mixerMode); + imuUpdate(¤tProfile->accelerometerTrims); // Measure loop rate just after reading the sensors currentTime = micros(); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 75148d72e1..9f61b73269 100644 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -122,8 +122,8 @@ const mpu6050Config_t *selectMPU6050Config(void) #ifdef USE_FAKE_GYRO static void fakeGyroInit(void) {} -static void fakeGyroRead(int16_t *gyroData) { - memset(gyroData, 0, sizeof(int16_t[XYZ_AXIS_COUNT])); +static void fakeGyroRead(int16_t *gyroADC) { + memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT])); } static void fakeGyroReadTemp(int16_t *tempData) { UNUSED(tempData);