DWORD WINAPI SensorFusionProc(LPVOID lpParam)
{
#define MAGNET_VELOCITY 1
float dT = 0.05f, fs = 1.0f/dT, mfs = fs;
float ax, ay, az;
float mx, my, mz;
float gx, gy, gz;
#ifndef MAGNET_VELOCITY
Rotation *rotation = NULL;
ISTBOOL rotation_done = ISTFALSE;
#endif
Magnet *magnet = NULL;
ISTBOOL magnet_done = ISTFALSE;
ISTFLOAT ws[3] = {0};
ISTFLOAT gs[3] = {0};
ISTFLOAT gdata[3];
ISTFLOAT mdata[3];
CSensorBase *accel_sensor = NULL;
CSensorBase *magnet_sensor = NULL;
CSensorBase *gyro_sensor = NULL;
AccelerationCalibration *accel_cal = NULL;
AcceleratorMeasurement cx, cy, cz;
float g;
float tmA, tmM;
DWORD dwRet, dwCnt, dwStart, dwEnd, dwTimeout;
DWORD samples = (DWORD)(fs/mfs);
DWORD period = (DWORD)(1000.0f/fs);
HANDLE event = NULL;
IST_DBG("+++ Enter SensorFusionProc() +++\n");
FreezeGUI(false);
accel_sensor = CreateSensor(DEV_ID_KIONIX, fs, g_pMain->m_iSenI2c);
if (!accel_sensor) {
IST_DBG("!accel_sensor\n");
goto EXIT;
}
magnet_sensor = CreateSensor(DEV_ID_ISENTEK_8303, mfs, g_pMain->m_iSenI2c);
if (!magnet_sensor) {
IST_DBG("!magnet_sensor\n");
goto EXIT;
}
gyro_sensor = CreateSensor(DEV_ID_LG3GYRO, fs, g_pMain->m_iSenI2c);
if (!gyro_sensor) {
IST_DBG("!gyro_sensor\n");
goto EXIT;
}
magnet = IST_Magnet()->New();
if (!magnet) {
IST_DBG("!magnet\n");
goto EXIT;
}
#ifndef MAGNET_VELOCITY
rotation = IST_Rotation()->New();
if (!rotation) {
IST_DBG("!rotation\n");
goto EXIT;
}
#endif
#define GRAVITY_SENSOR_POSITIVE_X_OFFSET 12.600f
#define GRAVITY_SENSOR_NEGATIVE_X_OFFSET -8.450f
#define GRAVITY_SENSOR_ZERO_X_OFFSET 1.838f
#define GRAVITY_SENSOR_POSITIVE_Y_OFFSET 11.350f
#define GRAVITY_SENSOR_NEGATIVE_Y_OFFSET -9.365f
#define GRAVITY_SENSOR_ZERO_Y_OFFSET 0.981f
#define GRAVITY_SENSOR_POSITIVE_Z_OFFSET 14.920f
#define GRAVITY_SENSOR_NEGATIVE_Z_OFFSET -7.000f
#define GRAVITY_SENSOR_ZERO_Z_OFFSET 3.154f
#define GRAVITY_SENSOR_G 9.81f
g = GRAVITY_SENSOR_G;
cx.at_positive_gravity = GRAVITY_SENSOR_POSITIVE_X_OFFSET;
cx.at_negative_gravity = GRAVITY_SENSOR_NEGATIVE_X_OFFSET;
cx.at_zero_gravity = GRAVITY_SENSOR_ZERO_X_OFFSET;
cy.at_positive_gravity = GRAVITY_SENSOR_POSITIVE_Y_OFFSET;
cy.at_negative_gravity = GRAVITY_SENSOR_NEGATIVE_Y_OFFSET;
cy.at_zero_gravity = GRAVITY_SENSOR_ZERO_Y_OFFSET;
cz.at_positive_gravity = GRAVITY_SENSOR_POSITIVE_Z_OFFSET;
cz.at_negative_gravity = GRAVITY_SENSOR_NEGATIVE_Z_OFFSET;
cz.at_zero_gravity = GRAVITY_SENSOR_ZERO_Z_OFFSET;
accel_cal = CAccelerationCalibration()->New(g, cx, cy, cz);
if (!accel_cal) {
IST_DBG("!accel_cal\n");
goto EXIT;
}
event = CreateEvent(NULL, TRUE, FALSE, NULL);
if (!event) {
goto EXIT;
}
dwCnt = 0;
tmM = tmA = GetCurrentSeconds();
dwTimeout = period;
while (!g_pMain->bStopSignal) {
xIST_DBG("dwTimeout = %u ms\n", dwTimeout);
dwRet = WaitForSingleObject(event, dwTimeout);
if (WAIT_TIMEOUT != dwRet) {
xIST_DBG("dwRet = %u\n", dwRet);
g_pMain->bStopSignal = true;
continue;
}
dwStart = GetTickCount();
/* get real gyroscope data */
if (FALSE == ReadSensorData(gyro_sensor, &gx, &gy, &gz, CSensorBase::GYRO_UNITS::CONFIG_GYRO_UNITS_RAD)){
g_pMain->bStopSignal = true;
continue;
}
gs[0] = FloatType(gx);
gs[1] = FloatType(gy);
gs[2] = FloatType(gz);
/* Get accelerator data */
if (FALSE == ReadSensorData(accel_sensor, &ax, &ay, &az, CSensorBase::ACCEL_UNITS::CONFIG_ACCEL_UNITS_MPSS)){
g_pMain->bStopSignal = true;
continue;
}
if (accel_cal->Process(accel_cal, ax, ay, az, dT)) {
accel_cal->GetData(accel_cal, &ax, &ay, &az);
}
gdata[0] = FloatType(ax);
gdata[1] = FloatType(ay);
gdata[2] = FloatType(az);
xIST_DBG("a[%14.10f,%14.10f,%14.10f]\n", ax, ay, az);
/* Get magnet field data */
if (FALSE == ReadSensorData(magnet_sensor, &mx, &my, &mz, CSensorBase::MAG_UNITS::CONFIG_MAG_UNITS_UT)){
g_pMain->bStopSignal = true;
continue;
}
dT = EllapsedSeconds(&tmM);
mdata[0] = FloatType(mx);
mdata[1] = FloatType(my);
mdata[2] = FloatType(mz);
xIST_DBG("m[%14.10f,%14.10f,%14.10f]\n", mx, my, mz);
#ifndef MAGNET_VELOCITY
rotation_done = rotation->Process(rotation, mdata, gdata, FloatType(dT));
#endif
magnet_done = magnet->Process(magnet, mdata, FloatType(dT));
#ifndef MAGNET_VELOCITY
/* check valid */
xIST_DBG("rotation_done:%s\n", rotation_done ? "YES" : "NO");
#endif
xIST_DBG("magnet_done:%s\n", magnet_done ? "YES" : "NO");
#ifndef MAGNET_VELOCITY
if (rotation_done && magnet_done) {
#else
if (magnet_done) {
#endif
CString tmpStr;
if (g_pMain->GetCurrentPage() == CiSensorsDlg::GYROSCOPE_DATA) {
xIST_DBG("update gyro\n");
#ifdef MAGNET_VELOCITY
ws[0] = magnet->Velocity[0];
ws[1] = magnet->Velocity[1];
ws[2] = magnet->Velocity[2];
#else
ws[0] = rotation->Velocity[0];
ws[1] = rotation->Velocity[1];
ws[2] = rotation->Velocity[2];
#endif
xIST_DBG("%14.10f,%14.10f,%14.10f,%14.10f,%14.10f,%14.10f\n", gx, gy, gz,
(float)NativeType(ws[0]), (float)NativeType(ws[1]), (float)NativeType(ws[2]));
for (int i = 0; i < 3; ++i) {
tmpStr.Format(_T("%0.2f"), (float)NativeType(ws[i]));
g_pMain->m_SubDlgGyroscopeData.GetDlgItem(IDC_STATIC_EGYROSCOPE_X + i)->SetWindowTextW(tmpStr);
tmpStr.Format(_T("%0.2f"), (float)NativeType(gs[i]));
g_pMain->m_SubDlgGyroscopeData.GetDlgItem(IDC_STATIC_GYROSCOPE_X + i)->SetWindowTextW(tmpStr);
g_pMain->m_SubDlgGyroscopeData.m_GyroWaveform[i].SetData(
(float)NativeType(gs[0]), (float)NativeType(gs[1]), (float)NativeType(gs[2]));
g_pMain->m_SubDlgGyroscopeData.m_SoftGyroWaveform[i].SetData(
(float)NativeType(ws[0]), (float)NativeType(ws[1]), (float)NativeType(ws[2]));
}
}
}
dwEnd = GetTickCount();
if (period > (dwEnd - dwStart))
dwTimeout = period - (dwEnd - dwStart);
else
dwTimeout = 0;
}
EXIT:
if (event) {
CloseHandle(event);
event = NULL;
}
#ifndef MAGNET_VELOCITY
if (rotation) {
IST_Rotation()->Delete(rotation);
rotation = NULL;
}
#endif
if (magnet) {
IST_Magnet()->Delete(magnet);
magnet = NULL;
}
DestroySensor(&gyro_sensor);
DestroySensor(&magnet_sensor);
DestroySensor(&accel_sensor);
FreezeGUI(true);
IST_DBG("--- Leave SensorFusionProc() ---\n");
ExitThread(0);
}
// public object GetSeconds(Fuse.Scripting.Context c, object[] args) [instance] :110
uObject* Stopwatch::GetSeconds(::g::Fuse::Scripting::Context* c, uArray* args)
{
uObject* ret = uBox(::g::Uno::Double_typeof(), EllapsedSeconds());
return ret;
}
