Posted on May 24, 2012 at 14:47Hi 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.
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L117
while
(
1
) {
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L118
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L119
gyroscope.readGyroscopeData(x, y, z);
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L120
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L121
// Data rate is 100Hz, thus T = 0.01s
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L122
uint64_t currentValue = util::currentCounterValue();
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L123
uint32_t delta = currentValue - previousValue;
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L124
timeElapsed = (
float
)delta /
1000000
;
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L125
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L126
previousValue = currentValue;
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L127
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L128
/* Convert data to dps */
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L129
if
((x - x0) > kThreshold || (x - x0) < -kThreshold)
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L130
wxz = (x - x0) *
0
.00875
;
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L131
else
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L132
wxz =
0
;
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L133
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L134
if
((y - y0) > kThreshold || (y - y0) < -kThreshold)
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L135
wyz = (y - y0) *
0
.00875
;
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L136
else
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L137
wyz =
0
;
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L138
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L139
if
((z - z0) > kThreshold || (z - z0) < -kThreshold)
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L140
wxy = (z - z0) *
0
.00875
;
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L141
else
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L142
wxy =
0
;
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L143
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L144
xz += (wxz * timeElapsed);
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L145
yz += (wyz * timeElapsed);
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L146
xy += (wxy * timeElapsed);
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L147
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L148
sprintf(bufferOut,
''
#wxz=%+f wyz=%+f wxy=%+f xz=%+f yz=%+f xy=%+f
\n
''
,
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L149
wxz, wyz, wxy, xz, yz, xy);
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L150
trace(bufferOut);
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L151
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L152
gpioPortC.toggle(GPIO::ePin12);
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L153
/* Sensor data rate is set at 100Hz. */
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L154
//util::delayms(1);
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L155
}
http://geneva.servebeer.com/redmine/projects/opencortex-m/repository/revisions/master/entry/demos/l3g200d-i2c/main.cpp#L156
