// optimization : This code don´t wait for data, only proccess the data available
// We can call this function on the main loop (50Hz loop)
// If we get a complete packet this function calls parse_ubx_gps() to parse and update the GPS info.
void GPS_MTK_Class::Read(void)
{
static unsigned long GPS_timer=0;
byte data;
int numc;
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) // If AtMega1280/2560 then Serial port 1...
numc = Serial1.available();
#else
numc = Serial.available();
#endif
if (numc > 0)
for (int i=0;i<numc;i++) // Process bytes received
{
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
data = Serial1.read();
#else
data = Serial.read();
#endif
switch(UBX_step) //Normally we start from zero. This is a state machine
{
case 0:
if(data==0xB5) // UBX sync char 1
UBX_step++; //OH first data packet is correct, so jump to the next step
break;
case 1:
if(data==0x62) // UBX sync char 2
UBX_step++; //ooh! The second data packet is correct, jump to the step 2
else
UBX_step=0; //Nop, is not correct so restart to step zero and try again.
break;
case 2:
UBX_class=data;
ubx_checksum(UBX_class);
UBX_step++;
break;
case 3:
UBX_id=data;
UBX_step=4;
UBX_payload_length_hi=26;
UBX_payload_length_lo=0;
UBX_payload_counter=0;
ubx_checksum(UBX_id);
break;
case 4:
if (UBX_payload_counter < UBX_payload_length_hi) // We stay in this state until we reach the payload_length
{
UBX_buffer[UBX_payload_counter] = data;
ubx_checksum(data);
UBX_payload_counter++;
if (UBX_payload_counter==UBX_payload_length_hi)
UBX_step++;
}
break;
case 5:
UBX_ck_a=data; // First checksum byte
UBX_step++;
break;
case 6:
UBX_ck_b=data; // Second checksum byte
// We end the GPS read...
if((ck_a==UBX_ck_a)&&(ck_b==UBX_ck_b)) // Verify the received checksum with the generated checksum..
parse_ubx_gps(); // Parse the new GPS packet
else
{
if (PrintErrors)
Serial.println("ERR:GPS_CHK!!");
}
// Variable initialization
UBX_step=0;
ck_a=0;
ck_b=0;
GPS_timer=millis(); //Restarting timer...
break;
}
} // End for...
// If we don´t receive GPS packets in 2 seconds => Bad FIX state
if ((millis() - GPS_timer)>2000)
{
Fix = 0;
if (PrintErrors)
Serial.println("ERR:GPS_TIMEOUT!!");
}
}
// optimization : This code don´t wait for data, only proccess the data available
// We can call this function on the main loop (50Hz loop)
// If we get a complete packet this function calls parse_ubx_gps() to parse and update the GPS info.
void GPS_UBLOX_Class::Read(void)
{
static unsigned long GPS_timer=0;
byte data;
int numc;
numc = Serial.available();
if (numc > 0)
for (int i=0; i<numc; i++) // Process bytes received
{
data = Serial.read();
switch(UBX_step) //Normally we start from zero. This is a state machine
{
case 0:
if(data==0xB5) // UBX sync char 1
UBX_step++; //OH first data packet is correct, so jump to the next step
break;
case 1:
if(data==0x62) // UBX sync char 2
UBX_step++; //ooh! The second data packet is correct, jump to the step 2
else
UBX_step=0; //Nop, is not correct so restart to step zero and try again.
break;
case 2:
UBX_class=data;
ubx_checksum(UBX_class);
UBX_step++;
break;
case 3:
UBX_id=data;
ubx_checksum(UBX_id);
UBX_step++;
break;
case 4:
UBX_payload_length_hi=data;
ubx_checksum(UBX_payload_length_hi);
UBX_step++;
// We check if the payload lenght is valid...
if (UBX_payload_length_hi>=UBX_MAXPAYLOAD)
{
if (PrintErrors)
Serial.println("ERR:GPS_BAD_PAYLOAD_LENGTH!!");
UBX_step=0; //Bad data, so restart to step zero and try again.
ck_a=0;
ck_b=0;
}
break;
case 5:
UBX_payload_length_lo=data;
ubx_checksum(UBX_payload_length_lo);
UBX_step++;
UBX_payload_counter=0;
break;
case 6: // Payload data read...
if (UBX_payload_counter < UBX_payload_length_hi) // We stay in this state until we reach the payload_length
{
UBX_buffer[UBX_payload_counter] = data;
ubx_checksum(data);
UBX_payload_counter++;
if (UBX_payload_counter==UBX_payload_length_hi)
UBX_step++;
}
break;
case 7:
UBX_ck_a=data; // First checksum byte
UBX_step++;
break;
case 8:
UBX_ck_b=data; // Second checksum byte
// We end the GPS read...
if((ck_a==UBX_ck_a)&&(ck_b==UBX_ck_b)) // Verify the received checksum with the generated checksum..
parse_ubx_gps(); // Parse the new GPS packet
else
{
if (PrintErrors)
Serial.println("ERR:GPS_CHK!!");
}
// Variable initialization
UBX_step=0;
ck_a=0;
ck_b=0;
GPS_timer=millis(); //Restarting timer...
break;
}
} // End for...
// If we don´t receive GPS packets in 2 seconds => Bad FIX state
if ((millis() - GPS_timer)>2000)
{
Fix = 0;
if (PrintErrors)
Serial.println("ERR:GPS_TIMEOUT!!");
}
}
void Read(void)
{
static unsigned long GPS_timer=0;
uint8_t data;
int numc;
numc = ((getdataz())/(getdataz()));
if (numc > 0)
//i = 0;
for (i=0;i<numc;i++) // Process bytes received
{
getdataz();
data = getdataz();
switch(UBX_step)
{
case 0:
if(data==0xB5)
UBX_step++;
break;
case 1:
if(data==0x62)
UBX_step++;
else
UBX_step=0;
break;
case 2:
UBX_class=data;
ubx_checksum(UBX_class);
UBX_step++;
break;
case 3:
UBX_id=data;
ubx_checksum(UBX_id);
UBX_step++;
break;
case 4:
UBX_length_hi=data;
ubx_checksum(UBX_length_hi);
UBX_step++;
if (UBX_length_hi>=UBX_MAX_SIZE)
{
if (PrintErrors)
UARTprintf("ERR:GPS_BAD_PAYLOAD_LENGTH!!");
UBX_step=0;
ck_a=0;
ck_b=0;
}
break;
case 5:
UBX_length_lo=data;
ubx_checksum(UBX_length_lo);
UBX_step++;
UBX_counter=0;
break;
case 6:
if (UBX_counter < UBX_length_hi)
{
UBX_buffer[UBX_counter] = data;
ubx_checksum(data);
UBX_counter++;
if (UBX_counter==UBX_length_hi)
UBX_step++;
}
break;
case 7:
UBX_ck_a=data;
// Serial.println(UBX_ck_a);
UBX_step++;
break;
case 8:
UBX_ck_b=data;
// Serial.println(UBX_ck_b);
if((ck_a==UBX_ck_a)&&(ck_b==UBX_ck_b))
parse_ubx_gps();
else
{
if (PrintErrors)
UARTprintf("ERR:GPS_CHK!!");
}
UBX_step=0;
ck_a=0;
ck_b=0;
GPS_timer=millis();
break;
}
}
if ((millis() - GPS_timer)>2000)
{
Fix = 0;
if (PrintErrors)
UARTprintf("ERR:GPS_TIMEOUT!!");
}
}