示例#1
0
s32 i2c_read(u32 Address,u8 *Data, u32 DataSize)
{
	s32 sReturn = -1;

	do
	{
		sReturn = SetAddressSize(Address);
		if(sReturn < 0)
		{	
			break;
		}
		
		TRACE((I2C_TRACE_LEVEL,"calling adl_busRead %x %x\n",AccessConfig.Address,Address));
		TRACE ((I2C_TRACE_LEVEL, "call adl_busReadExt "));
		sReturn = adl_busRead(i2cHandle,&AccessConfig,DataSize,Data);
		printf("bus read return = %d\n",(int )sReturn);
#ifdef REMOVE_SEM
		sReturn = adl_semConsumeDelay( gI2CSemHndl,60);
#endif
		if( sReturn < 0 )
		  {
		    if( sReturn == -8 )
		      {
			// timeout happened, produce and continue
			TRACE((I2C_TRACE_LEVEL,"semConsumeDelay happened!!\n"));
			if( adl_semIsConsumed( gI2CSemHndl ) == TRUE )
			  {
			    sReturn = adl_semProduce( gI2CSemHndl );
			    if( sReturn < 0 )
			      DisplayErrorCode("adl_semProduce",__FILE__,__LINE__,sReturn);
			  }
		      }
		    else
		      DisplayErrorCode("adl_semConsumeDelay",__FILE__,__LINE__,sReturn);
		    printf ("I2C Return : %d\n",(int)sReturn);
		    return sReturn;
		  }
//		sReturn = adl_busReadExt(i2cHandle,&AccessConfig,DataSize,Data,AsyncReadCtxt);
//		while( busMutex )
//			adl_ctxSleep(1);
//		busMutex=1;

		TRACE ((I2C_TRACE_LEVEL, "complete adl_busReadExt "));
		if(sReturn < 0)
		{ //ADL_RET_ERR_UNKNOWN_HDL
			DisplayErrorCode("adl_busReadExt",__FILE__,__LINE__,sReturn);
			return sReturn;
		}

	}while(0);

	return sReturn;
}
示例#2
0
s32 SetAddressSize(u32 i_Address)
{
	s32 sReturn=-1;
	u32 size = 0;
	/*---- Set the address to read from ----*/
	if( (i2cSettings.ChipAddress == gI2CDevice1) || 
			(i2cSettings.ChipAddress == gI2CDevice2) || 
			(i2cSettings.ChipAddress == gI2CDevice3) )
	{
		size = 8;
		// lets shift the address up the MSB
		AccessConfig.Address=(i_Address << 24);
	}
	else
	{
		size = 16;
		// lets shift the address up the MSB
		AccessConfig.Address=(i_Address << 16);
	}	

	sReturn = adl_busIOCtl( i2cHandle, ADL_BUS_CMD_SET_ADD_SIZE,&size);
	if(sReturn < 0)
	{
		DisplayErrorCode("adl_busIOCtl",__FILE__,__LINE__,sReturn);
	}
	return sReturn;

}
示例#3
0
s32 i2c_write(u32 Address, u8 *Data, u32 DataSize)
{
	unsigned int i;
	s32 sReturn = -1;
	u32 AddressSize = 0;

	do
	{
		for (i=0; i<DataSize; i++)
		{
			TRACE((I2C_TRACE_LEVEL,"%x,",*(Data+i)));
		}
		TRACE((I2C_TRACE_LEVEL,"\n"));
		sReturn = SetAddressSize(Address);
		if(sReturn < 0)
		{
			break;
		}
		if( (i2cSettings.ChipAddress == gI2CDevice1) || 
				(i2cSettings.ChipAddress == gI2CDevice2) || 
				(i2cSettings.ChipAddress == gI2CDevice3) )
		{
			AddressSize = 0;
			sReturn = adl_busIOCtl( i2cHandle, ADL_BUS_CMD_SET_ADD_SIZE,&AddressSize );
			TRACE((I2C_TRACE_LEVEL,"set address size to zero\n"));
		}

		sReturn = adl_busWrite(i2cHandle,&AccessConfig,DataSize,(void *)Data);
		if(sReturn < 0)
			DisplayErrorCode("adl_busWrite",__FILE__,__LINE__,sReturn);
	}while(0);
	return sReturn;
}
示例#4
0
/** @brief Check the current coordinate
 *
 * @par
 * This is what triggers the geofence system. Everytime we get a fix we need to check if we are violating any geofences.
 *
 * @return void
 */
static void CheckCoord(void)
{
	UINT8 current_fence_stat;
	int fence_num;

	adl_rtcTimeStamp_t TimeStamp0, TimeStamp1;
	adl_rtcTimeStamp_t DeltaTimeStamp;

	// If we are in dog walk mode,
    if (GetDogWalkMode() == DOGWALK_ENABLED)
	{
		DumpMessage("Dog walk mode\r\n");
		return;
	}

	GetConvertTime(&TimeStamp0);
	current_fence_stat = eval_fix(&fence_num, GPS_FIX_REASON);
	GetConvertTime(&TimeStamp1);

	wm_sprintf(g_traceBuf, "Fence Number = %d\r\n", fence_num);
	DumpMessage(g_traceBuf);

	if (adl_rtcDiffTime(&TimeStamp1, &TimeStamp0, &DeltaTimeStamp) < 0)
	{
		DisplayErrorCode("adl_rtcConvertTime", __FILE__, __LINE__, -1);
	}

	wm_sprintf(g_traceBuf, "Calc time %d:%d\r\n", (int )DeltaTimeStamp.TimeStamp, (int)ADL_RTC_GET_TIMESTAMP_MS(DeltaTimeStamp));
	DumpMessage(g_traceBuf);
	wm_sprintf(g_traceBuf, "current_fence_stat = %c\r\n", current_fence_stat);
	DumpMessage(g_traceBuf);
}
示例#5
0
/** @brief Convert a timestamp structure to seconds
 *
 * @par
 * This reads the RTC and stores it as a adl_rtcTimeStamp_t type instead of adl_rtcTime_t type.
 * @param TimeStamp
 * @return void
 */
