bool GPSSensorUBX::GetPosition(int32_t *lat, int32_t *lon, int32_t *alt, uint32_t *hAcc, uint32_t *vAcc) {
//DBG("TX NAVPOS");
PollCmd(UBXMsgClassNAV, UBXMsgIdNAV_POSLLH);
//DBG("RX NAVPOS");
if(ReceiveFrame(10000)){
if((recvBuff[0] == 0xb5) && (recvBuff[1] == 0x62)) {
//DBG("There is a Frame in buffer");
if((recvBuff[2] == 0x01) && (recvBuff[3] == 0x02)) {
DBG("OK this is a POSLLH frame");
UBXNAV_POSLLH *pPosLlH = (UBXNAV_POSLLH*)(recvBuff+6);
*lat = pPosLlH->lat;
*lon = pPosLlH->lon;
*alt = pPosLlH->height;
*hAcc = pPosLlH->hAcc;
*vAcc = pPosLlH->vAcc;
DBG("%08x %08x %08x %08x %08x",*lat,*lon,*alt,*hAcc,*vAcc);
DBG("%ld %ld %ld %ld %ld",*lat,*lon,*alt,*hAcc,*vAcc);
if((*lat != 0) & (*lon != 0)) {
DBG("VALID POS");
fixed = true;
return true;
}
}
}
}
return false;
}
// Poll timer callback implementation: call PollCmd and set completion event
VOID
CALLBACK PollTimerCb(PVOID lpParameter, BOOL TimerOrWaitFired)
{
HRESULT hRes;
UNREFERENCED_PARAMETER(lpParameter);
UNREFERENCED_PARAMETER(TimerOrWaitFired);
hRes = PollCmd();
// If an error occured in PollCmd() the timer callback is deleted.
// (typically happens when client has closed the connection)
if (FAILED(hRes))
ReleasePollTimer();
}
bool GPSSensorUBX::GetTime(uint32_t *date,uint32_t *time) {
DBG("TX TIMEUTC");
PollCmd(UBXMsgClassNAV, UBXMsgIdNAV_TIMEUTC);
DBG("RX TIMEUTC");
if(ReceiveFrame(10000)){
if((recvBuff[0] == 0xb5) && (recvBuff[1] == 0x62)) {
DBG("There is a Frame in buffer");
if((recvBuff[2] == 0x01) && (recvBuff[3] == 0x21)) {
DBG("OK This is a TIMEUTC Frame");
UBXNAV_TIMEUTC *pTimeUTC= (UBXNAV_TIMEUTC*)(recvBuff+6);
*date = (pTimeUTC->year>2000)?(pTimeUTC->year-2000):pTimeUTC->year;
*date += (pTimeUTC->month * 100);
*date += (pTimeUTC->day * 10000);
*time = pTimeUTC->sec;
*time *= pTimeUTC->min;
*time *= pTimeUTC->hour;
return true;
}
}
}
return false;
}
void GPSSensorUBX::NAV_RATE(void) {
DBG("TX check Nav Rate\n");
PollCmd(UBXMsgClassCFG, UBXMsgIdCFG_RATE );
}
void GPSSensorUBX::NAV_POSLLH(void) {
//DBG("Tx NavPos\n");
PollCmd(UBXMsgClassNAV, UBXMsgIdNAV_POSLLH);
}
void GPSSensorUBX::MON_VER(void) {
DBG("TX checkVersion\n");
PollCmd(UBXMsgClassMON, UBXMsgIdMON_VER);
}