MEMSIC 2125 object at 45deg?
simonl
Posts: 866
I wonder if someone would produce a version·of the MEMSIC 2125 object that accepts 'rest' to be with both axes at 45deg to horizontal?
I'd like to implement MEMSIC's formula for dual axis inclination sensing of a moving vehicle·[noparse][[/noparse] angle = acos( 0.707 x ( Ax + Ay ) / g ) ], as shown in their app' note #AN-00MX-012, but don't really know where to start.
Thanks in advance.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,
Simon
I'd like to implement MEMSIC's formula for dual axis inclination sensing of a moving vehicle·[noparse][[/noparse] angle = acos( 0.707 x ( Ax + Ay ) / g ) ], as shown in their app' note #AN-00MX-012, but don't really know where to start.
Thanks in advance.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,
Simon
Comments
This is essentially what it describes, only the sensor is fixed at 45 Deg. If you look at the 'MEMSIC 2125 object' there is a line of code that reads...
Really all this does is read the RAW X and Y values at that moment whatever the position of the sensor, and then uses those values as an offset value to subtract
from future RAW X and Y values.
If you omit this line, then you will see nothing but RAW X and Y values, because the offset will be Zero.
Mount your sensor at 45 Deg and the formula should apply to the RAW X and Y sensor values.
The difficulty then is getting 'acos' but not impossible.... There are some objects floating around to do that, but it takes up a huge amount of space with many unnecessary
functions let me look at it and I will get back to you.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Beau Schwabe
IC Layout Engineer
Parallax, Inc.
I've been looking for an ACos routine, but haven't yet found one. I thought there might be one in the 'Assembly Function Engine v.1.0' but there isn't. Am I looking in the right place?
Also, thanks for the acc.setlevel advice
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,
Simon
Here is an updated version with ArcSine and ArcCosine.
ArcSine and ArcCosine are derived from the built-in ROM Sine table by way of successive approximation.
A reverse lookup table if you will, but instead of looking at every entry... 2048 of them, the "pivot" method
I am using here only requires 11 lookups (1 for each bit of resolution). If the table had 256 entries this
process would require 8 lookups to find the solution.
{ ******************************************** Assembly Function Engine ******************************************** (C) 2005 Parallax, Inc. V1.1 ******************************************** coded by Beau Schwabe (Parallax) ******************************************** Version 1.0 - initial [b]test[/b] release Version 1.1 - fixed Sine/Cosine routines (problems in quadrants 3 & 4) - added successive approximation ArcSine [b]and[/b] ArcCosine functions. } [b]CON[/b] #1, _Sine,_Cosine,_ArcSine [b]VAR[/b] [b]long[/b] cog, command '################################################################################################################ [b]PUB[/b] Sine(Ang)|Arg1_ ' Input = 13-bit angle ranging from 0 to 8191 'Output = 16-bit Sine value ranging from $FFFF0001 ('-1') to $0000FFFF ('1') setcommand(_Sine, @Ang) [b]Result[/b] := Arg1_ [b]PUB[/b] Cosine(Ang)|Arg1_ ' Input = 13-bit angle ranging from 0 to 8191 'Output = 16-bit Cosine value ranging from $FFFF0001 ('-1') to $0000FFFF ('1') setcommand(_Cosine, @Ang) [b]Result[/b] := Arg1_ [b]PUB[/b] ArcSine(Ang)|Arg1_ ' Input = signed 16-bit value ranging from $FFFF0001 ('-1') to $0000FFFF ('1') 'Output = signed 11-bit angle ranging from -2047 (-pi/2) to 2047 (pi/2) setcommand(_ArcSine, @Ang) [b]Result[/b] := Arg1_ [b]PUB[/b] ArcCosine(Ang)|Arg1_,sign ' Input = signed 16-bit value ranging from $FFFF0001 ('-1') to $0000FFFF ('1') sign := Ang 'Output = signed 11-bit angle ranging from -2047 (-pi/2) to 2047 (pi/2) Ang := || Ang setcommand(_ArcSine, @Ang) [b]if[/b] sign <> Ang sign := -1 [b]else[/b] sign := 1 [b]Result[/b] := (Sin_90 - Arg1_)* sign '################################################################################################################ [b]PUB[/b] start : okay '' Start Assembly Function Engine - starts a cog '' returns false if no cog available stop okay := cog := [b]cognew[/b](@loop, @command) + 1 [b]PUB[/b] stop '' Stop Assembly Function Engine - frees a cog [b]if[/b] cog [b]cogstop[/b](cog~ - 1) command~ [b]PRI[/b] setcommand(cmd, argptr) command := cmd << 16 + argptr 'write command and pointer [b]repeat[/b] [b]while[/b] command 'wait for command to be cleared, signifying receipt '################################################################################################################ [b]DAT[/b] [b]org[/b] ' ' ' Function Engine - main loop ' loop [b]rdlong[/b] t1,[b]par[/b] [b]wz[/b] 'wait for command [b]if_z[/b] [b]jmp[/b] #loop [b]movd[/b] :arg,#arg0 'get 8 arguments [b]mov[/b] t2,t1 [b]mov[/b] t3,#8 :arg [b]rdlong[/b] arg0,t2 [b]add[/b] :arg,d0 [b]add[/b] t2,#4 [b]djnz[/b] t3,#:arg [b]mov[/b] address,t1 'preserve address location for passing 'variables back to spin language. [b]wrlong[/b] zero,[b]par[/b] 'zero command to signify command received [b]ror[/b] t1,#16+2 'lookup command address [b]add[/b] t1,#jumps [b]movs[/b] :table,t1 [b]rol[/b] t1,#2 [b]shl[/b] t1,#3 :table [b]mov[/b] t2,0 [b]shr[/b] t2,t1 [b]and[/b] t2,#$FF [b]jmp[/b] t2 'jump to command jumps [b]byte[/b] 0 '0 [b]byte[/b] Sine_ '1 [b]byte[/b] Cosine_ '2 [b]byte[/b] ArcSine_ '3 [b]byte[/b] NotUsed_A '─┐ [b]byte[/b] NotUsed_B ' │ [b]byte[/b] NotUsed_C ' ┣─ Additional functions MUST be in groups of 4-bytes (1 long) [b]byte[/b] NotUsed_D '─┘ With this setup, there is a limit of 256 possible functions. NotUsed_A NotUsed_B NotUsed_C NotUsed_D [b]jmp[/b] #loop '################################################################################################################ { Sine/cosine quadrant: 1 2 3 4 angle: $0000...$07FF $0800...$0FFF $1000...$17FF $1800...$1FFF table index: $0000...$07FF $0800...$0001 $0000...$07FF $0800...$0001 mirror: +offset -offset +offset -offset flip: +sample +sample -sample -sample on entry: sin[noparse][[/noparse]12..0] holds angle (0° to just under 360°) on exit: sin holds signed value ranging [b]from[/b] $0000FFFF ('1') to $FFFF0001 ('-1') } Cosine_ [b]mov[/b] t1, Arg0 '<--- cosine entry [b]add[/b] t1, sin_90 [b]jmp[/b] #CSentry Sine_ [b]mov[/b] t1, Arg0 '<--- sine entry CSentry [b]test[/b] t1, Sin_90 [b]wc[/b] [b]test[/b] t1, Sin_180 [b]wz[/b] [b]negc[/b] t1, t1 [b]or[/b] t1, Sin_Table [b]shl[/b] t1, #1 [b]rdword[/b] t1, t1 [b]negnz[/b] t1, t1 [b]mov[/b] t2, address 'Write data back to Arg1 [b]add[/b] t2, #4 [b]wrlong[/b] t1, t2 '<--- cosine/sine exit [b]jmp[/b] #loop 'Go wait for next command { ArcSine on entry: t2 holds signed 16-bit value ranging [b]from[/b] $FFFF0001 ('-1') to $0000FFFF ('1') on exit: t7 holds signed 11-bit angle ranging [b]from[/b] -2047 (-[b]pi[/b]/2) to 2047 ([b]pi[/b]/2) } ArcSine_ '<--- ArcSine entry (t2) [b]mov[/b] t2, Arg0 [b]mov[/b] t3, t2 'Preserve sign (t3) ; if '-' then t3 = 1 [b]shr[/b] t3, #31 [b]abs[/b] t2, t2 'Convert to absolute value [b]mov[/b] t4, sin_90 'Preload RefHigh (t4) to 2048 [b]mov[/b] t5, #0 'Preload RefLow (t5) to 0 [b]mov[/b] t6, #11 'Iterations (t6) - equals # of bits on output resolution. Iteration_Loop [b]mov[/b] t7, t4 'Add RefHigh and RefLow ; divide by 2 to get Pivot point (t7) [b]add[/b] t7, t5 [b]shr[/b] t7, #1 [b]mov[/b] t8, t7 'Lookup sine value from Pivot location ; range 0-2048 ; 0 to pi/2 [b]or[/b] t8, sin_table [b]shl[/b] t8, #1 [b]rdword[/b] t8, t8 't8 holds sine value ranging from $0000FFFF ('1') to $FFFF0001 ('-1') [b]cmps[/b] t2, t8 [b]wc[/b] 'Set 'C' if Input (t2) < 'Sine value'(t8) [b]if_c[/b] [b]mov[/b] t4, t7 'If Input < 'Sine value' then RefHigh = Pivot [b]if_nc[/b] [b]mov[/b] t5, t7 'If Input >= 'Sine value' then RefLow = Pivot [b]djnz[/b] t6, #Iteration_Loop 'Re-Iterate to Pin-Point Reverse Sine lookup value. [b]cmp[/b] t3, #1 [b]wc[/b] 'Restore sign from t3 [b]if_nc[/b] [b]neg[/b] t7, t7 [b]mov[/b] t1, address 'Write data back to Arg1 [b]add[/b] t1, #4 [b]wrlong[/b] t7, t1 [b]jmp[/b] #loop 'Go wait for next command { ########################### Defined data ########################### } zero [b]long[/b] 0 'constants d0 [b]long[/b] $200 Sin_90 [b]long[/b] $0800 Sin_180 [b]long[/b] $1000 sin_table [b]long[/b] $E000 >> 1 'sine table base shifted right { ########################### Undefined data ########################### } t1 [b]res[/b] 1 'temp variables available for program overhead t2 [b]res[/b] 1 t3 [b]res[/b] 1 t4 [b]res[/b] 1 t5 [b]res[/b] 1 t6 [b]res[/b] 1 t7 [b]res[/b] 1 t8 [b]res[/b] 1 address [b]res[/b] 1 arg0 [b]res[/b] 1 'arguments passed from high-level arg1 [b]res[/b] 1 arg2 [b]res[/b] 1 arg3 [b]res[/b] 1 arg4 [b]res[/b] 1 arg5 [b]res[/b] 1 arg6 [b]res[/b] 1 arg7 [b]res[/b] 1▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Beau Schwabe
IC Layout Engineer
Parallax, Inc.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,
Simon
Unfortunately I'm not getting anywhere with my demo code of the MEMSIC #AN-00MX-012 45° formula [noparse][[/noparse]angle = ACos( 0.707 x ( Ax + Ay ) / g )]. I think I've got the floating-point stuff correct, but it all seems to hang-up when I add the ArcCosine part.
I have a feeling it's something to do with scaling, but can't get my head around it. I wonder if you would mind taking a look at my code and laugh at my obvious mistake(s)!
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,
Simon
·
In·the formula· ...[noparse][[/noparse]angle = ACos( 0.707 x ( Ax + Ay ) / g )]
·
'g' I believe is acceleration not 'G' gravity as you have it in your formula try using roVal instead.
There are some other scaling issues I see.· The function ArcCosine is looking for a 16-bit signed
number that represents "1" (65535)·to "-1" (-65535).· The·ArcCosine function returns an 11-bit
signed angle ranging from "pi/2" (2048)·to "-pi/2" (-2048).
··
You want RAW values for Ax and Ay· ... you need to remove the line from your code that reads:
·
acc.setlevel
·
...This line of code simply creates a mathematical offset for the accelerometer that says "Whatever
position you are currently in (<-- The accelerometer), we are going to call that level".· For your
application you do not want this feature.
·
Ax, and Ay might be ok ... remember Ax and Ay are supposed to represent the number of "ticks"
the pulse from the Memsic2125 remains HIGH.· So this value needs to be scaled according to your
clock speed to represent the proper value.
·
Under "normal" conditions, Ax and Ay should produce a 5mS pulse·at "level" ... at·80MHz·(12.5nS)
Ax and Ay should return something like 400,000
·
·
·
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Beau Schwabe
IC Layout Engineer
Parallax, Inc.
Post Edited (Beau Schwabe (Parallax)) : 9/25/2006 3:39:51 AM GMT
I'll try your suggestions, and see if I can figure-out how to do the scaling -- I'm sure I've seen some Parallax documents that show examples (I think one of them's by Andy Lindsey).
As for the 400,000 ticks, I'm seeing nothing like that but I guess there's some calculation going on in the MEMSIC driver -- I'll see if I can spot it.
Thanks again.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,
Simon
that is also called within '.sensor.memsic.2125'· ... also your doing a conversion (cvt) within '.sensor.memsic.2125' that attaches a valid unit of micro-Seconds to your value.· In doing
this, you should also apply 'cvt' to your roValue also.·
·
In an earlier post...
"I think I've got the floating-point stuff correct, but it all seems to hang-up when I add the ArcCosine part."
·
You need to 'start' the function engine with something like.... 'func.start' in your initialization routine.
·
·
·
Attached is a version of the '.sensor.memsic.2125' with some modifications.· With this version, you should be
able to use 'theta', and just add or subtract 45 Deg to get your position.· You should calculate 'ro' separately
though.· You should be able to derive this from the floating point math routines.
·
ro = sqr ( x^2 + y^2 )
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Beau Schwabe
IC Layout Engineer
Parallax, Inc.
Thanks for the updated .sensor.memsic.2125 object, I'll try that a.s.a.p.
Last night I ended-up removing the driver completely and used BS2.Pulsin_Clk instead; that got me the 400_000 you mentioned
I've found the "Smart Sensors and Applications" (Chapter #3) article, and am trying to get my head around the scaling stuff. Am I correct in saying that I need to scale my 'ArcCosine( angle )' to be between (dec) -65535 to 65535? This will give me numbers between (dec) -2048 to 2048?
I will then need to convert that output to degrees. I'm presuming that (say) -2000 will correspond to an angle 'below horizontal', and 2000 above horizontal. Any pointer on how I determine the actual angle in degrees would be greatly appreciated (from anyone -- I realise that I'm 'leaning' on Beau quite a lot at the moment...)
Thanks in advance.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,
Simon
Yes
"...This will give me numbers between (dec) -2048 to 2048?"
Should
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Beau Schwabe
IC Layout Engineer
Parallax, Inc.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,
Simon