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;
}
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;
}
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;
}
/** @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);
}
/** @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;
}
/** @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);
}
}
}
/** @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;
}
/** @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;
}
/** @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;
}
/** @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;
}
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);
}
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;
}
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;
}
/** @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();
}
}
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;
}
}
