Code: Select all
delta = imu.gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = imu.gyroData[axis];
DTerm = delta1[axis]+delta2[axis]+delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
The intermediate deltas are irrelevant, only current value and a single value 3 samples ago factor into DTerm. Unless I am missing something painfully obvious, the following would be shorter and more logical but still yield exactly the same answer:
Code: Select all
Dterm = imu.gyroData[axis] - last3Gyro[axis];
last3Gyro[axis] = last2Gyro[axis];
last2Gyro[axis] = lastGyro[axis];
lastGyro[axis] = imu.gyroData[axis];