rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #869 from SteveAmor/remove_tricopter_yaw_gyro_smoothing 

Remove tricopter yaw gyro smoothing from imuUpdate
This commit is contained in:
Dominic Clifton 2015-05-29 15:39:02 +01:00
parent b194288668 6448b4b3d7
commit 41d737e29a
17 changed files with 85 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
docs/development/PID Internals.md
View File

@ -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

12
src/main/blackbox/blackbox.c
View File

@ -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++) {

2
src/main/blackbox/blackbox.h
View File

@ -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];

10
src/main/drivers/accgyro_l3g4200d.c
View File

@ -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]);
}

8
src/main/drivers/accgyro_l3gd20.c
View File

@ -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]);

8
src/main/drivers/accgyro_lsm303dlhc.c
View File

@ -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]);

10
src/main/drivers/accgyro_mpu3050.c
View File

@ -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)

10
src/main/drivers/accgyro_mpu6050.c
View File

@ -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]);
}

4
src/main/drivers/accgyro_spi_mpu6000.c
View File

@ -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)
{

4
src/main/drivers/accgyro_spi_mpu6000.h
View File

@ -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);

10
src/main/drivers/accgyro_spi_mpu6500.c
View File

@ -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]);
}

16
src/main/flight/imu.c
View File

@ -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)

3
src/main/flight/imu.h
View File

@ -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);

62
src/main/flight/pid.c
View File

@ -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;

2
src/main/io/serial_msp.c
View File

@ -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;

2
src/main/mw.c
View File

@ -728,7 +728,7 @@ void loop(void)
if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
loopTime = currentTime + masterConfig.looptime;
imuUpdate(&currentProfile->accelerometerTrims, masterConfig.mixerMode);
imuUpdate(&currentProfile->accelerometerTrims);
// Measure loop rate just after reading the sensors
currentTime = micros();

4
src/main/sensors/initialisation.c
View File

@ -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);