int GetConvertTime(adl_rtcTimeStamp_t *TimeStamp)
{
	adl_rtcTime_t CurrentTime;
	s32 sReturn = -1;

	// get the current time.
	if ((sReturn = adl_rtcGetTime(&CurrentTime)) < 0)
	{
		DisplayErrorCode("adl_rtcGetTime", __FILE__, __LINE__, sReturn);
	}

	// convert to time stamp.
	if ((sReturn = adl_rtcConvertTime(&CurrentTime, TimeStamp, ADL_RTC_CONVERT_TO_TIMESTAMP)) < 0)
	{
		DisplayErrorCode("adl_rtcConvertTime", __FILE__, __LINE__, sReturn);
	}

	return sReturn;
}
示例#6
0
/** @brief Calls the adl_flhErase on the provided handle and ID
 *
 * @par description Calls the adl_flhErase on the provided handle and ID, if i_id is set to ADL_FLH_ALL_IDS, then all IDs associated with the handle will be erased
 *
 * @param i_Handle
 * @param i_id
 * @return < 0 on failure
 * @return >= 0 on Success
 */
s8 FlashErase(char const *const i_Handle, u16 i_id)
{
	s8 RetCode = -1;
	TRACE((1, "FlashErase"));

	RetCode = adl_flhErase((char *)i_Handle, i_id);
	if (RetCode < 0)
	{
		DisplayErrorCode("adl_flhExist", __FILE__, __LINE__ - 3, RetCode);
	}
	else
	{
		TRACE((1, "Successfully erased flash id: %d", i_id));
	}
	return RetCode;
}
		static msg_t mfiThread(void)
#endif
		{
	chRegSetThreadName("MFIndicator");
	error_codes_set_s localErrorCopy;

	while (true) {
		chThdSleepSeconds(10);

		getErrorCodes(&localErrorCopy);
		for (int p = 0; p < localErrorCopy.count; p++) {
			// Calculate how many digits in this integer and display error code from start to end
			int code = localErrorCopy.error_codes[p];
			DisplayErrorCode(DigitLength(code), code);
		}
	}
}
示例#8
0
/** @brief Calls the adl_flhRead function on the provided handle and number of objects
 *
 * @param i_Handle
 * @param i_Id
 * @param i_Length
 * @param o_Data
 * @return < 0 on failure
 * @return >= 0 on Success
 */
s8 FlashRead(char const *const i_Handle, u16 i_Id, u16 i_Length, u8 *const o_Data)
{
	s8 RetCode = 0;

	RetCode = adl_flhRead((char *)i_Handle, i_Id, i_Length, o_Data);
	//wm_sprintf(g_traceBuf, "\r\nRetCode from adl_flhRead: %d\r\n", RetCode);
	//DumpMessage(g_traceBuf);
	if (RetCode < 0)
	{
		DisplayErrorCode("adl_flhRead", __FILE__, __LINE__ - 5, RetCode);
	}
	else
	{
		TRACE((1, "Successfully read from flash ID: %d", i_Id));
	}
	return RetCode;
}
示例#9
0
/** @brief Calls the adl_flhSubscribe function on the provided handle and number of objects
 *
 * @param flhHandle
 * @param numObjects
 * @return OK on success
 * @return Error Code on failure
 */
s8 FlashSubscribe(char const *const flhHandle, u16 numObjects)
{
	s8 RetCode = 0;

	RetCode = adl_flhSubscribe((char *)flhHandle, numObjects);
	if ((RetCode == OK) || (RetCode == ADL_RET_ERR_ALREADY_SUBSCRIBED))
	{
		TRACE((1, "Successfully subscribed to the flash"));
		RetCode = OK;
	}
	else
	{
		DisplayErrorCode("adl_flhSubscribe", __FILE__, __LINE__ - 2, RetCode);
	}

	return RetCode;
}
示例#10
0
/** @brief Calls the adl_flhExist function on the provided handle and check to see if the ID exists
 *
 * @param flhHandle
 * @param flhId
 * @return < 0 on failure
 * @return >= 0 on Success
 */
s8 FlashExists(char const *const flhHandle, u16 flhId)
{
	s32 RetCode = 0;
	RetCode = adl_flhExist((char *)flhHandle, flhId);

	TRACE((1, "FlashExist "));
	TRACE((1, "Ids %x", flhId));

	if (RetCode < 0)
	{
		DisplayErrorCode("adl_flhExist", __FILE__, __LINE__ - 7, RetCode);
	}
	else
	{
		TRACE((1, "Successfully found the flash ID and its length is %d", RetCode));
	}
	return RetCode;
}
示例#11
0
/** @brief Calls the adl_flhWrite function on the provided handle
 *
 * @par description: Calls the adl_flhWrite function on the provided handle at the specified ID. The data in o_Data is the data to be written
 *
 * @param i_Handle
 * @param i_Id
 * @param i_Length
 * @param i_Data
 * @return < 0 on failure
 * @return >= 0 on Success
 */
