AnsweredAssumed Answered

L3G4200D angle calculation

Question asked by benedetto.salvatore on May 24, 2012
Hi there,

I'm using an L3G4200D sensor for calculating the angle displacement
with the code below. However from the computation I get half of the real rotation
the sensor make, that is, if I rotate it of 90 degree, I measure a 45 degree rotation.

The output data rate is set a 100hz, resolution is 250dps and the BW is set at 12,5Hz.
I realized that I might loose some samples (I don't use interrupts),
but that should not be a problem when rotating at constant speed,
as I always calculate the delta from the last sample.

Any suggestion would be appreciated.

                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       
117           
while (1) {
           
118           
            
119           
        gyroscope.readGyroscopeData(x, y, z);
           
120           
            
121           
// Data rate is 100Hz, thus T = 0.01s
           
122           
        uint64_t currentValue = util::currentCounterValue();
           
123           
        uint32_t delta = currentValue - previousValue;
           
124           
        timeElapsed = (float)delta / 1000000;
           
125           
            
126           
        previousValue = currentValue;
           
127           
            
128           
/* Convert data to dps */
           
129           
if ((x - x0) > kThreshold || (x - x0) < -kThreshold)
           
130           
          wxz = (x - x0) * 0.00875;
           
131           
else
           
132           
          wxz = 0;
           
133           
            
134           
if ((y - y0) > kThreshold || (y - y0) < -kThreshold)
           
135           
          wyz = (y - y0) * 0.00875;
           
136           
else
           
137           
          wyz = 0;
           
138           
            
139           
if ((z - z0) > kThreshold || (z - z0) < -kThreshold)
           
140           
          wxy = (z - z0) * 0.00875;
           
141           
else
           
142           
          wxy = 0;
           
143           
            
144           
        xz += (wxz * timeElapsed);
           
145           
        yz += (wyz * timeElapsed);
           
146           
        xy += (wxy * timeElapsed);
           
147           
            
148           
        sprintf(bufferOut, "#wxz=%+f wyz=%+f wxy=%+f xz=%+f yz=%+f xy=%+f\n",
           
149           
                wxz, wyz, wxy, xz, yz, xy);
           
150           
        trace(bufferOut);
           
151           
            
152           
        gpioPortC.toggle(GPIO::ePin12);
           
153           
/* Sensor data rate is set at 100Hz. */
           
154           
//util::delayms(1);
           
155           
      }
           
156           
            

Outcomes