public SensorsData(IMUData imu0, IMUData imu1, IMUData imu2, IMUData imu3)
{
this.imu0 = new SampleIMUData(imu0);
this.imu1 = new SampleIMUData(imu1);
this.imu2 = new SampleIMUData(imu2);
this.imu3 = new SampleIMUData(imu3);
}
public void addAllData(IMUData imu0, IMUData imu1, IMUData imu2, IMUData imu3)
{
this.imu0 = new SampleIMUData(imu0);
this.imu1 = new SampleIMUData(imu1);
this.imu2 = new SampleIMUData(imu2);
this.imu3 = new SampleIMUData(imu3);
}
public SampleIMUData(IMUData imudata)
{
this.accx = imudata.accx;
this.accy = imudata.accy;
this.accz = imudata.accz;
this.gysx = imudata.gysx;
this.gysy = imudata.gysy;
this.gysz = imudata.gysz;
this.mgtx = imudata.mgtx;
this.mgty = imudata.mgty;
this.mgtz = imudata.mgtz;
this.roll = imudata.eulerx;
this.pitch = imudata.eulery;
this.yaw = imudata.eulerz;
}
public void Copy(IMUData c_imuData)
{
this.raw_accx = c_imuData.raw_accx;
this.raw_accy = c_imuData.raw_accy;
this.raw_accz = c_imuData.raw_accz;
this.accx = c_imuData.accx;
this.accy = c_imuData.accy;
this.accz = c_imuData.accz;
this.raw_gysx = c_imuData.raw_gysx;
this.raw_gysy = c_imuData.raw_gysy;
this.raw_gysz = c_imuData.raw_gysz;
this.gysx = c_imuData.gysx;
this.gysy = c_imuData.gysy;
this.gysz = c_imuData.gysz;
this.raw_mgtx = c_imuData.raw_mgtx;
this.raw_mgty = c_imuData.raw_mgty;
this.raw_mgtz = c_imuData.raw_mgtz;
this.mgtx = c_imuData.mgtx;
this.mgty = c_imuData.mgty;
this.mgtz = c_imuData.mgtz;
this.eulerx = c_imuData.eulerx;
this.eulery = c_imuData.eulery;
this.eulerz = c_imuData.eulerz;
this.quatx = c_imuData.quatx;
this.quaty = c_imuData.quaty;
this.quatz = c_imuData.quatz;
this.isNull = c_imuData.isNotSet();
}
public static void transferDevice4_Data(IMUData imuData)
{
MainForm.imuData_Device4.Copy(imuData);
}
// Non-static method
public void sensorEventThread()
{
try
{
OpenZen.ZenInit(device_Handle);
ZenEvent zenEvent = new ZenEvent();
Boolean connected = false;
ZenSensorInitError sensorInitError = ZenSensorInitError.ZenSensorInitError_Max;
// try three connection attempts
for (int i = 0; i < 1; i++)
{
sensorInitError = OpenZen.ZenObtainSensor(device_Handle, mSensor, device_Sensor);
if (sensorInitError == ZenSensorInitError.ZenSensorInitError_None)
{
connected = true;
// Enable Disconnect Button
MainForm mainForm = FormProvider.getMain();
switch (device_No)
{
case 1:
if (mainForm.InvokeRequired)
{
mainForm.BeginInvoke((Action) delegate() {
mainForm.device_List1.Enabled = false;
mainForm.disconnect_Device1.Visible = true;
mainForm.connect_Device1.Enabled = false;
});
}
else
{
mainForm.device_List1.Enabled = false;
mainForm.disconnect_Device1.Visible = true;
mainForm.connect_Device1.Enabled = false;
}
break;
case 2:
if (mainForm.InvokeRequired)
{
mainForm.BeginInvoke((Action) delegate() {
mainForm.device_List2.Enabled = false;
mainForm.disconnect_Device2.Visible = true;
mainForm.connect_Device2.Enabled = false;
});
}
else
{
mainForm.device_List2.Enabled = false;
mainForm.disconnect_Device2.Visible = true;
mainForm.connect_Device2.Enabled = false;
}
break;
case 3:
if (mainForm.InvokeRequired)
{
mainForm.BeginInvoke((Action) delegate() {
mainForm.device_List3.Enabled = false;
mainForm.disconnect_Device3.Visible = true;
mainForm.connect_Device3.Enabled = false;
});
}
else
{
mainForm.device_List3.Enabled = false;
mainForm.disconnect_Device3.Visible = true;
mainForm.connect_Device3.Enabled = false;
}
break;
case 4:
if (mainForm.InvokeRequired)
{
mainForm.BeginInvoke((Action) delegate() {
mainForm.device_List4.Enabled = false;
mainForm.disconnect_Device4.Visible = true;
mainForm.connect_Device4.Enabled = false;
});
}
else
{
mainForm.device_List4.Enabled = false;
mainForm.disconnect_Device4.Visible = true;
mainForm.connect_Device4.Enabled = false;
}
break;
}
MessageBox.Show("Succesfully connected to sensor for Device #" + device_No.ToString());
break;
}
else
{
MessageBox.Show("Failed");
}
}
while (connected)
{
//Console.WriteLine(mZenHandle);
if (OpenZen.ZenWaitForNextEvent(device_Handle, zenEvent))
{
if (zenEvent.component.handle != 0)
{
switch (zenEvent.eventType)
{
case (int)ZenImuEvent.ZenImuEvent_Sample:
count++;
// read raw accelerometer
OpenZenFloatArray raw_fa = OpenZenFloatArray.frompointer(zenEvent.data.imuData.aRaw);
// read calibrated accelerometer
OpenZenFloatArray fa = OpenZenFloatArray.frompointer(zenEvent.data.imuData.a);
// read raw accelerometer
OpenZenFloatArray raw_fg = OpenZenFloatArray.frompointer(zenEvent.data.imuData.gRaw);
// read calibrated gyroscope
OpenZenFloatArray fg = OpenZenFloatArray.frompointer(zenEvent.data.imuData.g);
// read raw magnetometer
OpenZenFloatArray raw_fb = OpenZenFloatArray.frompointer(zenEvent.data.imuData.bRaw);
// read calibrated magnetometer
OpenZenFloatArray fb = OpenZenFloatArray.frompointer(zenEvent.data.imuData.b);
// read euler angle
OpenZenFloatArray fr = OpenZenFloatArray.frompointer(zenEvent.data.imuData.r);
// read quaternion
OpenZenFloatArray fq = OpenZenFloatArray.frompointer(zenEvent.data.imuData.q);
// Create new imuData object
IMUData imuData = new IMUData(
raw_fa.getitem(0), raw_fa.getitem(1), raw_fa.getitem(2),
fa.getitem(0), fa.getitem(1), fa.getitem(2),
raw_fg.getitem(0), raw_fg.getitem(1), raw_fg.getitem(2),
fg.getitem(0), fg.getitem(1), fg.getitem(2),
raw_fb.getitem(0), raw_fb.getitem(1), raw_fb.getitem(2),
fb.getitem(0), fb.getitem(1), fb.getitem(2),
fr.getitem(0), fr.getitem(1), fr.getitem(2),
fq.getitem(0), fq.getitem(1), fq.getitem(2)
); // Push data in to the new imuData Object
switch (device_No)
{
case (1):
transferDevice1_Data(imuData);
//Debug.WriteLine(count);
break;
case (2):
transferDevice2_Data(imuData);
break;
case (3):
transferDevice3_Data(imuData);
break;
case (4):
transferDevice4_Data(imuData);
Debug.WriteLine(count);
break;
}
break;
}
}
}
}
}
catch (ThreadAbortException)
{
}
}
