// Use this for initialization
void Awake()
{
if (phidgetsAttached)
{
if (SystemInfo.deviceType == DeviceType.Handheld)
{
return;
}
spatial = new Spatial();
spatial.open();
spatial.waitForAttachment(1000);
spatial.DataRate = 32;
//spatial.zeroGyro();
//These constants are geographic dependent --- do not change
spatial.setCompassCorrectionParameters(0.64473, 0.04197, -0.01880, -0.01187, 1.51858, 1.51659, 1.61795, 0.00715, 0.00798, 0.00675, -0.01492, 0.00817, -0.01580);
spatial.SpatialData += new SpatialDataEventHandler(spatial_SpatialData);
rotation = Quaternion.identity;
timestampLastUpdate = DateTime.MinValue;
IMUangles = new float[3];
for (int i = 0; i < 3; i++)
{
IMUangles[i] = 0;
}
}
// AHRS init
mMatrix = Matrix4x4.identity;
rMatrix = Matrix4x4.identity;
mTcAcc = Vector3.zero;
mLinAcc = Vector3.zero;
mLinVel = Vector3.zero;
mLinVelPrev = Vector3.zero;
mLinPos = Vector3.zero;
mLinPosPrev = Vector3.zero;
mSamplePeriod = 0.03125;
xValuesVel = new List <Vector3>(); yValuesVel = new List <Vector3>();
xValuesPos = new List <Vector3>(); yValuesPos = new List <Vector3>();
xValuesAcc = new List <Vector3>(); yValuesAcc = new List <Vector3>();
xValuesGyr = new List <Vector3>(); yValuesGyr = new List <Vector3>();
mButterHP = new ButterWorthFilter.ButterWorth(ButterWorthFilter.ButterWorth.BandType.high);
mButterLP = new ButterWorthFilter.ButterWorth(ButterWorthFilter.ButterWorth.BandType.low);
//Debug.Log("Max " + spatial.DataRateMax);
//Debug.Log("Min " + spatial.DataRateMin);
pos = Vector3.zero;
accelBeforeFilter = new StreamWriter("c:/Users/Juan/Desktop/accelbeforefilter32ms.txt", true);
accelAfterFilter = new StreamWriter("c:/Users/Juan/Desktop/accelafterfilter32ms.txt", true);
velocityBeforeFilter = new StreamWriter("c:/Users/Juan/Desktop/velBefore32ms.txt", true);
velocityAfterFilter = new StreamWriter("c:/Users/Juan/Desktop/velAfter32ms.txt", true);
position = new StreamWriter("c:/Users/Juan/Desktop/pos32ms.txt", true);
}
// Use this for initialization
void Awake()
{
if (phidgetsAttached)
{
if (SystemInfo.deviceType == DeviceType.Handheld)
{
return;
}
spatial = new Spatial();
spatial.open();
spatial.waitForAttachment(1000);
spatial.DataRate = 64;
spatial.zeroGyro();
//These constants are geographic dependent --- do not change
spatial.setCompassCorrectionParameters(0.64473, 0.04197, -0.01880, -0.01187, 1.51858, 1.51659, 1.61795, 0.00715, 0.00798, 0.00675, -0.01492, 0.00817, -0.01580);
spatial.SpatialData += new SpatialDataEventHandler(spatial_SpatialData);
rotation = Quaternion.identity;
timestampLastUpdate = DateTime.MinValue;
IMUangles = new float[3];
for (int i = 0; i < 3; i++)
{
IMUangles[i] = 0;
}
}
mMatrix = Matrix4x4.identity;
rMatrix = Matrix4x4.identity;
mTcAcc = Vector3.zero;
mLinAcc = Vector3.zero;
mLinVel = Vector3.zero;
mLinVelPrev = Vector3.zero;
mLinPos = Vector3.zero;
mLinPosPrev = Vector3.zero;
mSamplePeriod = 0.004;
xValuesVel = new List <Vector3>(); yValuesVel = new List <Vector3>();
xValuesPos = new List <Vector3>(); yValuesPos = new List <Vector3>();
xValuesAcc = new List <Vector3>(); yValuesAcc = new List <Vector3>();
xValuesGyr = new List <Vector3>(); yValuesGyr = new List <Vector3>();
mButterHP = new ButterWorthFilter.ButterWorth(ButterWorthFilter.ButterWorth.BandType.high);
mButterLP = new ButterWorthFilter.ButterWorth(ButterWorthFilter.ButterWorth.BandType.low);
pos = Vector3.zero;
}
