myvtm/Module/mod_sensors/SensorsFSM.cpp

#include "stdafx.h"
#include "SensorsFSM.h"
#include "EventCode.h"

const DWORD HEALTHMANAGER_TIMER_ID = 1;
const DWORD HEALTHMANAGER_WD_TIMER_ID = 2;
const DWORD HEALTHMANAGER_TIMER_INTERVAL = 1000;
ErrorCodeEnum CSensorsFSM::OnInit()
{
	LOG_FUNCTION();
	LoadGnssVendorDLL();
	return Error_Succeed;

	// Create a COM interface to the SensorManager object.
	HRESULT hr = CoInitialize(NULL);
	hr = ::CoCreateInstance(CLSID_SensorManager, NULL, CLSCTX_INPROC_SERVER, 
		IID_PPV_ARGS(&m_pSensorManager));
	long tmp = hr;
	if (FAILED(hr))
	{
		Dbg("create SensorManager failed,%d.",hr);
		return Error_Resource;
	}
	m_mapGUID[AMBIENT_LIGHT] = SENSOR_TYPE_AMBIENT_LIGHT;
	m_mapGUID[GYROMETER_3D] = SENSOR_TYPE_GYROMETER_3D;
	m_mapGUID[COMPASS_3D] = SENSOR_TYPE_COMPASS_3D;
	m_mapGUID[ACCELEROMETER_3D] = SENSOR_TYPE_ACCELEROMETER_3D;
	m_mapGUID[INCLINOMETER_3D] = SENSOR_TYPE_INCLINOMETER_3D;

	SensorValue sValue = {0,0,0,0};
	m_mapLastValue[AMBIENT_LIGHT] = sValue;
	m_mapLastValue[GYROMETER_3D] = sValue;
	m_mapLastValue[COMPASS_3D] = sValue;
	m_mapLastValue[ACCELEROMETER_3D] = sValue;
	m_mapLastValue[INCLINOMETER_3D] = sValue;
	return Error_Succeed;
}
ErrorCodeEnum CSensorsFSM::OnExit()
{
	LOG_FUNCTION();
	if (m_pLightSensor != NULL)
		m_pLightSensor->SetEventSink(NULL);
	return Error_Succeed;
}
void CSensorsFSM::s0_on_entry()
{
	LOG_FUNCTION();
	//FSMEvent *pEvt =  new FSMEvent(USER_EVT_INIT);
	//PostEventFIFO(pEvt);
}
unsigned int CSensorsFSM::s0_on_event(FSMEvent *pEvt)
{
	Dbg("s0 init evt %d",pEvt->iEvt);
	switch(pEvt->iEvt)
	{
	case USER_EVT_INIT:
		{
			pEvt->SetHandled();
			DoInitTask *pTask = new DoInitTask(this);
			GetEntityBase()->GetFunction()->PostEntityTaskFIFO(pTask);
			break;
		}
	case USER_EVT_INIT_FINISHED:
		pEvt->SetHandled();
		return pEvt->param1;
	default:
		break;
	}
	return 0;
}
void CSensorsFSM::s0_on_exit()
{

}
//normal
void CSensorsFSM::s1_on_entry()
{
	//oilyang@20200225 comment the following two line as no need for sensors now
	//DoWorkTask *pTask = new DoWorkTask(this);
	//GetEntityBase()->GetFunction()->PostEntityTaskFIFO(pTask);
}
unsigned int CSensorsFSM::s1_on_event(FSMEvent *pEvt)
{
	Dbg("s1 normal evt %d",pEvt->iEvt);
	switch(pEvt->iEvt)
	{
	case USER_EVT_WORK_FINISHED:
		break;
	default:
		break;
	}
	return 0;
}
void CSensorsFSM::s1_on_exit()
{

}
//failed
void CSensorsFSM::s2_on_entry()
{

}
unsigned int CSensorsFSM::s2_on_event(FSMEvent *pEvt)
{
	return 0;
}
void CSensorsFSM::s2_on_exit()
{

}
//to get all sensors on the device
int CSensorsFSM::DoInit()
{
	if (!m_pSensorManager)
	{
		Dbg("initialize first.");
		return -1;
	}
	ISensorCollection* pSensorCollection = NULL;
	HRESULT hr = m_pSensorManager->GetSensorsByCategory(SENSOR_CATEGORY_ALL, &pSensorCollection);
	if (FAILED(hr))
	{
		Dbg("Unable to find any sensor on the computer.");
		return -1;
	}
	ULONG ulCount = 0;
	hr = pSensorCollection->GetCount(&ulCount);
	if (FAILED(hr) || ulCount < 1)
	{
		Dbg("no sensor");
		return -1;
	}
	//MessageBox(0,0,0,0);
	for (int i = 0; i < ulCount; ++i)
	{
		ISensor *pSensor;
		Dbg("i %d",i);
		hr = pSensorCollection->GetAt(i,&pSensor);
		if (SUCCEEDED(hr))
		{
			SENSOR_TYPE_ID id;
			hr = pSensor->GetType(&id);
			ISensorDataReport *pData;
			PROPVARIANT var;
			if (IsEqualGUID(id,m_mapGUID[AMBIENT_LIGHT]))
			{
				Dbg("is light");
				m_pLightSensor = pSensor;
				m_dwSensorsHave |= AMBIENT_LIGHT;
				//m_pLightEvtClass = new MyEvents();
				//hr = m_pLightEvtClass->QueryInterface(IID_PPV_ARGS(&m_pLightEvents));
				//if (SUCCEEDED(hr))
				//{
				//	Dbg("to set light event");
				//	hr = m_pLightSensor->SetEventSink(m_pLightEvents);
				//	Dbg("set light event %d",hr);
					//GUID pguid[2];
					//pguid[0] = SENSOR_EVENT_DATA_UPDATED;
					//pguid[1] = SENSOR_EVENT_STATE_CHANGED;
					//Dbg("to set event interest");
					//hr = m_pLightSensor->SetEventInterest(pguid, 2);
					//Dbg("set event interest %d",hr);
					//GUID pguid[1];
					//pguid[0] = SENSOR_EVENT_DATA_UPDATED;
					//pguid[1] = SENSOR_EVENT_STATE_CHANGED;
					//Dbg("to set event interest");
					//hr = m_pLightSensor->SetEventInterest(pguid, 1);
					//Dbg("set event interest %d,%d",hr,GetLastError());
					//hr = SetCurrentReportInterval(m_pLightSensor,1000);
					//Dbg("set light interval %d",hr);
					//Sleep(10000);

				//}
			}
			else if (IsEqualGUID(id,m_mapGUID[GYROMETER_3D]))
			{
				Dbg("is gyro meter 3D");
				m_dwSensorsHave |= GYROMETER_3D;
				m_pGyroMeter3DSensor = pSensor;
				//m_pGyroMeter3DClass = new MyEvents(this);
				//hr = m_pGyroMeter3DClass->QueryInterface(IID_PPV_ARGS(&m_pGyroMeter3DEvents));
				//if (SUCCEEDED(hr))
				//{
				//	Dbg("to set gyro meter 3D event");
				//	hr = m_pGyroMeter3DSensor->SetEventSink(m_pGyroMeter3DEvents);
				//	Dbg("set gyro 3D event %d",hr);

				//	//hr = SetCurrentReportInterval(m_pGyroMeter3DSensor,1000);
				//	//Dbg("set gyro interval %d",hr);
				//}
			}
			else if (IsEqualGUID(id,m_mapGUID[COMPASS_3D]))
			{
				Dbg("is compass 3d.");
				m_dwSensorsHave |= COMPASS_3D;
				m_pCompass3DSensor = pSensor;
			}
			else if (IsEqualGUID(id,m_mapGUID[INCLINOMETER_3D]))
			{
				Dbg("is inclinometer 3D");
				m_dwSensorsHave |= INCLINOMETER_3D;
				m_pInclinoMeter3DSensor = pSensor;
				//m_pInclinoMeter3DClass = new MyEvents(this);
				//hr = m_pGyroMeter3DClass->QueryInterface(IID_PPV_ARGS(&m_pInclinoMeter3DEvents));
				//if (SUCCEEDED(hr))
				//{
				//	Dbg("to set inclinometer 3D event");
				//	hr = m_pInclinoMeter3DSensor->SetEventSink(m_pInclinoMeter3DEvents);
				//	Dbg("set inclinometer event %d",hr);

				//	//hr = SetCurrentReportInterval(m_pInclinoMeter3DSensor,1000);
				//	//Dbg("set inclinometer interval %d",hr);
				//}
			}
			else if (IsEqualGUID(id,m_mapGUID[ACCELEROMETER_3D]))
			{
				Dbg("is accelerometer 3D.");
				m_dwSensorsHave |= ACCELEROMETER_3D;
				m_pAcceleroMeter3DSensor = pSensor;
			}
			else 
				Dbg("sensor haven't be predefined? Er...");
		}
	}
	CSmartPointer<IConfigInfo> spConfig;
	ErrorCodeEnum err = GetEntityBase()->GetFunction()->OpenConfig(Config_Software, spConfig);
	if (err != Error_Succeed) {
		LOG_TRACE("open cfg file failed!");
		return 1;
	}
	spConfig->ReadConfigValueInt("init","LightVar",m_lightVar);
	spConfig->ReadConfigValueInt("init","CompassVar",m_compassVar);
	spConfig->ReadConfigValueInt("init","GyroVar",m_gyroVar);
	spConfig->ReadConfigValueInt("init","InclineVar",m_inclineVar);
	spConfig->ReadConfigValueInt("init","AcceleroVar",m_acceleroVar);
	Dbg("sensors:%d",m_dwSensorsHave);
	return 0;
}
//working
int CSensorsFSM::DoWork()
{
	ITimerListener *pListener = new TimerOutHelper<CSensorsFSM>(this, &CSensorsFSM::OnCheckTimeTimeout);
	Dbg("set timer");
	GetEntityBase()->GetFunction()->SetTimer(HEALTHMANAGER_TIMER_ID, pListener, HEALTHMANAGER_TIMER_INTERVAL);
	return 0;
}
void CSensorsFSM::OnCheckTimeTimeout()
{
	GetAmbientLightData();
	GetCompass3DData();
	GetGyroMeter3DData();
	GetAcceleroMeter3DData();
	GetInclinoMeter3DData();
	GetEntityBase()->GetFunction()->ResetTimer(HEALTHMANAGER_TIMER_ID, HEALTHMANAGER_TIMER_INTERVAL);
}
HRESULT CSensorsFSM::SetCurrentReportInterval(ISensor* pSensor, ULONG ulNewInterval)
{
	//assert(pSensor);

	HRESULT hr = S_OK;

	IPortableDeviceValues* pPropsToSet = NULL; // Input
	IPortableDeviceValues* pPropsReturn = NULL; // Output

	// Create the input object.
	hr = CoCreateInstance(__uuidof(PortableDeviceValues),
		NULL,
		CLSCTX_INPROC_SERVER,                           
		IID_PPV_ARGS(&pPropsToSet));

	if(SUCCEEDED(hr))
	{
		// Add the current report interval property.
		hr = pPropsToSet->SetUnsignedIntegerValue(SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL, ulNewInterval);
	}

	if(SUCCEEDED(hr))
	{
		// Only setting a single property, here.
		hr = pSensor->SetProperties(pPropsToSet, &pPropsReturn);
	}

	// Test for failure.
	if(hr == S_FALSE)
	{
		HRESULT hrError = S_OK;

		// Check results for failure.
		hr = pPropsReturn->GetErrorValue(SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL, &hrError);

		if(SUCCEEDED(hr))
		{
			// Print an error message.
			wprintf_s(L"\nSetting current report interval failed with error 0x%X\n", hrError);

			// Return the error code.
			hr = hrError;
		}
	}
	else if(hr == E_ACCESSDENIED)
	{
		// No permission. Take appropriate action.
	}

	//SafeRelease(&pPropsToSet);
	//SafeRelease(&pPropsReturn);

	return hr;
}


