From 6448b4b3d71eb7660115c880fb74b807c3d22e54 Mon Sep 17 00:00:00 2001 From: Steveis Date: Sun, 17 May 2015 07:14:42 +0100 Subject: [PATCH] Replaced gyroData with gyroADC as they both contain the same value --- docs/development/PID Internals.md | 16 +++---- src/main/blackbox/blackbox.c | 12 ++--- src/main/blackbox/blackbox.h | 2 +- src/main/drivers/accgyro_l3g4200d.c | 10 ++--- src/main/drivers/accgyro_l3gd20.c | 8 ++-- src/main/drivers/accgyro_lsm303dlhc.c | 8 ++-- src/main/drivers/accgyro_mpu3050.c | 10 ++--- src/main/drivers/accgyro_mpu6050.c | 10 ++--- src/main/drivers/accgyro_spi_mpu6000.c | 4 +- src/main/drivers/accgyro_spi_mpu6000.h | 4 +- src/main/drivers/accgyro_spi_mpu6500.c | 10 ++--- src/main/flight/imu.c | 4 -- src/main/flight/imu.h | 1 - src/main/flight/pid.c | 62 +++++++++++++------------- src/main/io/serial_msp.c | 2 +- src/main/sensors/initialisation.c | 4 +- 16 files changed, 81 insertions(+), 86 deletions(-) diff --git a/docs/development/PID Internals.md b/docs/development/PID Internals.md index 8e9e55af6..6cb81db68 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 4dbf02686..47bb4c48a 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -199,9 +199,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)}, @@ -469,7 +469,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++) { @@ -578,7 +578,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++) { @@ -751,7 +751,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 59085249e..45d7ab2ca 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 bece47171..dd083ea73 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 a50e3d34f..2253548d3 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 c750e9c79..2217c2257 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 fbf8be1a9..ff90dd06c 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 f1a5067bc..52a77c40d 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, @@ -438,7 +438,7 @@ static void mpu6050GyroInit(void) #endif } -static void mpu6050GyroRead(int16_t *gyroData) +static void mpu6050GyroRead(int16_t *gyroADC) { uint8_t buf[6]; @@ -446,7 +446,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 8795bc54d..e8f38548b 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 ad4fd855d..d296b0427 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 7b3893cc1..f293d7104 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 962d9e562..50b009abb 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; @@ -301,9 +300,6 @@ static void imuCalculateEstimatedAttitude(void) void imuUpdate(rollAndPitchTrims_t *accelerometerTrims) { gyroUpdate(); - gyroData[FD_ROLL] = gyroADC[FD_ROLL]; - gyroData[FD_PITCH] = gyroADC[FD_PITCH]; - gyroData[FD_YAW] = gyroADC[FD_YAW]; if (sensors(SENSOR_ACC)) { updateAccelerationReadings(accelerometerTrims); // TODO rename to accelerometerUpdate and rename many other 'Acceleration' references to be 'Accelerometer' diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 6cb95d7c0..c97ebd39b 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 { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 74ad06dc2..ea5781b08 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -173,7 +173,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 @@ -257,12 +257,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; @@ -280,9 +280,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; @@ -320,10 +320,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; } @@ -360,10 +360,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; @@ -382,9 +382,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)); @@ -451,12 +451,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; @@ -474,9 +474,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; @@ -492,9 +492,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)); @@ -528,7 +528,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; @@ -545,8 +545,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 @@ -568,10 +568,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); } @@ -589,10 +589,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; @@ -610,7 +610,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; @@ -621,7 +621,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; @@ -697,7 +697,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]) >> 7; diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 0034a0d97..69f3372c7 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -833,7 +833,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/sensors/initialisation.c b/src/main/sensors/initialisation.c index 75148d72e..9f61b7326 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);