Bearing in MatLAB

Legacy support with Phidget21
pamecool2

Bearing in MatLAB

Postby pamecool2 » Mon Sep 26, 2011 4:53 am

Hi,

I have one 1056 - PhidgetSpatial 3/3/3 sensor. I have been trying to make get the Bearing Angle between 0 and 360 degrees just like the one in User Interface.

I've read the Form1.cs example, but still i don't seem to get it right in MatLAB.

The code is below,

I hope you can help me. Thanks in advanced.

Pam.-

Code: Select all

% int    CPhidgetSpatial_getAcceleration (CPhidgetSpatialHandle phid, int index, double *acceleration) : Units = g (standard gravity = 9.81m/s^2)
Acceleration = libpointer('doublePtr', 0);

error=calllib('phidget21', 'CPhidgetSpatial_getAcceleration', handle, 0,Acceleration);
[error2 Message]=calllib('phidget21', 'CPhidget_getErrorDescription', error, errorMessage);
ax=(Acceleration.Value);
 
error=calllib('phidget21', 'CPhidgetSpatial_getAcceleration', handle, 1,Acceleration);
[error2 Message]=calllib('phidget21', 'CPhidget_getErrorDescription', error, errorMessage);
ay=(Acceleration.Value);

error=calllib('phidget21', 'CPhidgetSpatial_getAcceleration', handle, 2,Acceleration);
[error2 Message]=calllib('phidget21', 'CPhidget_getErrorDescription', error, errorMessage);
az=(Acceleration.Value);

%int CPhidgetSpatial_getAngularRate (CPhidgetSpatialHandle phid, int index, double *angularRate);
AngularRate=libpointer('doublePtr', 0);

error=calllib('phidget21', 'CPhidgetSpatial_getAngularRate', handle, 0, AngularRate);
[error2 Message]=calllib('phidget21', 'CPhidget_getErrorDescription', error, errorMessage);
Ang1=(AngularRate.Value);

error=calllib('phidget21', 'CPhidgetSpatial_getAngularRate', handle, 1, AngularRate);
[error2 Message]=calllib('phidget21', 'CPhidget_getErrorDescription', error, errorMessage);
Ang2=(AngularRate.Value);

error=calllib('phidget21', 'CPhidgetSpatial_getAngularRate', handle, 2, AngularRate);
[error2 Message]=calllib('phidget21', 'CPhidget_getErrorDescription', error, errorMessage);
Ang3=(AngularRate.Value);

%int CPhidgetSpatial_getMagneticField (CPhidgetSpatialHandle phid, int index, double *magneticField);
MagneticField=libpointer('doublePtr', 0);

error=calllib('phidget21', 'CPhidgetSpatial_getMagneticField', handle, 0, MagneticField);
[error2 Message]=calllib('phidget21', 'CPhidget_getErrorDescription', error, errorMessage);
MFx=(MagneticField.Value);

error=calllib('phidget21', 'CPhidgetSpatial_getMagneticField', handle, 1, MagneticField);
[error2 Message]=calllib('phidget21', 'CPhidget_getErrorDescription', error, errorMessage);
MFy=(MagneticField.Value);

error=calllib('phidget21', 'CPhidgetSpatial_getMagneticField', handle, 2, MagneticField);
[error2 Message]=calllib('phidget21', 'CPhidget_getErrorDescription', error, errorMessage);
MFz=(MagneticField.Value);



%% Calcular Angulo Bearing, Punto cardinal %%

Pitch=asin(ax)*57.296
Roll=asin(ay)*57.296

if az<0
    Pitch=-Pitch;
    Roll=-Roll;
end

MX = [cos(Pitch) sin(Pitch)  0
      sin(Pitch) cos(Pitch)  0
      0 0 1];
       
MZ = [1 0 0
      0 cos(Roll) sin(Roll)
      0 -sin(Roll) cos(Roll)];

rotM = MX * MZ;

VecMag=[MFx MFy -MFz];

MagData=VecMag*rotM;

Xh=-MagData(3);
Yh=-MagData(1);

                if (Xh < 0)
                        bearing = pi - atan(Yh / Xh);
                elseif  (Xh > 0 && Yh < 0)
                        bearing = -atan(Yh / Xh);
                elseif  (Xh > 0 && Yh > 0)
                        bearing = (pi*2) - atan(Yh / Xh);
                elseif  (Xh == 0 && Yh < 0)
                        bearing = pi / 2.0;
                else    (Xh == 0 && Yh > 0)
                        bearing = pi * 1.5;
                end
               
bearing=bearing*57.296

if az<0
    bearing=abs(bearing-360);
end

User avatar
Patrick
Lead Developer
Posts: 3099
Joined: Mon Jun 20, 2005 8:46 am
Location: Canada
Contact:

Re: Bearing in MatLAB

Postby Patrick » Mon Sep 26, 2011 12:50 pm

Look a little more closely at the example. You have some signs wrong in your matrices, and some values mixed up in your vectors.

-Patrick


Return to “Matlab”

Who is online

Users browsing this forum: No registered users and 2 guests