void main(void)
{
// USER CODE BEGIN (Main,2)
signed int ret_left;
signed int ret_right;
volatile signed int gyrovalue;
unsigned char ledvalue;
volatile signed int winkel = 0.0;
volatile signed int val_x;
volatile signed int val_y;
volatile signed int val_z;
unsigned char counter = 0;
unsigned char timebase = 20; // * 5 ms
// USER CODE END
MAIN_vInit();
// USER CODE BEGIN (Main,3)
BalanceInit();
// USER CODE END
while(1)
{
// USER CODE BEGIN (Main,4)
while(0 == timerevent);
timerevent = 0;
counter--;
if (counter == 0)
{
counter = timebase;
ReadSensorData();
Balance();
// start reading the data, they will be get then
gyrovalue = ReadSpinValue();
winkel = GetCurrentAngle();
ledvalue = ReadSpinValueRaw();
val_x = ReadAccelValue(DIRECTION_X) + 512;
val_y = ReadAccelValue(DIRECTION_Y) + 512;
val_z = ReadAccelValue(DIRECTION_Z) + 512;
P10_OUT_P7 = (gyrovalue > 35) ? 0 : 1;
P10_OUT_P6 = (gyrovalue > 25) ? 0 : 1;
P10_OUT_P5 = (gyrovalue > 15) ? 0 : 1;
P10_OUT_P4 = (gyrovalue > 5) ? 0 : 1;
P10_OUT_P3 = (gyrovalue < -5) ? 0 : 1;
P10_OUT_P2 = (gyrovalue < -15) ? 0 : 1;
P10_OUT_P1 = (gyrovalue < -25) ? 0 : 1;
P10_OUT_P0 = (gyrovalue < -35) ? 0 : 1;
// do some kind of stuff
//ret_left = 00;
//ret_right = 00;
//SetMotorSpeeds(&ret_left, &ret_right);
//SetMotorSpeedsNoReturn(links_p, rechts_p);
// faehrt vorwärts hoffentlich
// und au langsam
//SetMotorSpeedsNoReturn(speed, speed);//20, 20);
}
// USER CODE END
}
} // End of function main
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);
}