s8 FlashWrite(char const *const i_Handle, u16 i_Id, u16 i_Length, u8 const *const i_Data)
{
	s8 RetCode = 0;

	TRACE((1, "FlashWrite %d", i_Length));

	RetCode = adl_flhWrite((char *)i_Handle, i_Id, i_Length, (u8 *)i_Data);
	//wm_sprintf(g_traceBuf, "\r\nRetCode from adl_flhWrite: %d\r\n", RetCode);
	//DumpMessage(g_traceBuf);
	if (RetCode >= 0)
	{
		TRACE((1, "Successfully wrote to the flash"));
	}
	else
	{
		DisplayErrorCode("adl_flhWrite", __FILE__, __LINE__ - 8, RetCode);
	}

	return RetCode;
}
示例#12
0
LRESULT CALLBACK WndProc(HWND hWnd, UINT uMsg, WPARAM wParam, LPARAM lParam)
{
  PAINTSTRUCT ps;


  switch(uMsg) 
  {
  case WM_COMMAND:
	  {
		  // load new model here
		  unsigned int id = LOWORD(wParam);
			if( id > 500 && (int)(id - 500) <= g_iNumModels ) 
			{
			  int index = id - 501;
			  strcpy(g_szHandFileName,g_szModelFileNames[index]);

				FreeControllers();
				HRESULT rval = InitControllers();
			  if (FAILED(rval))
			  {
					if( ERR_FILENOTFOUND != rval )	// annunciated by InitControllers()
				    DisplayErrorCode(rval);
					exit(rval);
			  }
			}
	  }
	  break;
    case WM_ACTIVATE:
      break;

    case WM_ACTIVATEAPP:
      g_bActive = wParam;
      break;

    case WM_PAINT:
      BeginPaint(hWnd, &ps);
      EndPaint(hWnd, &ps);
      break;

    case WM_CLOSE:
			PostQuitMessage( 0 );
			return 0;

    case WM_KEYDOWN:
      switch( wParam )
      {
        case VK_ESCAPE:
          PostQuitMessage( 0 );
        break;

      }
      break;


    default:
      break;
  }

	// grab keypresses for the keyboardcontrol
	if (WM_CHAR == uMsg)
  {
    TCHAR chCharCode = (TCHAR) wParam;
    if (!g_pKeyboardControl->HandleKey(chCharCode))
    {
      if (TCHAR(' ') == chCharCode)
      {
				//toggle full screen mode
				uMsg = WM_COMMAND;
				wParam = IDM_TOGGLEFULLSCREEN;
      }
      else if (TCHAR('z') == chCharCode)
      {
				//toggle full screen mode
				uMsg = WM_MOUSEWHEEL;
				wParam = MAKEWPARAM( 0, ZOOM_DELTA );
      }
      else if (TCHAR('x') == chCharCode)
      {
				//toggle full screen mode
				uMsg = WM_MOUSEWHEEL;
				wParam = MAKEWPARAM( 0, -ZOOM_DELTA );
      }
    }
  }


  // then allow the engine its chance to handle messages (like WM_SIZE)
  if (g_pEngine)
  {
    g_pEngine->HandleMessage(hWnd, uMsg, wParam, lParam);
  }
  
  return DefWindowProc(hWnd, uMsg, wParam, lParam); 

}
示例#13
0
HRESULT InitControllers()
{
	_ASSERT( !g_pKeyboardControl );
	
	// load model file
	g_pKeyboardControl = new KeyboardControl( g_szHandFileName, "GLOVE" );

	if( !g_pKeyboardControl->GetNumObjects() )
	{
		char msg[256];
		sprintf( msg, "Could not load %s.", g_szHandFileName );
		int ok = MessageBox( g_hWnd, msg, "", MB_OKCANCEL );
//		if( IDOK != ok )
			return ERR_FILENOTFOUND;
	}

	// load model data
	strcpy( g_szHandFileName, g_pKeyboardControl->GetString( "MODEL", "Hand", "media\\newhand.x" ) );
	strcpy( g_szRoomFileName, g_pKeyboardControl->GetString( "MODEL", "Room", "media\\testroom.x" ) );
	g_fHandScale = g_pKeyboardControl->GetFloat( "MODEL", "HandScale", 1.0f );
	g_fRoomScale = g_pKeyboardControl->GetFloat( "MODEL", "RoomScale", 1.0f );


  // create a hand object
  g_pHandObject = new C3DFrameObject();
  HRESULT rval = g_pHandObject->Init(g_pEngine, g_szHandFileName );
  if (rval)
  {
    DisplayErrorCode(rval);
    exit(rval);
  }

	// scale it
	D3DXMATRIX mScale;
	D3DXMatrixScaling( &mScale, g_fHandScale, g_fHandScale, g_fHandScale );
	D3DXMATRIX mIdentity;
	D3DXMatrixIdentity( &mIdentity );
	g_pHandObject->SetMatrices( &mIdentity, &mScale );

  // add it to our world
  rval = g_pEngine->AddChild(g_pHandObject);
  if (rval)
  {
    DisplayErrorCode(rval);
    exit(rval);
  }

	// create text and controllers
	const float fFirstTextLine = UI_FIRST_LINE_Y;
	const float fTextIndent = UI_OFFSET_X;
	const float fTextLineHeight = UI_GROUP_OFFSET_Y;

	float fTextLine = fFirstTextLine;
	C3DFont *pFont = g_pEngine->GetFont();

	int iNumParts = g_pKeyboardControl->GetNumObjects();
	for( int i = 0; i < iNumParts; ++i )
	{
		KeyboardObject *pKBO = g_pKeyboardControl->GetObjectPtr(i);
		ASSERT( pKBO );

		const char *szBoneName = pKBO->GetName();
		ASSERT(szBoneName);

		int iNumValues = pKBO->GetNumValues();
		ASSERT( iNumValues );

		const char *szAnimFileName = pKBO->GetAnimation();

		const char *szControlType = pKBO->GetType();
		if( !strlen( szControlType ) )
		{
			// default to pitch controller if 1 control value
			if( 1 == iNumValues )
				szControlType = g_szPitchControllerType;
			// else default to 6-dof controller if 6 control values
			else if( 6 == iNumValues )
				szControlType = g_sz6DOFControllerType;
			// else default to animation frame controller if anim file name present
//			else if( strlen( szAnimFileName )
//				szControlType = g_szAnimationControllerType;
			else
				return E_FAIL;	// unrecognized controller type
		}

		Controller *pController = NULL;
		C3DText *pText = NULL;

		SIZE size;
		pFont->GetTextExtent("1",&size);

		//
		// create controller of type specified
		//
		// animation frame controller
		if( 0 == stricmp( g_szAnimationControllerType, szControlType ) )
		{
			ASSERT( 1 == iNumValues );
			float *fInput = pKBO->GetValuePtr(0);

			pController = g_pHandObject->AttachAnimController( szBoneName, fInput );
			if( !pController )
				return E_FAIL;

			// attach text display
			if( strlen( pKBO->GetDisplayText() ) )
			{
				pText = new C3DText(2);
				pText->SetOffset( fTextIndent, fTextLine );
				pText->SetLine( 0, pKBO->GetDisplayText() );
				pText->SetColor(UI_HAND_RED,UI_HAND_GREEN,UI_HAND_BLUE,UI_HAND_ALPHA);
				pController->SetText( pText );
				g_pEngine->AddText( pText );
				fTextLine += pText->GetTextBlockHeight(pFont) + fTextLineHeight;
			}
		}

		// 6-dof controller
		else if( 0 == stricmp( g_sz6DOFControllerType, szControlType ) )
		{
			ASSERT( 6 == iNumValues );
			float *fX = pKBO->GetValuePtr(0);
			float *fY = pKBO->GetValuePtr(1);
			float *fZ = pKBO->GetValuePtr(2);
			float *fRoll = pKBO->GetValuePtr(3);
			float *fPitch = pKBO->GetValuePtr(4);
			float *fYaw = pKBO->GetValuePtr(5);

			pController = g_pHandObject->Attach6DOFController( szBoneName, fX, fY, fZ, fRoll, fPitch, fYaw );
			if( !pController )
				return E_FAIL;

			// attach text display
			if( strlen( pKBO->GetDisplayText() ) )
			{
				pText = new C3DText(4);
				pText->SetOffset( fTextIndent, fTextLine );
				pText->SetLine( 0, pKBO->GetDisplayText() );
				pText->SetColor(UI_HAND_RED,UI_HAND_GREEN,UI_HAND_BLUE,UI_HAND_ALPHA);
				pController->SetText( pText );
				g_pEngine->AddText( pText );
				fTextLine += pText->GetTextBlockHeight(pFont) + fTextLineHeight;
			}
		}

		// pitch-only controller
		else if( 0 == stricmp( g_szPitchControllerType, szControlType ) )
		{
			ASSERT( 1 == iNumValues );
			float *fPitch = pKBO->GetValuePtr(0);

			pController = g_pHandObject->Attach6DOFController( szBoneName, NULL, NULL, NULL, NULL, fPitch, NULL );
			if( !pController )
				return E_FAIL;

			// attach text display
			if( strlen( pKBO->GetDisplayText() ) )
			{
				pText = new C3DText(2);
				pText->SetOffset( fTextIndent, fTextLine );
				pText->SetLine( 0, pKBO->GetDisplayText() );
				pText->SetColor(UI_HAND_RED,UI_HAND_GREEN,UI_HAND_BLUE,UI_HAND_ALPHA);
				pController->SetText( pText );
				g_pEngine->AddText( pText );
				fTextLine += pText->GetTextBlockHeight(pFont) + fTextLineHeight;
			}
		}
	}
	return S_OK;
}
示例#14
0
int APIENTRY WinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nCmdShow)
{
  MSG    msg;
  HRESULT rval;

  rval = InitInstance(hInstance, lpCmdLine, nCmdShow, SCREEN_WIDTH, SCREEN_HEIGHT);
  if (rval != S_OK)
  {
		if( ERR_FILENOTFOUND != rval )	// annunciated by InitInstance
	    DisplayErrorCode(rval);
    exit(rval);
  }

  // create the core engine
  g_pEngine = new (_NORMAL_BLOCK, __FILE__, __LINE__)C3DEngine;
  rval = g_pEngine->Init(g_hWnd, g_hInstance, g_szAppName);
  if (rval)
  {
    DisplayErrorCode(rval);
    exit(rval);
  }  

	// initialize controllers
	rval = InitControllers();
  if (rval)
  {
    DisplayErrorCode(rval);
    exit(rval);
  }

  // create a room object
  g_pRoomObject = new C3DFrameObject;
  rval = g_pRoomObject->Init(g_pEngine, g_szRoomFileName );
  if (rval)
  {
    DisplayErrorCode(rval);
    exit(rval);
  }

	// scale it
	D3DXMATRIX mScale, mIdentity;
	D3DXMatrixIdentity( &mIdentity );
	D3DXMatrixScaling( &mScale, g_fRoomScale, g_fRoomScale, g_fRoomScale );
	g_pRoomObject->SetMatrices( &mIdentity, &mScale );

  // add it to our world
  rval = g_pEngine->AddChild(g_pRoomObject);
  if (rval)
  {
    DisplayErrorCode(rval);
    exit(rval);
  }

	// message pump
  do
  {
    if (PeekMessage(&msg, g_hWnd, 0, 0,PM_REMOVE))
    {
        TranslateMessage(&msg);
        DispatchMessage(&msg);
    }
    else
    {

      // idle tasks here...

			// This is where the glove can change values
		  // Read the current values
		  float x = g_pKeyboardControl->GetRawValue("HandX");
		  float y = g_pKeyboardControl->GetRawValue("HandY");
		  float z = g_pKeyboardControl->GetRawValue("HandZ");
		  float yaw = g_pKeyboardControl->GetRawValue("HandYaw");
		  float pitch = g_pKeyboardControl->GetRawValue("HandPitch");
		  float roll = g_pKeyboardControl->GetRawValue("HandRoll");
		  float thumb = g_pKeyboardControl->GetRawValue("ThumbCurl");
		  float index = g_pKeyboardControl->GetRawValue("IndexCurl1");
		  float middle = g_pKeyboardControl->GetRawValue("MiddleCurl1");
		  float ring = g_pKeyboardControl->GetRawValue("RingCurl1");
		  float pinky = g_pKeyboardControl->GetRawValue("PinkyCurl1");
		 
		  // Allow Glove to change values
		  ReadGloveValues(x,y,z,yaw,pitch,roll,thumb,index,middle,ring,pinky);
		 
		  // Write the changes back to the keyboard control
		  g_pKeyboardControl->SetRawValue("HandX",x);
		  g_pKeyboardControl->SetRawValue("HandY",y);
		  g_pKeyboardControl->SetRawValue("HandZ",z);
		  g_pKeyboardControl->SetRawValue("HandYaw",yaw);
		  g_pKeyboardControl->SetRawValue("HandPitch",pitch);
		  g_pKeyboardControl->SetRawValue("HandRoll",roll);
		  g_pKeyboardControl->SetRawValue("ThumbCurl",thumb);
		  g_pKeyboardControl->SetRawValue("ThumbCurl2",thumb);
		  g_pKeyboardControl->SetRawValue("IndexCurl1",index);
		  g_pKeyboardControl->SetRawValue("IndexCurl2",index);
		  g_pKeyboardControl->SetRawValue("IndexCurl3",index);
		  g_pKeyboardControl->SetRawValue("MiddleCurl1",middle);
		  g_pKeyboardControl->SetRawValue("MiddleCurl2",middle);
		  g_pKeyboardControl->SetRawValue("MiddleCurl3",middle);
		  g_pKeyboardControl->SetRawValue("RingCurl1",ring);
		  g_pKeyboardControl->SetRawValue("RingCurl2",ring);
		  g_pKeyboardControl->SetRawValue("RingCurl3",ring);
		  g_pKeyboardControl->SetRawValue("PinkyCurl1",pinky);
		  g_pKeyboardControl->SetRawValue("PinkyCurl2",pinky);
		  g_pKeyboardControl->SetRawValue("PinkyCurl3",pinky);

			// render the scene
			g_pEngine->Render();

      // show the back surface
      g_pEngine->Flip();
    }

  } while (msg.message != WM_QUIT);

	// clean up
	delete g_pKeyboardControl;
	
  // destroy and delete the engine
  g_pEngine->Destroy();
  delete g_pEngine;
  g_pEngine = NULL;

	for(int n=0; n<g_iNumModels; n++)
	{
		delete g_szMenuModelNames[n];
		delete g_szModelFileNames[n];
	}
  return 0;
}
示例#15
0
/** @brief Process the GPS fix
 *
 * @return void
 */
