The problem was originally reported by "mistral":
mistral wrote:I try the ziss_dm mod with WMP V1.6 ITG 3205. Work fine but (their is always "but" !!) on quick movements the gyro's signal goes on the wrong side :
The problem occur on all 3 axis in only one side by axis (upper side of graphic). I try to fly with, slow fly ok, but quick fly or windy condition, my tri makes unwanted flips !!
Initial thoughts was it is related to TG3205 chip itself. But recently I have connected "genuine" sparkfun ITG3200 and found, that it has the same issue. After couple of hours of investigation. I think, I have found the issue. This chip has full 16bit ADC which reporting signed integers in range -32768..32767. The problem starts when we try to correct gyro orientation:
Code: Select all
gyroADC[ROLL] = + ((rawADC_ITG3200[2]<<8) | rawADC_ITG3200[3]);
gyroADC[PITCH] = - ((rawADC_ITG3200[0]<<8) | rawADC_ITG3200[1]);
gyroADC[YAW] = - ((rawADC_ITG3200[4]<<8) | rawADC_ITG3200[5]);
The operation x = - (-32768) cannot be complited by runtime, due the fact that +32768 is out of range of int16_t. In practice result of thes operation is still -32768. There are lot of workarounds for this, but I thing the best way is to move scaling of the gyroADC[] from "updateIMU" to the "i2c_Gyro_getADC" (the same way as we doing for all accelerometers) and apply orientation corrections after scaling.
i2c_Gyro_getADC
Code: Select all
..
gyroADC[ROLL] = + (GYRO_buff.Data.X / 5 / 4);
gyroADC[PITCH] = + (GYRO_buff.Data.Y / 5 / 4);
gyroADC[YAW] = - (GYRO_buff.Data.Z / 5 / 4);
..
updateIMU
Code: Select all
..
gyroADC[ROLL] = (gyroADC[ROLL] - gyroZero[ROLL]);
gyroADC[PITCH] = (gyroADC[PITCH] - gyroZero[PITCH]);
gyroADC[YAW] = (gyroADC[YAW] - gyroZero[YAW]);
..
I have created patch, which implements this idea:
PS: In this patch I have used union and structs instead of bit operations (compiller should do hard work intead of us ..). This way it economises couple bytes of memory, and probably litle bit more efficient.
Regards,
ziss_dm