STDMETHODIMP MyEvents::OnDataUpdated(ISensor *pSensor,ISensorDataReport *pNewData)
{
	HRESULT hr = S_OK;

	if(NULL == pNewData ||
		NULL == pSensor)
	{
		return E_INVALIDARG;
	}
	SENSOR_TYPE_ID id;
	hr = pSensor->GetType(&id);
	ISensorDataReport *pData;
	PROPVARIANT var;
	Dbg("on data id %d",id.Data1);
	if (IsEqualGUID(id,m_mapGUID[AMBIENT_LIGHT]))
		OnAmbientLightUpdate(pSensor,pNewData);
	else if (IsEqualGUID(id,m_mapGUID[GYROMETER_3D]))
		OnGyroMeter3DUpdate(pSensor,pNewData);
	else if (IsEqualGUID(id,m_mapGUID[COMPASS_3D]))
		OnCompass3DUpdate(pSensor,pNewData);
	else if (IsEqualGUID(id,m_mapGUID[INCLINOMETER_3D]))
		OnInclinoMeter3DUpdate(pSensor,pNewData);
	else if (IsEqualGUID(id,m_mapGUID[ACCELEROMETER_3D]))
		OnAcceleroMeter3DUpdate(pSensor,pNewData);

	return hr;
}
void MyEvents::OnAmbientLightUpdate(ISensor *pSensor,ISensorDataReport *pNewData)
{
	PROPVARIANT var = {};
	HRESULT hr = pNewData->GetSensorValue(SENSOR_DATA_TYPE_LIGHT_LEVEL_LUX,&var);

	if (SUCCEEDED(hr))
	{
		if (var.vt == VT_R4)
		{
			Dbg("light %f",var.fltVal);
		}
	}
	PropVariantClear(&var);
}
void MyEvents::OnGyroMeter3DUpdate(ISensor *pSensor,ISensorDataReport *pNewData)
{
	PROPVARIANT var = {};
	HRESULT hr = pNewData->GetSensorValue(SENSOR_DATA_TYPE_ANGULAR_ACCELERATION_X_DEGREES_PER_SECOND_SQUARED,&var);

	if (SUCCEEDED(hr))
	{
		//Dbg("gyro meter 3d(X) var.vt %d",var.vt);
		if (var.vt == VT_R8)
		{
			Dbg("gyro(x) %f",var.fltVal);
		}
	}
	else
		Dbg("no gyro x");
	hr = pNewData->GetSensorValue(SENSOR_DATA_TYPE_ANGULAR_ACCELERATION_Y_DEGREES_PER_SECOND_SQUARED,&var);

	if (SUCCEEDED(hr))
	{
		//Dbg("gyro meter 3d(Y) var.vt %d",var.vt);
		if (var.vt == VT_R8)
		{
			Dbg("gyro(y) %f",var.fltVal);
		}
	}
	else
		Dbg("no gyro y");
	hr = pNewData->GetSensorValue(SENSOR_DATA_TYPE_ANGULAR_ACCELERATION_Z_DEGREES_PER_SECOND_SQUARED,&var);

	if (SUCCEEDED(hr))
	{
		//Dbg("gyro meter 3d(Z) var.vt %d",var.vt);
		if (var.vt == VT_R8)
		{
			Dbg("gyro(z) %f",var.fltVal);
		}
	}
	else
		Dbg("no gyro z");
	PropVariantClear(&var);
}
void MyEvents::OnCompass3DUpdate(ISensor *pSensor,ISensorDataReport *pNewData)
{
	PROPVARIANT var = {};
	HRESULT hr = pNewData->GetSensorValue(SENSOR_DATA_TYPE_MAGNETIC_HEADING_X_DEGREES,&var);

	if (SUCCEEDED(hr))
	{
		if (var.vt == VT_R4)
		{
			Dbg("compass(x) %f",var.fltVal);
		}
	}
	else
		Dbg("no compass x");
	hr = pNewData->GetSensorValue(SENSOR_DATA_TYPE_MAGNETIC_HEADING_Y_DEGREES,&var);

	if (SUCCEEDED(hr))
	{
		if (var.vt == VT_R4)
		{
			Dbg("compass(y) %f",var.fltVal);
		}
	}
	else
		Dbg("no compass y");
	hr = pNewData->GetSensorValue(SENSOR_DATA_TYPE_MAGNETIC_HEADING_Z_DEGREES,&var);

	if (SUCCEEDED(hr))
	{
		if (var.vt == VT_R4)
		{
			Dbg("compass(z) %f",var.fltVal);
		}
	}
	else
		Dbg("no compass z");
	PropVariantClear(&var);
}
void MyEvents::OnInclinoMeter3DUpdate(ISensor *pSensor,ISensorDataReport *pNewData)
{
	PROPVARIANT var = {};
	HRESULT hr = pNewData->GetSensorValue(SENSOR_DATA_TYPE_TILT_X_DEGREES,&var);

	if (SUCCEEDED(hr))
	{
		//Dbg("inclinometer 3d(X) var.vt %d",var.vt);
		if (var.vt == VT_R4)
		{
			Dbg("inclinometer(x) %f",var.fltVal);
		}
	}
	else
		Dbg("no inclinometer x");
	hr = pNewData->GetSensorValue(SENSOR_DATA_TYPE_TILT_Y_DEGREES,&var);

	if (SUCCEEDED(hr))
	{
		//Dbg("inclinometer 3d(Y) var.vt %d",var.vt);
		if (var.vt == VT_R4)
		{
			Dbg("inclinometer(y) %f",var.fltVal);
		}
	}
	else
		Dbg("no inclinometer y");
	hr = pNewData->GetSensorValue(SENSOR_DATA_TYPE_TILT_Z_DEGREES,&var);

	if (SUCCEEDED(hr))
	{
		//Dbg("inclinometer 3d(Z) var.vt %d",var.vt);
		if (var.vt == VT_R4)
		{
			Dbg("inclinometer(z) %f",var.fltVal);
		}
	}
	else
		Dbg("no inclinometer z");
	PropVariantClear(&var);
}
void MyEvents::OnAcceleroMeter3DUpdate(ISensor *pSensor,ISensorDataReport *pNewData)
{
	PROPVARIANT var = {};
	HRESULT hr = pNewData->GetSensorValue(SENSOR_DATA_TYPE_ACCELERATION_X_G,&var);

	if (SUCCEEDED(hr))
	{
		if (var.vt == VT_R8)
		{
			Dbg("accelerometer(x) %f",var.fltVal);
		}
	}
	else
		Dbg("no accelerometer x");
	hr = pNewData->GetSensorValue(SENSOR_DATA_TYPE_ACCELERATION_Y_G,&var);

	if (SUCCEEDED(hr))
	{
		if (var.vt == VT_R8)
		{
			Dbg("accelerometer(y) %f",var.fltVal);
		}
	}
	else
		Dbg("no accelerometer y");
	hr = pNewData->GetSensorValue(SENSOR_DATA_TYPE_ACCELERATION_Z_G,&var);

	if (SUCCEEDED(hr))
	{
		if (var.vt == VT_R8)
		{
			Dbg("accelerometer(z) %f",var.fltVal);
		}
	}
	else
		Dbg("no accelerometer z");
	PropVariantClear(&var);
}

