Author: wmb
Date: Wed Jul 18 23:45:40 2012
New Revision: 3070
URL: http://tracker.coreboot.org/trac/openfirmware/changeset/3070
Log:
OLPC accelerometer demo - added "level" command to implement a clinometer precise to 0.1 degrees up to 45 degrees.
Modified:
cpu/arm/olpc/roller.fth
Modified: cpu/arm/olpc/roller.fth
==============================================================================
--- cpu/arm/olpc/roller.fth Wed Jul 18 09:35:51 2012 (r3069)
+++ cpu/arm/olpc/roller.fth Wed Jul 18 23:45:40 2012 (r3070)
@@ -19,7 +19,7 @@
d# 8 value #fraction-bits
: fraction* ( -- ) * #fraction-bits >>a ;
-: 1/2 ( -- n ) 1 #fraction-bits 1- lshift ;
+: 1/2 ( -- n ) 1 #fraction-bits 1- lshift ;
: >fraction ( n -- fraction ) #fraction-bits lshift ;
: a/b>fraction ( num denom -- fraction ) swap #fraction-bits lshift swap / ;
@@ -80,13 +80,29 @@
d# 20 constant ball-radius
d# 40 constant ball-diameter
-: maxx ( -- n ) screen-wh drop ball-diameter - >fraction ;
-: maxy ( -- n ) screen-wh nip ball-diameter - >fraction ;
+: maxx-raw ( -- n ) screen-wh drop ball-diameter - ;
+: maxy-raw ( -- n ) screen-wh nip ball-diameter - ;
+
+: maxx ( -- n ) maxx-raw >fraction ;
+: maxy ( -- n ) maxy-raw >fraction ;
d# 400 >fraction constant maxz
: 3swap ( x1 y1 z1 x2 y2 z2 -- x2 y2 z2 x1 y1 z1 ) 5 roll 5 roll 5 roll ;
: 3over ( x1 y1 z1 x2 y2 z2 -- x2 y2 z2 x1 y1 z1 ) 5 pick 5 pick 5 pick ;
+\ This is a simple approximation for the magnitude of a 3-vector
+\ - largest component plus 1/3 the sum of the two smaller components.
+\ It works surprisingly well, especially since we use it to estimate
+\ drag, which is a complicated phenomenon for which the formula is
+\ only an approximation anyway.
+
+: xyz-magnitude ( x y z -- magnitude )
+ rot abs rot abs rot abs
+ 2dup > if swap then ( x min-y,z max-y,z )
+ rot 2dup > if swap then ( a b max )
+ -rot + 3 / + ( magnitude )
+;
+
[ifdef] notdef
0 0 0 3value pos-b' \ New ball position
0 0 0 3value vel-l \ Laptop velocity
@@ -107,19 +123,6 @@
vel-l damping xyz* xyz+ to vel-l ( )
;
-\ This is a simple approximation for the magnitude of a 3-vector
-\ - largest component plus 1/3 the sum of the two smaller components.
-\ It works surprisingly well, especially since we use it to estimate
-\ drag, which is a complicated phenomenon for which the formula is
-\ only an approximation anyway.
-
-: xyz-magnitude ( x y z -- magnitude )
- rot abs rot abs rot abs
- 2dup > if swap then ( x min-y,z max-y,z )
- rot 2dup > if swap then ( a b max )
- -rot + 3 / + ( magnitude )
-;
-
\ This is a coupling coefficient between the net velocity (laptop - ball)
\ and the ball acceleration. It encapsulates various constant factors
\ such as the drag coefficient and unit conversions. Its value is adjusted
@@ -251,6 +254,93 @@
text-on
;
+: filtered-acceleration ( -- acc-x,y,z )
+ 0 0 0
+ d# 16 0 do net-acceleration xyz+ loop
+ 1 d# 16 a/b>fraction xyz*
+;
+
+d# 573 constant deg*10/rad
+d# 1460 value gravity
+: .decidegrees ( degrees*10 -- )
+ dup abs d# 450 > if ( degrees*10 )
+ 0< if ." <-45" else ." >45" then
+ else
+ push-decimal
+ dup abs <# u# [char] . hold u#s swap sign u#> type
+ ." "
+ pop-base
+ then
+;
+
+0 value xsq
+0 value gsq
+
+: arcsin-step ( n -- n' ) deg*10/rad + xsq gsq */ ;
+: arcsin*10 ( x -- degrees*10 )
+ dup dup * to xsq ( x )
+ gravity dup * to gsq ( x )
+ \ Starting with this bias instead of 0 helps with roundoff error,
+ \ so the result is good to within 0.1 degree up to 45 degrees.
+ d# 7500 ( x n )
+ arcsin-step d# 20 / ( x n' ) \ x^5 / 5! term
+ arcsin-step d# 06 / ( x n' ) \ x^3 / 3! term
+ deg*10/rad + ( x n' ) \ x term
+ gravity */
+;
+
+: x-center ( -- x ) screen-wh drop 2/ ;
+: y-center ( -- y ) screen-wh nip 2/ ;
+
+: put-ball ( x y z -- ) 3dup pos-b redraw to pos-b ;
+: put-centered ( offset-x offset-y z-size -- )
+ >r ( x y r: z )
+ swap x-center + 0 max maxx-raw min >fraction ( y x' r: z )
+ swap y-center + 0 max maxy-raw min >fraction ( x' y' r: z )
+ r> put-ball
+;
+
+: show-center-mark ( -- )
+ 0 set-fg x-center ball-radius + y-center ball-radius + d# 16 circleat
+;
+
+0 0 0 3value cal-xyz
+: update-angle ( -- )
+ filtered-acceleration ( acc-x,y,z )
+ cal-xyz xyz- drop ( x' y' )
+ 0 0 at-xy
+ ." Degrees X " over arcsin*10 .decidegrees ." Y " dup arcsin*10 .decidegrees ( x' y' )
+ swap 2/ swap 2/
+ d# 0 put-centered
+ show-center-mark
+ \ ." X " rot 5 .r ." Y " swap 5 .r ." Z " 5 .r
+;
+
+: ?zero ( -- )
+ rotate-button? if
+ filtered-acceleration to cal-xyz
+ cal-xyz xyz-magnitude to gravity
+ 0 1 at-xy ." Zeroing"
+ begin rotate-button? 0= until
+ (cr ." "
+ then
+;
+: clinometer ( -- )
+ init-accelerometer
+ init-ball
+ cursor-off
+ clear-drawing
+ d# 40 0 at-xy ." Rotate button zeroes"
+ begin
+ ?zero
+ ( update-laptop ) update-angle d# 50 ms
+ key? until
+ cursor-on
+;
+: level ( -- )
+ clinometer
+;
+
\ LICENSE_BEGIN
\ Copyright (c) 2011 FirmWorks
\
Author: quozl
Date: Wed Jul 18 04:56:18 2012
New Revision: 3068
URL: http://tracker.coreboot.org/trac/openfirmware/changeset/3068
Log:
disklabel - support ISO 9660 images on non-optical drives such as USB flash and hard drives, by using a logical sector offset to the primary volume descriptor that is consistent with the 512 byte sector size on these devices. ISO 9660:1999 6.1.2 specifies minimum logical sector size, and an offset of 16 sectors to the primary volume descriptor, but does not specify arrangement of data on devices that are not a CD-ROM. Common use is to place the image on other devices in the same way it appears on CD-ROM. #12018
Modified:
ofw/disklabel/common.fth
ofw/fs/cdfs/partition.fth
Modified: ofw/disklabel/common.fth
==============================================================================
--- ofw/disklabel/common.fth Mon Jul 16 02:53:10 2012 (r3067)
+++ ofw/disklabel/common.fth Wed Jul 18 04:56:18 2012 (r3068)
@@ -19,7 +19,7 @@
0 instance value sector-offset
-\ For ISO-9660 CD-ROMs
+\ For ISO-9660 CD-ROMs, ISO-9660 flash or hard drives, and GPT
: read-hw-sector ( sector# -- )
sector-offset + /sector um* " seek" $call-parent abort" Seek failed"
sector-buf /sector " read" $call-parent /sector <> abort" Read failed"
Modified: ofw/fs/cdfs/partition.fth
==============================================================================
--- ofw/fs/cdfs/partition.fth Mon Jul 16 02:53:10 2012 (r3067)
+++ ofw/fs/cdfs/partition.fth Wed Jul 18 04:56:18 2012 (r3068)
@@ -4,7 +4,8 @@
\ Returns true if the disk or partition is an ISO 9660 file
\ system volume, as determined by reading the primary volume descriptor.
: iso-9660? ( -- flag )
- d# 16 ['] read-hw-sector catch if drop false exit then
+ d# 16 /sector h# 200 = if 4* then
+ ['] read-hw-sector catch if drop false exit then
sector-buf c@ 1 = sector-buf 1+ 5 " CD001" $= and dup if ( flag )
iso-type to partition-type