void OnDestroy()
{
Debug.Log("PrintOnDestroy");
LpSensorManager.disconnectLpms(lpmsSensor);
// Destroy sensor manager and free up memory
LpSensorManager.deinitSensorManager();
}
// Update is called once per frame
void Update()
{
if (LpSensorManager.getConnectionStatus(lpmsSensor) == LpSensorManager.SENSOR_CONNECTION_CONNECTED)
{
SensorData sd;
Vector3 current_rot = transform.rotation.eulerAngles;
unsafe
{
sd = *((SensorData *)LpSensorManager.getSensorData(lpmsSensor));
}
if (Input.GetButtonDown("Fire1"))
{
tracking = !tracking;
Debug.Log("Tracking:" + tracking);
last_sensor = new Vector3(-sd.ry, -sd.rz, sd.rx);
//transform.rotation = virtualcube.transform.rotation;
}
//Debug.Log("Timestamp: " + sd.timeStamp + " q: " + sd.qw + " " + sd.qx + " " + sd.qy + " " + sd.qz);
//Quaternion q = new Quaternion(sd.qx - offset.x, sd.qz - offset.y, sd.qy - offset.z, sd.qw - offset.w);
if (tracking)
{
current_sensor = new Vector3(-sd.ry, -sd.rz, sd.rx);
//Debug.Log("OFFSET:" + offset);
}
offset = current_sensor - last_sensor;
transform.rotation = Quaternion.Euler(current_rot + offset);
last_sensor = current_sensor;
if (Input.GetKeyDown("space"))
{
transform.rotation = new Quaternion(0, 0, 0, 0);
//transform.rotation = virtualcube.transform.rotation;
}
}
}
// Start is called before the first frame update
void Start()
{
Application.targetFrameRate = 20;
// Initialize sensor manager
LpSensorManager.initSensorManager();
// connects to sensor
LpSensorManager.connectToLpms(LpSensorManager.DEVICE_LPMS_B2, lpmsSensor);
// Wait for establishment of sensor1 connection
while (LpSensorManager.getConnectionStatus(lpmsSensor) != 1)
{
//System.Threading.Thread.Sleep(100);
}
Debug.Log("Sensor connected");
// Sets sensor offset
LpSensorManager.setOrientationOffset(lpmsSensor, LpSensorManager.LPMS_OFFSET_MODE_HEADING);
Debug.Log("Offset set");
tracking = false;
if (LpSensorManager.getConnectionStatus(lpmsSensor) == LpSensorManager.SENSOR_CONNECTION_CONNECTED)
{
SensorData sd;
unsafe
{
sd = *((SensorData *)LpSensorManager.getSensorData(lpmsSensor));
}
last_sensor = new Vector3(0, 0, 0);
current_sensor = last_sensor;
}
}
// Update is called once per frame
void Update()
{
if (LpSensorManager.getConnectionStatus(lpmsSensor) == LpSensorManager.SENSOR_CONNECTION_CONNECTED)
{
SensorData sd;
Vector3 current_rot = transform.rotation.eulerAngles;
unsafe
{
sd = *((SensorData *)LpSensorManager.getSensorData(lpmsSensor));
}
this.transform.rotation = new Quaternion(sd.qx, sd.qz, sd.qy, sd.qw);
if (first_time)
{
created = (GameObject)Instantiate(cube);
created.transform.rotation = Quaternion.Euler(new Vector3(0, 0, 0));
//created.GetComponent<MeshRenderer>().enabled = false;
created.transform.parent = this.transform;
last_sensor = created.transform.rotation.eulerAngles;
first_time = !first_time;
}
//Debug.Log("Timestamp: " + sd.timeStamp + " q: " + sd.qw + " " + sd.qx + " " + sd.qy + " " + sd.qz);
//Quaternion q = new Quaternion(sd.qx - offset.x, sd.qz - offset.y, sd.qy - offset.z, sd.qw - offset.w);
current_sensor = created.transform.rotation.eulerAngles;
error = current_sensor - last_sensor;
datamanager.GetComponent <DataManager>().set_Iniertial(this.transform.localRotation);
last_sensor = current_sensor;
}
}
// Start is called before the first frame update
void Start()
{
//default_rot = new Vector3((float)0.01, (float)0.01, (float)0.01);
//transform.rotation = Quaternion.Euler(default_rot);
Application.targetFrameRate = 20;
// Initialize sensor manager
LpSensorManager.initSensorManager();
// connects to sensor
LpSensorManager.connectToLpms(LpSensorManager.DEVICE_LPMS_B2, lpmsSensor);
// Wait for establishment of sensor1 connection
while (LpSensorManager.getConnectionStatus(lpmsSensor) != 1)
{
//System.Threading.Thread.Sleep(100);
}
if (LpSensorManager.getConnectionStatus(lpmsSensor) == 1)
{
Debug.Log("Sensor connected");
}
// Sets sensor offset
LpSensorManager.setOrientationOffset(lpmsSensor, LpSensorManager.LPMS_OFFSET_MODE_HEADING);
first_time = true;
last_sensor = new Vector3(0, 0, 0);
current_sensor = last_sensor;
}
// Update is called once per frame
void Update()
{
if (LpSensorManager.getConnectionStatus(lpmsSensor) == LpSensorManager.SENSOR_CONNECTION_CONNECTED)
{
SensorData sd;
unsafe
{
sd = *((SensorData *)LpSensorManager.getSensorData(lpmsSensor));
}
//Debug.Log("Timestamp: " + sd.timeStamp + " q: " + sd.qw + " " + sd.qx + " " + sd.qy + " " + sd.qz);
Quaternion q = new Quaternion(sd.qx, sd.qz, sd.qy, sd.qw);
transform.rotation = q;
if (dm != null)
{
dm.GetComponent <DataManager>().set_Iniertial(this.transform.rotation);
}
}
}
// Use this for initialization
void Start()
{
offset = new Quaternion(0, 0, 0, 0);
// Initialize sensor manager
LpSensorManager.initSensorManager();
// connects to sensor
LpSensorManager.connectToLpms(LpSensorManager.DEVICE_LPMS_B2, lpmsSensor);
// Wait for establishment of sensor1 connection
while (LpSensorManager.getConnectionStatus(lpmsSensor) != 1)
{
//System.Threading.Thread.Sleep(100);
}
Debug.Log("Sensor connected");
// Sets sensor offset
LpSensorManager.setOrientationOffset(lpmsSensor, LpSensorManager.LPMS_OFFSET_MODE_HEADING);
Debug.Log("Offset set");
}
// Update is called once per frame
void Update()
{
if (Input.GetButtonDown("Fire1"))
{
Debug.Log("Doing The Calibration");
offset = transform.rotation;
offsetEuler = offset.eulerAngles;
Debug.Log("OFFSET: X:" + offsetEuler.x + "Y:" + offsetEuler.z + "Z:" + offsetEuler.y);
}
if (LpSensorManager.getConnectionStatus(lpmsSensor) == LpSensorManager.SENSOR_CONNECTION_CONNECTED)
{
SensorData sd;
unsafe
{
sd = *((SensorData *)LpSensorManager.getSensorData(lpmsSensor));
}
//Debug.Log("Timestamp: " + sd.timeStamp + " q: " + sd.qw + " " + sd.qx + " " + sd.qy + " " + sd.qz);
//Quaternion q = new Quaternion(sd.qx - offset.x, sd.qz - offset.y, sd.qy - offset.z, sd.qw - offset.w);
Quaternion q = new Quaternion(sd.qx, sd.qz, sd.qy, sd.qw);
}
}
//初始化设备
public bool InitDevice()
{
// Get the KinectManager instance
if (kinectManager == null)
{
kinectManager = KinectManager.Instance;
}
if (gloveManager == null)
{
gloveManager = DTGloveManager.instance;
}
if (lpSensorManager == null)
{
lpSensorManager = new LpSensorManager(ref test_data, ref cur_ModelCtrlData.wristCtrlData, 23333, "00:04:3E:9E:00:C5", 20);
if (lpSensorManager.Init())
{
lpThread = new Thread(new ThreadStart(lpSensorManager.receiveData));
lpThread.Start();
}
}
if (!KinectManager.IsKinectInitialized())
{
Debug.Log("Kinect 未初始化");
return(false);
}
if (!DTGloveManager.instance.glove.IsOpen())
{
Debug.Log("5DT 未初始化");
return(false);
}
return(true);
}
// Use this for initialization
void Start()
{
// Initialize sensor manager
dm = GameObject.Find("DataManager");
if (dm != null)
{
Debug.Log("DataManager found!\n");
}
LpSensorManager.initSensorManager();
// connects to sensor
LpSensorManager.connectToLpms(LpSensorManager.DEVICE_LPMS_B2, lpmsSensor);
// Wait for establishment of sensor1 connection
while (LpSensorManager.getConnectionStatus(lpmsSensor) != 1)
{
System.Threading.Thread.Sleep(100);
}
Debug.Log("Sensor connected");
// Sets sensor offset
LpSensorManager.setOrientationOffset(lpmsSensor, LpSensorManager.LPMS_OFFSET_MODE_HEADING);
Debug.Log("Offset set");
}