static void ProcFix(FIX_QUALITY fix_quality)
{
	int sv, strong, weak;
	ascii buffer[8];
	ascii satBuf[1];
	double SpeedMPH = 0.0;
	static u8 OverSpeedAlarmGenerated = 0;
	adl_rtcTimeStamp_t GPSTimeStamp;
	adl_rtcTime_t GPSTime;
	static bool RTC_Update = 1;
	s32 sReturn=0;

	DumpMessage("processing proc fix\n\r");

	if ((TrackingMode == DOG_PARK_TRACKING_MODE) && FirstDogParkFix)
	{
		g_DogParkFence.posts[0].lat_32 = g_GGAInfo.Latitude;
		g_DogParkFence.posts[0].long_32 = g_GGAInfo.Longitude;
		// these are "dummy" posts since we only support >=2.
		// The dog park radius is handled in fence.c
		g_DogParkFence.posts[1].lat_32 = g_GGAInfo.Latitude;
		g_DogParkFence.posts[1].long_32 = g_GGAInfo.Longitude;
		Config_SetMode(DOG_PARK_MODE, 0, &g_DogParkFence, TRUE); // TODO add dog park fence.
		FirstDogParkFix = 0;
	}

	g_current_fix_32.lat_32 = g_GGAInfo.Latitude;
	g_current_fix_32.long_32 = g_GGAInfo.Longitude;

	// Signal strength.
	GetSV_SRN(&sv, &strong, &weak);
	g_status.GPSSignalStrength1 = '0' + strong;
	g_status.GPSSignalStrength2 = '0' + weak;

	wm_sprintf(g_traceBuf, "sig strong = %c sig weak = %c\r\n", g_status.GPSSignalStrength1, g_status.GPSSignalStrength2);
	DumpMessage(g_traceBuf);

	// Time.
	wm_itohexa((ascii *)&buffer, g_GGAInfo.Time, 8);
	memcpy(&g_current_fix.utc, &buffer, 8);

	if(RTC_Update == 1 && fix_quality == FIX_3D)
	{
		//Update RTC with GPS time only once on power up
		GPSTimeStamp.TimeStamp = g_GGAInfo.Time;
		GPSTimeStamp.SecondFracPart = 0;
		if ((sReturn = adl_rtcConvertTime(&GPSTime, &GPSTimeStamp, ADL_RTC_CONVERT_FROM_TIMESTAMP)) < 0)
		{
			DisplayErrorCode("adl_rtcConvertTime", __FILE__, __LINE__, sReturn);
		}	
		if(adl_rtcSetTime(&GPSTime) != OK)
			DumpMessage("Error setting RTC time\r\n");
		else
		{
			//Restart accelerometer timer, to avoid sudden slow idle mode, due to change in RTC time.
			//If RTC time jumps up suddenly and if the diff compared to old RTC is more than accel sleep duration, then
			//the device can enter slow idle mode immediately in Track Mode and Low Power full track mode.
			ActivateAccelOnPowerUp();
			wm_sprintf(g_traceBuf,"RTC time udpated with GPS value : 0x%x\r\n",(unsigned int)GPSTimeStamp.TimeStamp);
			DumpMessage(g_traceBuf);
			DumpMessageUSB(g_traceBuf,1);
		}
		RTC_Update = 0;
	}
	// Number of sats
	wm_itohexa((ascii *)&satBuf, g_GGAInfo.NumberOfSatillites, 1);
	memcpy(&g_status.NumBirds, &satBuf, 1);

	// lat and long
	wm_itohexa((ascii *)&buffer, g_GGAInfo.Latitude, 8);
	memcpy(&g_current_fix.lat, &buffer, 8);
	wm_itohexa((ascii *)&buffer, g_GGAInfo.Longitude, 8);
	memcpy(&g_current_fix.longi, &buffer, 8);
	// copy HDOP to where EPE used to be.
	wm_itohexa((ascii *)&buffer, g_GGAInfo.HDOP, 2);
	memcpy(&g_current_fix.epe, &buffer, 2);

	wm_sprintf(g_traceBuf, "HDOP: %c%c\r\n", buffer[1], buffer[0]);
	DumpMessage(g_traceBuf);

	// check the speed. The speed from VTGInfo is in km/hr
	// the speed from the config struct is in miles/hr
	SpeedMPH = (double)g_VTGInfo.Speed * KM_IN_MILES / METERS_IN_KM;

	if (!OverSpeedAlarmGenerated && (SpeedMPH >= (double)g_config.OverSpeedAlertThresh) && (g_config.OverSpeedAlertThresh != 0))
	{
		DumpMessage("over speed alarm detected\n\r");
		g_status.OverSpeedAlarm = 'Y';
		OverSpeedAlarmGenerated = 1;
	}
	else if (SpeedMPH < (double)g_config.OverSpeedAlertThresh)
	{
		DumpMessage("over speed alarm NOT detected\n\r");
		g_status.OverSpeedAlarm = 'N';
		OverSpeedAlarmGenerated = 0;
	}

	// write the waypoint fix to serial flash.
	WriteWaypointFromFix(&g_current_fix, g_VTGInfo.Speed, g_VTGInfo.Course, fix_quality);

	// do not check on the first dog park fix.
	DumpMessage("\r\nCheckCoords\r\n");
	if (((!FirstDogParkFix) && (TrackingMode == DOG_PARK_TRACKING_MODE)) ||
	    (TrackingMode != DOG_PARK_TRACKING_MODE))
	{
		CheckCoord();
	}
}
示例#16
0
void NextFixHandler(u8 timerid, void *context)
{
	(void)timerid;
	(void)context;

	s32 sReturn = -1;

	char prevGPSstat;

	adl_rtcTimeStamp_t CurrentTimeStamp;
	adl_rtcTimeStamp_t DeltaTimeStamp;
	TCP_STATUS status;
	static BOOL AGPSRequested=0;
	static u8 Count=0;   //Count of how many times this function was entered.
	int ret_val;

	prevGPSstat = g_status.GPSStatus;
	TRACE((1, "Next GPS Fix timer triggered"));

	// control the tracking mode state. Process the tracking mode
	// requests from the rest of the system.
	switch (TrackingMode)
	{
	case TRACKING_MODE_OFF:
        if (GetDogParkMode() == DOGPARK_ENABLED)
		{
			TrackingMode = DOG_PARK_TRACKING_MODE;
		}
		else if (AccelTrackingReq == TRACKING_MODE_REQ_ACTIVE)
		{
			TrackingMode = ACCEL_TRIG_TRACKING_MODE;
		}

		else if (TimedWakeupTrackingReq == TRACKING_MODE_REQ_ACTIVE)
		{
			TrackingMode = TIMED_WAKE_TRACKING_MODE;
		}
		else
		{
			TrackingMode = TRACKING_MODE_OFF;
		}
		break;

	case ACCEL_TRIG_TRACKING_MODE:
        if (GetDogParkMode() == DOGPARK_ENABLED)
		{
			TrackingMode = DOG_PARK_TRACKING_MODE;
		}

		else if (AccelTrackingReq == TRACKING_MODE_REQ_RELEASE)
		{
			TrackingMode = TRACKING_MODE_OFF;
		}
		else
		{
			TrackingMode = ACCEL_TRIG_TRACKING_MODE;
		}
		break;

	case DOG_PARK_TRACKING_MODE:
        if (GetDogParkMode() == DOGPARK_DISABLED)
		{
			TrackingMode = TRACKING_MODE_OFF;
		}
		else
		{
			TrackingMode = DOG_PARK_TRACKING_MODE;
		}
		break;

	case TIMED_WAKE_TRACKING_MODE:
		if (TimedWakeupTrackingReq == TRACKING_MODE_REQ_RELEASE)
		{
			TrackingMode = TRACKING_MODE_OFF;
		}
		else
		{
			TrackingMode = TIMED_WAKE_TRACKING_MODE;
		}
		break;

	default:
		DumpMessage("Undefined tracking mode\r\n");
		TrackingMode = TRACKING_MODE_OFF;
		break;
	}

	switch (gps_state)
	{
	// the GPS should be in OFF mode while in this state.
	case GPS_OFF:
		if ((TrackingMode == ACCEL_TRIG_TRACKING_MODE) || (TrackingMode == DOG_PARK_TRACKING_MODE) ||
		    (TrackingMode == TIMED_WAKE_TRACKING_MODE))
		{
			GetConvertTime(&TrackTimeStamp);
			gps_state = GPS_START;
			DumpMessage("GPS START!\r\n");
			//Reset the flag
			AGPSRequested = 0;
		}
		else
		{
			gps_state = GPS_OFF;
		}
		break;

	// attempt to start up the GPS.
	case GPS_START:
		gps_Start();

		if (!HadFix && !AttemptedAGPSNoFix)
		{
			DumpMessage("Never had fix. Going to use AGPS!\r\n");
			//performAGPS(0.0, 0.0, 0.0, 0.0);
			AttemptedAGPSNoFix = 1;
		}
		else if (HadFix)
		{
			GetConvertTime(&CurrentTimeStamp);
			//Request AGPS is last fix is more than 2 hrs old.		
			if((CurrentTimeStamp.TimeStamp - g_AGPSData.LastReqTimeStamp) >= 2*60*60)
			{
				AGPSRequested = 0;
				wm_sprintf(g_traceBuf,"g_AGPSData.LastReqTimeStamp : 0x%x, CurrentTimeStamp: 0x%x\r\n",(unsigned int)g_AGPSData.LastReqTimeStamp,(unsigned int)CurrentTimeStamp.TimeStamp);
				DumpMessage(g_traceBuf);
				DumpMessageUSB(g_traceBuf,1);				
				DumpMessage("AGPS data is old. Request fresh data\r\n");
			}

//			if (adl_rtcDiffTime(&CurrentTimeStamp, &FixTimeStamp, &DeltaTimeStamp) < 0)
//			{
//				DisplayErrorCode("adl_rtcConvertTime", __FILE__, __LINE__, sReturn);
//			}
//			else
//			{
//				wm_sprintf(g_traceBuf,"FixTimeStamp : 0x%x, CurrentTimeStamp: 0x%x\r\n",(unsigned int)FixTimeStamp.TimeStamp,(unsigned int)CurrentTimeStamp.TimeStamp);
//				DumpMessage(g_traceBuf);
//				DumpMessageUSB(g_traceBuf,1);

				//if (DeltaTimeStamp.TimeStamp >= 60 * 60 * 2)
				//{
				//	DumpMessage("Over two hours since last fix, going to use AGPS!\r\n");
				//	performAGPS(0.0, 0.0, 0.0, 0.0);					
				//}
			//}
		}

		gps_state = GPS_GET_FIX;
		DumpMessage("GET FIX!\r\n");
		//}
		break;

	// keep on the GPS until we get a fix. probably add a timeout to this...
	case GPS_GET_FIX:

		//Increment the count to keep a track of how many times this state was entered.
		Count++;
		
		// if tracking mode has been disabled...
		if (TrackingMode == TRACKING_MODE_OFF)
		{
			gps_state = GPS_OFF;
			break;
		}

		GetGGAInfo(&g_GGAInfo);
		GetVGTInfo(&g_VTGInfo);

		//Request AGPS
		GetTCPStatus(&status);

		
		if(!AGPSRequested && status == TCP_CONNECT)
		{					
			//Get the current time
			GetConvertTime(&CurrentTimeStamp);	

			//Compare the times.
			if((CurrentTimeStamp.TimeStamp - g_AGPSData.LastReqTimeStamp) < 2*60*60)  //check for < 2 hours
			{
				wm_sprintf(g_traceBuf,"CurrentTimeStamp.TimeStamp: 0x%x, g_AGPSData.LastReqTimeStamp: 0x%x\r\n", 
										(unsigned int)CurrentTimeStamp.TimeStamp,(unsigned int)g_AGPSData.LastReqTimeStamp);
				DumpMessage(g_traceBuf);
				DumpMessageUSB(g_traceBuf,1);

				//Send the last stored AGPS data to ublox directly.
				if ((ret_val = adl_fcmSendData(ubloxDataFcmHandle, (unsigned char *)g_AGPSData.AGPSData, g_AGPSData.datalen)) != OK)
				{
					wip_debug("ERROR: Could not send data to ublox device: %d\r\n", ret_val);
					wm_sprintf(g_traceBuf,"ERROR: Could not send data to ublox device: %d\r\n", ret_val);
					DumpMessageUSB(g_traceBuf,1);						
					set_agps_status(AGPS_NOT_USED);
				}
				else
				{
					AGPSRequested = 1;
					wm_sprintf(g_traceBuf,"AGPS data sent to ublox (<2 hrs case): %ld bytes\r\n", g_AGPSData.datalen);
					DumpMessage(g_traceBuf);
					DumpMessageUSB(g_traceBuf,1);
				}
					
			}
		}

		// Check if we got a fix.
		if (g_GGAInfo.FixQuality == FIX_3D)
		{
			SetFixStatus(HAD_3D_FIX);

			alarmGenerated = 0;
			GetConvertTime(&FixTimeStamp);
			gps_state = GPS_PROC_FIX;

			if (g_config.WaypointInterval >= GPS_WAYPOINT_SHUTDOWN_INTERVAL)
			{
				gps_Stop();
			}
			// GPS working
			//g_status.GPSStatus = 'W';
			// clear alarm
			if (alarm_suppress_status(GPS_ALARM_SUP) == ALARM_EXPIRED)
			{
				g_status.GPSAlarm = 'N';
				GPSAlarmGenerated = 0;
			}
			gps_state = GPS_PROC_FIX;
			DumpMessage("GPS_PROC_FIX\r\n");
		}
		else
		{
			if (g_GGAInfo.FixQuality == FIX_2D)
			{
				DumpMessage("GOT 2D FIX -- will not use\r\n");
				
				if(status == TCP_CONNECT && !AGPSRequested)
				{					
					//Request AGPS with available 2D fix
					performAGPS((g_GGAInfo.Latitude/1000000.0), (g_GGAInfo.Longitude/1000000.0), 0.0, 0.0);
					AGPSRequested = 1;
						
				}
			}
			else     //this fucntion is called once every 925ms. so 16*925 = 15 secs
			{
				//Check for the validity of WaitTimeout
				if(g_AGPSData.WaitTimeout == 0 || g_AGPSData.WaitTimeout > 99)
					g_AGPSData.WaitTimeout = 15;    //Default to 15 if found out of range
					
				//If 2D fix not received in 15 secs then request AGPS with last valid 3D fix.				
				//Request AGPS with last GPS fix				
				if(Count > g_AGPSData.WaitTimeout && status == TCP_CONNECT && !AGPSRequested )
				{					
					DumpMessage("Request fresh AGPS data\r\n");
					performAGPS(0.0, 0.0, 0.0, 0.0);
					AGPSRequested = 1;
					Count=0;
				}
			}

			g_status.GPSStatus = 'N';
			if (prevGPSstat == 'W')
			{
				TRACE((1, "Lost GPS Signal"));
			}

			gps_state = GPS_GET_FIX;
		}
		break;

	case GPS_PROC_FIX:
		ProcFix(FIX_3D);
		// GPS working
		g_status.GPSStatus = 'W';
		DumpMessage("Got a fix1\r\n");
		adl_atSendResponsePort(ADL_AT_RSP, ADL_PORT_USB, "***GOT_FIX***\r\n");
		gps_state = GPS_WAIT_NEXT_FIX;
		HadFix = 1;
		break;

	case GPS_WAIT_NEXT_FIX:
		DumpMessage("GPS_WAIT_NEXT_FIX\r\n");
		GetConvertTime(&CurrentTimeStamp);

		// time difference between last fix and current time.
		if (adl_rtcDiffTime(&CurrentTimeStamp, &FixTimeStamp, &DeltaTimeStamp) < 0)
		{
			DisplayErrorCode("adl_rtcConvertTime", __FILE__, __LINE__, sReturn);
		}

		// If the tracking interval time has elapsed,
		// go and get another fix or if the trackind mode contoller has disabled tracking,
		// go into the off state.
		if (DeltaTimeStamp.TimeStamp >= g_config.WaypointInterval)
		{
			wm_sprintf(g_traceBuf, "currenttimestamp = 0x%x fix time stamp = 0x%x delta time stamp = 0x%x waypoint interval = %d\r\n", (unsigned int )CurrentTimeStamp.TimeStamp, (unsigned int )FixTimeStamp.TimeStamp, (unsigned int )DeltaTimeStamp.TimeStamp, (int )g_config.WaypointInterval);
			DumpMessage(g_traceBuf);
			gps_state = GPS_START;
		}
		else
		{
			if (TrackingMode == TRACKING_MODE_OFF)
			{
				gps_Stop();
				gps_state = GPS_OFF;
				FirstDogParkFix = 1;
			}
			else
			{
				gps_state = GPS_WAIT_NEXT_FIX;
			}
		}

		break;

	default:
		DumpMessage("Error: Unknown GPS state\r\n");
		break;
	}
}