MEL PICBASIC Forum - Angle-determine-tilt-from-3-axis-g-sensor

Here is an equation, and example for determining tilt angle of x, y, and z. It uses the cordic equation include file. For more info about the cordic, see: http://www.picbasic.co.uk/forum/cont...h-PicBasic-Pro The below equation required longs, and a few other adjustments. It seems to be pretty accurate, though I have not come up with an easy way to test it. I have included TRIGL.INC, which has a minor edit to run with pbpl.

Measuring Tilt using a Three Axis Solution
In order to define the angles of the accelerometer in three dimensions the pitch, roll and theta are sensed using all three outputs of the accelerometer. Pitch (ρ) is defined as the angle of the X-axis relative to ground. Roll (ϕ) is defined as the angle of the Y-axis relative to the ground. Theta (θ) is the angle of the Z axis relative to gravity.

http://www.freescale.com/files/senso...ote/AN3461.pdf

Here is the meat of the code:

Code:

solveangle:
'**********Solve for x angle ********************************************
if xresult < yresult + zresult then 
    dummy = 5000   'when x is smaller the following equation works best
    xlong = (dummy * xresult) /(sqr((yresult*yresult) + (zresult*zresult)))
else  'keeps results accurate at near 0 and near 90 degree values
    dummy =30      'when x is larger the following equation works best
    xlong = (dummy * xresult*xresult) / ((yresult*yresult)+(zresult*zresult))
endif
y = xlong
x = dummy 'Assign x and y values for atan2 function
call atan2
xangle = 9000 - y

if yresult < xresult + zresult then 
    dummy = 5000  'when y is smaller the following eqauation works best
    ylong = (dummy * yresult) / (sqr((xresult*xresult) + (zresult*zresult)))
else
    dummy =30 'when y is larger the following equation works best
    ylong = (dummy * yresult*yresult) / ((xresult*xresult)+(zresult*zresult))
endif 
y = ylong
x = dummy '
call atan2
yangle = 9000 - y

Menu

Recent Forum Posts

For Sale (5 Pieces) New Samsung Galaxy Z Fold7 256GB Original (Unlocked) $7,995CAD

Angle-determine-tilt-from-3-axis-g-sensor