Changeset 3421
 Timestamp:
 Nov 14, 2012, 12:10:08 AM (18 months ago)
 Location:
 cpu/arm/olpc
 Files:

 2 edited
Legend:
 Unmodified
 Added
 Removed

cpu/arm/olpc/accelerometer.fth
r3373 r3421 4 4 " accelerometer" name 5 5 " lis3lv02d" +compatible 6 7 true value hires? 6 8 7 9 \ reg is set dynamically by probing to find which chip is present … … 14 16 : ctl1! ( b  ) h# 20 accreg! ; 15 17 : ctl4! ( b  ) h# 23 accreg! ; 16 : accelerometeron (  ) h# 47 ctl1! ; 18 : ctl4@ (  b ) h# 23 accreg@ ; 19 : accelerometeron (  ) 20 h# 47 ctl1! 21 hires? if 8 ctl4! then \ High resolution mode 22 ; 17 23 : accelerometeroff (  ) h# 07 ctl1! ; \ should this be 00? 18 24 : wext ( b  n ) dup h# 8000 and if h# ffff0000 or then ; 25 26 \ The scale factor for an acceleration component is 1 gravity = 1000 units. 19 27 : acceleration@ (  x y z ) 20 28 begin h# 27 accreg@ h# 08 and until \ wait for data available 21 29 h# 0a8 1 6 " bytesoutin" $callparent ( xl xh yl yh zl zh ) 22 bwjoin wext 5>>a ( xl xh yl yh z )30 bwjoin wext 4 >>a ( xl xh yl yh z ) 23 31 >r ( xl xh yl yh r: z ) 24 bwjoin wext 5>>a ( xl xh y r: z )32 bwjoin wext 4 >>a ( xl xh y r: z ) 25 33 >r ( xl xh r: z y ) 26 bwjoin wext 5>>a ( x r: z y )34 bwjoin wext 4 >>a ( x r: z y ) 27 35 r> r> ( x y z ) 28 36 ; … … 40 48 rot r>  ( x3 y3 z3 ) 41 49 ; 50 42 51 \ Averaging a lot of samples reduces the effect of vibration 52 \ The vendor recommends averaging 5 samples for selftest. 43 53 : averageacceleration@ (  x y z ) 44 54 acceleration@ ( x y z ) 45 d# 64 0 do55 d# 4 0 do 46 56 acceleration@ t+ ( x' y' z' ) 47 loop 48 rot 6 >>a49 rot 6 >>a50 rot 6 >>a57 loop ( xsum ysum zsum ) 58 rot 5 / ( ysum zsum x ) 59 rot 5 / ( zsum x y ) 60 rot 5 / ( x y z ) 51 61 ; 52 62 … … 62 72 ; 63 73 64 \ The datasheet says the selftest change range is 3 to 32, but we 65 \ give a little headroom on the high end to allow for external 66 \ vibration. Typical is supposed to be 19. 67 \ Empirically, measured maximums were: 68 \  Mitch's unit, 32 69 \  James' A3, 41 (on rubber mat on bare ground) 70 \  James' A2, 39 71 72 d# 50 value minx 73 d# 50 value miny 74 d# 150 value minz 75 d# 150 value maxx 76 d# 150 value maxy 77 d# 450 value maxz 74 \ The vendor recommends the following min/max values for LIS3DHTR selftest: 75 \ X:80..1700 Y:80..1700 Z:80..1400. 76 \ The numbers are in units of "1LSB = 1mg", 1000 unit = 1 gravity. 77 78 d# 80 value minx 79 d# 80 value miny 80 d# 80 value minz 81 d# 1700 value maxx 82 d# 1700 value maxy 83 d# 1400 value maxz 78 84 : range? ( delta maxdelta  error? ) between 0= ; 79 85 … … 85 91 ; 86 92 93 \ Lower and upper test limits for the magnitude squared of the acceleration 94 \ vector when the device is stationary. The scaling is 1000 units = 1 gravity. 95 96 d# 900 dup * constant gsqmin \ 0.9 gravity squared 97 d# 1100 dup * constant gsqmax \ 1.1 gravity squared 98 99 : xyz>magsq ( z y z  magnitudesquared ) 100 dup * swap dup * + swap dup * + 101 ; 102 : not1g? ( x y z  error? ) 103 xyz>magsq ( magnitude**2 ) 104 gsqmin gsqmax between 0= 105 ; 106 87 107 : lis33deselftest (  error? ) 88 108 \ Use the device's selftest function to force a change in one direction … … 96 116 rot negate rot ( dx' dy dz ) 97 117 negate ( dx dy dz' ) 98 error? if accelerometerofftrue exit then118 error? if true exit then 99 119 100 120 \ Use the device's selftest function to force a change in the opposite direction … … 107 127 \ because we subtract the new measurement from the old. 108 128 swap negate swap ( dx dy' dz ) 109 error? if accelerometeroff true exit then 110 false 129 error? 111 130 ; 112 131 : lis3dhtrselftest (  error? ) 132 delay ( ) 133 averageacceleration@ ( x y z ) 134 135 \ Check that the magnitude of the acceleration vector is about 1 G 136 3dup not1g? if ( x y z ) 137 3drop ( ) 138 ." Acceleration is not 1 gravity" cr ( ) 139 true exit (  error? ) 140 then ( x y z ) 141 113 142 \ Use the device's selftest function to force a change in one direction 114 delay ( )115 averageacceleration@ ( x y z )116 143 h# 0a ctl4! ( x y z ) \ High res, Selftest mode 0 117 144 delay … … 119 146 rot negate rot negate rot negate 120 147 h# 08 ctl4! ( x y z ) \ High res, Normal mode 121 error? if accelerometerofftrue exit then148 error? if true exit then 122 149 123 150 \ Use the device's selftest function to force a change in the opposite direction … … 128 155 averageacceleration@ t ( dx dy dz ) 129 156 h# 08 ctl4! ( x y z ) \ High res, Normal mode 130 error? if accelerometeroff true exit then 131 132 accelerometeroff 133 false 157 error? 134 158 ; 135 159 defer lisselftest … … 137 161 open 0= if true exit then 138 162 139 finaltest? if accelerometeroff false exit then 140 141 ." Don't move!" cr 142 lisselftest 163 finaltest? if 164 false 165 else 166 ." Don't move!" cr 167 lisselftest 168 then 169 170 close 143 171 ; 144 172 … … 150 178 \ Support for new LIS3DHTR chip 151 179 d# 400,000 to busspeed 152 d# 50 to minx d# 50 to miny d# 50 to minz153 d# 1 50 to maxx d# 150 to maxy d# 450 to maxz180 d# 80 to minx d# 80 to miny d# 80 to minz 181 d# 1700 to maxx d# 1700 to maxy d# 1400 to maxz 154 182 h# 19 1 reg 155 183 ['] lis3dhtrselftest to lisselftest 184 true to hires? 156 185 else 157 186 accelerometeroff … … 159 188 \ Support for old LIS33DE chip 160 189 d# 25,000 to busspeed 161 d# 20 to minx d# 20 to miny d# 20 to minz162 d# 400 to maxx d# 400 to maxy d# 400 to maxz190 d# 40 to minx d# 40 to miny d# 40 to minz 191 d# 800 to maxx d# 800 to maxy d# 800 to maxz 163 192 h# 1d 1 reg 164 193 ['] lis33deselftest to lisselftest 194 false to hires? 165 195 then 166 196 ; 
cpu/arm/olpc/roller.fth
r3070 r3421 76 76 d# 19 d# 20 a/b>fraction value damping 77 77 78 d# 1 d# 80 a/b>fraction value accscale78 d# 1 d# 160 a/b>fraction value accscale 79 79 80 80 d# 20 constant ballradius
Note: See TracChangeset
for help on using the changeset viewer.