void CSensorsFSM::GetAmbientLightData()
{
	ISensorDataReport *pLightData;
	m_pLightSensor->GetData(&pLightData);
	PROPVARIANT var = {};
	HRESULT hr = pLightData->GetSensorValue(SENSOR_DATA_TYPE_LIGHT_LEVEL_LUX,&var);

	if (SUCCEEDED(hr))
	{
		if (var.vt == VT_R4)
		{
			Dbg("light %f",var.fltVal);
			FindSensorDataChange(AMBIENT_LIGHT,var.fltVal,0,0,0);
			m_mapLastValue[AMBIENT_LIGHT].dwData = var.fltVal;
		}
	}
	PropVariantClear(&var);
}
void CSensorsFSM::GetGyroMeter3DData()
{
	ISensorDataReport *pGyroMeter3DData;
	m_pGyroMeter3DSensor->GetData(&pGyroMeter3DData);
	double x,y,z;
	x = y = z = 0;
	PROPVARIANT var = {};
	HRESULT hr = pGyroMeter3DData->GetSensorValue(SENSOR_DATA_TYPE_ANGULAR_ACCELERATION_X_DEGREES_PER_SECOND_SQUARED,&var);

	if (SUCCEEDED(hr))
	{
		//Dbg("gyro meter 3d(X) var.vt %d",var.vt);
		if (var.vt == VT_R8)
		{
			Dbg("gyro(x) %f",var.dblVal);
			x = var.dblVal;
		}
	}
	else
		Dbg("no gyro x");
	hr = pGyroMeter3DData->GetSensorValue(SENSOR_DATA_TYPE_ANGULAR_ACCELERATION_Y_DEGREES_PER_SECOND_SQUARED,&var);

	if (SUCCEEDED(hr))
	{
		//Dbg("gyro meter 3d(Y) var.vt %d",var.vt);
		if (var.vt == VT_R8)
		{
			Dbg("gyro(y) %f",var.dblVal);
			y = var.dblVal;
		}
	}
	else
		Dbg("no gyro y");
	hr = pGyroMeter3DData->GetSensorValue(SENSOR_DATA_TYPE_ANGULAR_ACCELERATION_Z_DEGREES_PER_SECOND_SQUARED,&var);

	if (SUCCEEDED(hr))
	{
		//Dbg("gyro meter 3d(Z) var.vt %d",var.vt);
		if (var.vt == VT_R8)
		{
			Dbg("gyro(z) %f",var.dblVal);
			z = var.dblVal;
		}
	}
	else
		Dbg("no gyro z");
	hr = pGyroMeter3DData->GetSensorValue(SENSOR_DATA_TYPE_SPEED_METERS_PER_SECOND,&var);

	if (SUCCEEDED(hr))
	{
		if (var.vt == VT_R8)
		{
			Dbg("gyro(speed) %f",var.dblVal);
		}
	}
	else
		Dbg("no gyro speed");
	hr = pGyroMeter3DData->GetSensorValue(SENSOR_DATA_TYPE_MOTION_STATE,&var);

	if (SUCCEEDED(hr))
	{
		if (var.vt == VT_BOOL)
		{
			Dbg("gyro(motion state) %f",var.boolVal);
		}
	}
	else
		Dbg("no gyro motion state");
	FindSensorDataChange(GYROMETER_3D,0,x,y,z);
	if (x != 0)
		m_mapLastValue[GYROMETER_3D].dwX = x;
	if (y != 0)
		m_mapLastValue[GYROMETER_3D].dwY = y;
	if (z != 0)
		m_mapLastValue[GYROMETER_3D].dwZ = z;
	PropVariantClear(&var);
}
void CSensorsFSM::GetCompass3DData()
{
	ISensorDataReport *pCompass3DData;
	m_pCompass3DSensor->GetData(&pCompass3DData);
	PROPVARIANT var = {};
	HRESULT hr = pCompass3DData->GetSensorValue(SENSOR_DATA_TYPE_MAGNETIC_HEADING_X_DEGREES,&var);

	if (SUCCEEDED(hr))
	{
		if (var.vt == VT_R4)
		{
			Dbg("compass(x) %f",var.fltVal);

		}
	}
	else
		Dbg("no compass x");
	hr = pCompass3DData->GetSensorValue(SENSOR_DATA_TYPE_MAGNETIC_HEADING_Y_DEGREES,&var);

	if (SUCCEEDED(hr))
	{
		if (var.vt == VT_R4)
		{
			Dbg("compass(y) %f",var.fltVal);
		}
	}
	else
		Dbg("no compass y");
	hr = pCompass3DData->GetSensorValue(SENSOR_DATA_TYPE_MAGNETIC_HEADING_Z_DEGREES,&var);

	if (SUCCEEDED(hr))
	{
		if (var.vt == VT_R4)
		{
			Dbg("compass(z) %f",var.fltVal);
		}
	}
	else
		Dbg("no compass z");
	PropVariantClear(&var);
}
void CSensorsFSM::GetInclinoMeter3DData()
{
	ISensorDataReport *pInclinoMeter3DData;
	m_pInclinoMeter3DSensor->GetData(&pInclinoMeter3DData);
	double x,y,z;
	x = y = z = 0;
	PROPVARIANT var = {};
	HRESULT hr = pInclinoMeter3DData->GetSensorValue(SENSOR_DATA_TYPE_TILT_X_DEGREES,&var);

	if (SUCCEEDED(hr))
	{
		//Dbg("inclinometer 3d(X) var.vt %d",var.vt);
		if (var.vt == VT_R4)
		{
			Dbg("inclinometer(x) %f",var.fltVal);
			x = var.fltVal;
		}
	}
	else
		Dbg("no inclinometer x");
	hr = pInclinoMeter3DData->GetSensorValue(SENSOR_DATA_TYPE_TILT_Y_DEGREES,&var);

	if (SUCCEEDED(hr))
	{
		//Dbg("inclinometer 3d(Y) var.vt %d",var.vt);
		if (var.vt == VT_R4)
		{
			Dbg("inclinometer(y) %f",var.fltVal);
			y = var.fltVal;
		}
	}
	else
		Dbg("no inclinometer y");
	hr = pInclinoMeter3DData->GetSensorValue(SENSOR_DATA_TYPE_TILT_Z_DEGREES,&var);

	if (SUCCEEDED(hr))
	{
		//Dbg("inclinometer 3d(Z) var.vt %d",var.vt);
		if (var.vt == VT_R4)
		{
			Dbg("inclinometer(z) %f",var.fltVal);
			z = var.fltVal;
		}
	}
	else
		Dbg("no inclinometer z");
	FindSensorDataChange(INCLINOMETER_3D,0,x,y,z);
	if (x != 0)
		m_mapLastValue[INCLINOMETER_3D].dwX = x;
	if (y != 0)
		m_mapLastValue[INCLINOMETER_3D].dwY = y;
	if (z != 0)
		m_mapLastValue[INCLINOMETER_3D].dwZ = z;
	PropVariantClear(&var);
}
void CSensorsFSM::GetAcceleroMeter3DData()
{
	ISensorDataReport *pAcceleroMeter3DData;
	m_pAcceleroMeter3DSensor->GetData(&pAcceleroMeter3DData);
	double x,y,z;
	x=y=z=0;
	PROPVARIANT var = {};
	HRESULT hr = pAcceleroMeter3DData->GetSensorValue(SENSOR_DATA_TYPE_ACCELERATION_X_G,&var);

	if (SUCCEEDED(hr))
	{
		if (var.vt == VT_R8)
		{
			Dbg("accelerometer(x) %f",var.dblVal);
			x = var.dblVal;
		}
	}
	else
		Dbg("no accelerometer x");
	hr = pAcceleroMeter3DData->GetSensorValue(SENSOR_DATA_TYPE_ACCELERATION_Y_G,&var);

	if (SUCCEEDED(hr))
	{
		if (var.vt == VT_R8)
		{
			Dbg("accelerometer(y) %f",var.dblVal);
			y = var.dblVal;
		}
	}
	else
		Dbg("no accelerometer y");
	hr = pAcceleroMeter3DData->GetSensorValue(SENSOR_DATA_TYPE_ACCELERATION_Z_G,&var);

	if (SUCCEEDED(hr))
	{
		if (var.vt == VT_R8)
		{
			Dbg("accelerometer(z) %f",var.dblVal);
			z = var.dblVal;
		}
	}
	else
		Dbg("no accelerometer z");
	FindSensorDataChange(ACCELEROMETER_3D,0,x,y,z);
	if (x != 0)
		m_mapLastValue[ACCELEROMETER_3D].dwX = x;
	if (y != 0)
		m_mapLastValue[ACCELEROMETER_3D].dwY = y;
	if (z != 0)
		m_mapLastValue[ACCELEROMETER_3D].dwZ = z;
	PropVariantClear(&var);
}
void CSensorsFSM::FindSensorDataChange(SensorType eType,double dwValue1,double dwX,double dwY,double dwZ)
{
	bool bChanged = false;
	switch(eType)
	{
	case AMBIENT_LIGHT:
		if (m_mapLastValue[eType].dwData != 0)
		{
			if (dwValue1 > m_mapLastValue[eType].dwData && dwValue1 > (m_mapLastValue[eType].dwData+m_lightVar))
			{
				Dbg("brighttttttt");
				LogEvent(Severity_Middle,LOG_EVT_SENSORS_LIGHT_BRIGHT,"ambient light is more brighter.");
			}
			else if (dwValue1 < m_mapLastValue[eType].dwData && (dwValue1+m_lightVar) < m_mapLastValue[eType].dwData)
			{
				Dbg("darkerrrrrr");
				LogEvent(Severity_Middle,LOG_EVT_SENSORS_LIGHT_DARK,"ambient light is more darker.");
			}
		}
		break;
	case GYROMETER_3D:
		if (m_mapLastValue[eType].dwX != 0 || m_mapLastValue[eType].dwY != 0 || m_mapLastValue[eType].dwZ != 0)
		{
			bChanged = Is3DDataChanged(eType,dwX,dwY,dwZ);
			if (bChanged)
			{
				Dbg("gyro meter change");
				LogEvent(Severity_Middle,LOG_EVT_SENSORS_DYROMETER_CHANGE,"Gyro meter 3d changed.");
			}
		}
		break;
	case COMPASS_3D:
		if (m_mapLastValue[eType].dwX != 0 || m_mapLastValue[eType].dwY != 0 || m_mapLastValue[eType].dwZ != 0)
		{
			bChanged = Is3DDataChanged(eType,dwX,dwY,dwZ);
			if (bChanged)
				LogEvent(Severity_Middle,LOG_EVT_SENSORS_COMPASS_CHANGE,"compass 3d changed.");
		}
		break;
	case INCLINOMETER_3D:
		if (m_mapLastValue[eType].dwX != 0 || m_mapLastValue[eType].dwY != 0 || m_mapLastValue[eType].dwZ != 0)
		{
			bChanged = Is3DDataChanged(eType,dwX,dwY,dwZ);
			if (bChanged)
			{
				Dbg("inclinometer change");
				LogEvent(Severity_Middle,LOG_EVT_SENSORS_INCLINEMETER_CHANGE,"inclinometer 3d changed.");
			}
		}
		break;
	case ACCELEROMETER_3D:
		if (m_mapLastValue[eType].dwX != 0 || m_mapLastValue[eType].dwY != 0 || m_mapLastValue[eType].dwZ != 0)
		{
			bChanged = Is3DDataChanged(eType,dwX,dwY,dwZ);
			if (bChanged)
				LogEvent(Severity_Middle,LOG_EVT_SENSORS_ACCELEROMETER_CHANGE,"accelerometer 3d changed.");
		}
		break;
	default:
		break;
	}
}
bool CSensorsFSM::Is3DDataChanged(SensorType eType,double dwX,double dwY,double dwZ)
{
	bool bChanged = false;
	int iVar = 0;
	switch(eType)
	{
	case COMPASS_3D:
		iVar = m_compassVar;
		break;
	case GYROMETER_3D:
		iVar = m_gyroVar;
		break;
	case INCLINOMETER_3D:
		iVar = m_inclineVar;
		break;
	case ACCELEROMETER_3D:
		iVar = m_acceleroVar;
		break;
	default:
		Dbg("Is3DDataChanged unknown sensor type.");
		return false;
	}
	if (m_mapLastValue[eType].dwX != 0 || m_mapLastValue[eType].dwY != 0 || m_mapLastValue[eType].dwZ != 0)
	{
		if (dwX != 0)
			if (dwX > (m_mapLastValue[eType].dwX + iVar) || (dwX+iVar) < m_mapLastValue[eType].dwX)
				bChanged = true;
		if (dwY != 0)
			if (dwY > (m_mapLastValue[eType].dwY + iVar) || (dwY+iVar) < m_mapLastValue[eType].dwY)
				bChanged = true;
		if (dwZ != 0)
			if (dwZ > (m_mapLastValue[eType].dwZ + iVar) || (dwZ+iVar) < m_mapLastValue[eType].dwZ)
				bChanged = true;
	}
	return bChanged;
}

ErrorCodeEnum CSensorsFSM::LoadGnssVendorDLL()
{
	//oiyang@20200226
	LOG_FUNCTION();
	CSimpleStringA csDepPath(""), csBackslash("\\"), dllName(""), tmpDLLVersion("");
	GetEntityBase()->GetFunction()->GetSysVar("FWBVendor", m_csFWBVendor);
	GetEntityBase()->GetFunction()->GetSysVar("FWBVersion", tmpDLLVersion);
	if (tmpDLLVersion.GetLength() < 2)//如果忘记配置，则直接使用默认值8.1
		tmpDLLVersion = "8.1";
	
	ErrorCodeEnum eErrPath = GetEntityBase()->GetFunction()->GetPath("Dep", csDepPath);
	dllName = csDepPath + csBackslash + "gnss." + m_csFWBVendor + "." + tmpDLLVersion + ".dll";//oiltmp
	Dbg("load vendor dll %s", (const char*)dllName);
	m_hVerdorDll = LoadLibraryA(dllName);

	if (m_hVerdorDll == NULL)
	{
		Dbg("Load dll failed.%d", GetLastError());
		//GetEntityBase()->GetFunction()->ShowFatalError("加载厂商适配器失败！请确定是否没绑定蓝牙多合一。");
		return Error_DevLoadFileFailed;
	}
	if ((CreateDevComponent = (lpCreateDevCom)GetProcAddress(m_hVerdorDll, "CreateDevComponent")) == NULL)
	{
		Dbg("Get CreateDevComponent failed.");
		return Error_DevLoadFileFailed;
	}
	if ((ReleaseDevComponent = (lpReleaseDevCom)GetProcAddress(m_hVerdorDll, "ReleaseDevComponent")) == NULL)
	{
		Dbg("Get ReleaseDevComponent failed.");
		return Error_DevLoadFileFailed;
	}
	if (m_pGnssClass == NULL)
	{
		if (CreateDevComponent((DeviceBaseClass * &)m_pGnssClass) != Error_Succeed)
		{
			Dbg("CreateDevComponent failed.");
		}
	}
	Dbg("Create vendor handle suc.");
	GnssClass* pxx = (GnssClass*)m_pGnssClass;
	ErrorCodeEnum eErrDev = m_pGnssClass->DevOpen(0, 0);
	if (eErrDev == Error_Succeed)
	{
		Dbg("定位模块打开成功。");
		DoGetGnssTask *pTask = new DoGetGnssTask(this);
		GetEntityBase()->GetFunction()->PostThreadPoolTask(pTask);
		return Error_Succeed;
	}
	else
	{
		DevErrorInfo errInfo;
		ZeroMemory(errInfo.szErrMsg, sizeof(errInfo.szErrMsg));
		m_pGnssClass->GetLastErr(errInfo);
		Dbg("%x,%s", errInfo.dwErrMsgLen, errInfo.szErrMsg);
		return Error_DevCommFailed;
	}
	
}
void CSensorsFSM::DoGetGnss()
{
	memset(&m_gnssInfo, 0, sizeof(GnssInfoStru));
	memset(&m_lastGnssInfo, 0, sizeof(GnssInfoStru));
	CHAR gnssMsg[128];
	memset(gnssMsg, 0, sizeof(gnssMsg));
	ErrorCodeEnum eErr = Error_Unexpect;
	while (1) {
		//struct GnssInfoStru
		//{
		//	CHAR status;            // GNSS数据有效性标志，'V' 无效， 'A' 有效
		//	CHAR dateTime[14 + 1];    // 日期时间(北京时间)， 格式为: YYYYMMDDHHMMSS
		//	CHAR lat[20 + 1];         // 纬度，格式为: x,dd,mm.mmmm 
		//							// 其中: x为北纬或南纬指示：N - 北纬 S - 南纬
		//							//       dd - 度, mm.mmmm - 分 
		//	CHAR lon[20 + 1];         // 经度，格式为: y,ddd,mm.mmmm 
		//							// 其中: y 为东经或西经指示： E - 东经 W - 西经
		//							//       ddd - 度, mm.mmmm - 分 
		//};
		eErr = m_pGnssClass->ReadGnssInfo(m_gnssInfo);
		if (eErr == Error_Succeed) {
			//[13:38:20.628][DEBUG] Debug: {A,20200730133916,N,29,44.6831,E,106,33.8813}
			Dbg("%c,%s,%s,%s", m_gnssInfo.status, m_gnssInfo.dateTime, m_gnssInfo.lat, m_gnssInfo.lon);
			int lenLatMax = (strlen(m_gnssInfo.lat) > 20) ? 20 : strlen(m_gnssInfo.lat);
			int lenLonMax = (strlen(m_gnssInfo.lon) > 20) ? 20 : strlen(m_gnssInfo.lon);
			if (m_gnssInfo.status == 'A' && (_strnicmp(m_lastGnssInfo.lat,m_gnssInfo.lat,lenLatMax) != 0
				|| _strnicmp(m_lastGnssInfo.lon, m_gnssInfo.lon, lenLonMax) != 0))
			{
				strncpy_s(m_lastGnssInfo.lat, _countof(m_lastGnssInfo.lat), m_gnssInfo.lat, lenLatMax);
				strncpy_s(m_lastGnssInfo.lon, _countof(m_lastGnssInfo.lon), m_gnssInfo.lon, lenLonMax);
				sprintf_s(gnssMsg, "%s,lat:%s,lon:%s", m_gnssInfo.dateTime, m_gnssInfo.lat, m_gnssInfo.lon);
				LogWarn(Severity_Low, Error_Unexpect, LOG_EVT_SENSORS_GNSS_CHANGE, gnssMsg);
				memset(gnssMsg, 0, sizeof(gnssMsg));
			}
		}
		Sleep(1000 * 60);
	}
}