// 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!!");
}
}
// 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!!");
}
}
int ubx_parse(uint8_t b, char *gps_rx_buffer)
{
//printf("b=%x\n",b);
if (ubx_state->decode_state == UBX_DECODE_UNINIT) {
if (b == UBX_SYNC_1) {
ubx_state->decode_state = UBX_DECODE_GOT_SYNC1;
}
} else if (ubx_state->decode_state == UBX_DECODE_GOT_SYNC1) {
if (b == UBX_SYNC_2) {
ubx_state->decode_state = UBX_DECODE_GOT_SYNC2;
} else {
// Second start symbol was wrong, reset state machine
ubx_decode_init();
}
} else if (ubx_state->decode_state == UBX_DECODE_GOT_SYNC2) {
// Add to checksum
ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b));
//check for known class
switch (b) {
case UBX_CLASS_ACK:
ubx_state->decode_state = UBX_DECODE_GOT_CLASS;
ubx_state->message_class = ACK;
break;
case UBX_CLASS_NAV:
ubx_state->decode_state = UBX_DECODE_GOT_CLASS;
ubx_state->message_class = NAV;
break;
case UBX_CLASS_RXM:
ubx_state->decode_state = UBX_DECODE_GOT_CLASS;
ubx_state->message_class = RXM;
break;
case UBX_CLASS_CFG:
ubx_state->decode_state = UBX_DECODE_GOT_CLASS;
ubx_state->message_class = CFG;
break;
default: //unknown class: reset state machine
ubx_decode_init();
break;
}
} else if (ubx_state->decode_state == UBX_DECODE_GOT_CLASS) {
// Add to checksum
ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b));
//depending on class look for message id
switch (ubx_state->message_class) {
case NAV:
switch (b) {
case UBX_MESSAGE_NAV_POSLLH: //NAV-POSLLH: Geodetic Position Solution
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = NAV_POSLLH;
break;
case UBX_MESSAGE_NAV_SOL:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = NAV_SOL;
break;
case UBX_MESSAGE_NAV_TIMEUTC:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = NAV_TIMEUTC;
break;
case UBX_MESSAGE_NAV_DOP:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = NAV_DOP;
break;
case UBX_MESSAGE_NAV_SVINFO:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = NAV_SVINFO;
break;
case UBX_MESSAGE_NAV_VELNED:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = NAV_VELNED;
break;
default: //unknown class: reset state machine, should not happen
ubx_decode_init();
break;
}
break;
case RXM:
switch (b) {
case UBX_MESSAGE_RXM_SVSI:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = RXM_SVSI;
break;
default: //unknown class: reset state machine, should not happen
ubx_decode_init();
break;
}
break;
case CFG:
switch (b) {
case UBX_MESSAGE_CFG_NAV5:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = CFG_NAV5;
break;
default: //unknown class: reset state machine, should not happen
ubx_decode_init();
break;
}
break;
case ACK:
switch (b) {
case UBX_MESSAGE_ACK_ACK:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = ACK_ACK;
break;
case UBX_MESSAGE_ACK_NAK:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = ACK_NAK;
break;
default: //unknown class: reset state machine, should not happen
ubx_decode_init();
break;
}
break;
default: //should not happen
ubx_decode_init();
break;
}
} else if (ubx_state->decode_state == UBX_DECODE_GOT_MESSAGEID) {
// Add to checksum
ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b));
ubx_state->payload_size = b;
ubx_state->decode_state = UBX_DECODE_GOT_LENGTH1;
} else if (ubx_state->decode_state == UBX_DECODE_GOT_LENGTH1) {
// Add to checksum
ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b));
ubx_state->payload_size += b << 8;
ubx_state->decode_state = UBX_DECODE_GOT_LENGTH2;
} else if (ubx_state->decode_state == UBX_DECODE_GOT_LENGTH2) {
uint8_t ret = 0;
// Add to checksum if not yet at checksum byte
if (ubx_state->rx_count < ubx_state->payload_size) ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b));
// Fill packet buffer
gps_rx_buffer[ubx_state->rx_count] = b;
//if whole payload + checksum is in buffer:
if (ubx_state->rx_count >= ubx_state->payload_size + 1) {
//convert to correct struct
switch (ubx_state->message_id) { //this enum is unique for all ids --> no need to check the class
case NAV_POSLLH: {
// printf("GOT NAV_POSLLH MESSAGE\n");
gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) gps_rx_buffer;
//Check if checksum is valid and the store the gps information
if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
ubx_gps->lat = packet->lat;
ubx_gps->lon = packet->lon;
ubx_gps->alt = packet->height_msl;
ubx_gps->counter_pos_valid++;
ubx_gps->timestamp = hrt_absolute_time();
ubx_gps->counter++;
//pthread_mutex_lock(ubx_mutex);
ubx_state->last_message_timestamps[NAV_POSLLH - 1] = hrt_absolute_time();
//pthread_mutex_unlock(ubx_mutex);
ret = 1;
} else {
if (gps_verbose) printf("[gps] NAV_POSLLH: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
case NAV_SOL: {
// printf("GOT NAV_SOL MESSAGE\n");
gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) gps_rx_buffer;
//Check if checksum is valid and the store the gps information
if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
ubx_gps->fix_type = packet->gpsFix;
ubx_gps->timestamp = hrt_absolute_time();
ubx_gps->counter++;
ubx_gps->s_variance = packet->sAcc;
ubx_gps->p_variance = packet->pAcc;
//pthread_mutex_lock(ubx_mutex);
ubx_state->last_message_timestamps[NAV_SOL - 1] = hrt_absolute_time();
//pthread_mutex_unlock(ubx_mutex);
ret = 1;
} else {
if (gps_verbose) printf("[gps] NAV_SOL: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
case NAV_DOP: {
// printf("GOT NAV_DOP MESSAGE\n");
gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) gps_rx_buffer;
//Check if checksum is valid and the store the gps information
if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
ubx_gps->eph = packet->hDOP;
ubx_gps->epv = packet->vDOP;
ubx_gps->timestamp = hrt_absolute_time();
ubx_gps->counter++;
//pthread_mutex_lock(ubx_mutex);
ubx_state->last_message_timestamps[NAV_DOP - 1] = hrt_absolute_time();
//pthread_mutex_unlock(ubx_mutex);
ret = 1;
} else {
if (gps_verbose) printf("[gps] NAV_DOP: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
case NAV_TIMEUTC: {
// printf("GOT NAV_TIMEUTC MESSAGE\n");
gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) gps_rx_buffer;
//Check if checksum is valid and the store the gps information
if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
//convert to unix timestamp
struct tm timeinfo;
timeinfo.tm_year = packet->year - 1900;
timeinfo.tm_mon = packet->month - 1;
timeinfo.tm_mday = packet->day;
timeinfo.tm_hour = packet->hour;
timeinfo.tm_min = packet->min;
timeinfo.tm_sec = packet->sec;
time_t epoch = mktime(&timeinfo);
// printf("%d.%d.%d %d:%d:%d:%d\n", timeinfo.tm_year, timeinfo.tm_mon, timeinfo.tm_mday, timeinfo.tm_hour, timeinfo.tm_min, timeinfo.tm_sec, packet->time_nanoseconds);
ubx_gps->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
ubx_gps->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
ubx_gps->timestamp = hrt_absolute_time();
ubx_gps->counter++;
//pthread_mutex_lock(ubx_mutex);
ubx_state->last_message_timestamps[NAV_TIMEUTC - 1] = hrt_absolute_time();
//pthread_mutex_unlock(ubx_mutex);
ret = 1;
} else {
if (gps_verbose) printf("\t[gps] NAV_TIMEUTC: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
case NAV_SVINFO: {
// printf("GOT NAV_SVINFO MESSAGE\n");
//this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message
const int length_part1 = 8;
char gps_rx_buffer_part1[length_part1];
memcpy(gps_rx_buffer_part1, gps_rx_buffer, length_part1);
gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) gps_rx_buffer_part1;
//read checksum
const int length_part3 = 2;
char gps_rx_buffer_part3[length_part3];
memcpy(gps_rx_buffer_part3, &(gps_rx_buffer[ubx_state->rx_count - 1]), length_part3);
gps_bin_nav_svinfo_part3_packet_t *packet_part3 = (gps_bin_nav_svinfo_part3_packet_t *) gps_rx_buffer_part3;
//Check if checksum is valid and then store the gps information
if (ubx_state->ck_a == packet_part3->ck_a && ubx_state->ck_b == packet_part3->ck_b) {
//definitions needed to read numCh elements from the buffer:
const int length_part2 = 12;
gps_bin_nav_svinfo_part2_packet_t *packet_part2;
char gps_rx_buffer_part2[length_part2]; //for temporal storage
int i;
for (i = 0; i < packet_part1->numCh; i++) { //for each channel
/* Get satellite information from the buffer */
memcpy(gps_rx_buffer_part2, &(gps_rx_buffer[length_part1 + i * length_part2]), length_part2);
packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) gps_rx_buffer_part2;
/* Write satellite information in the global storage */
ubx_gps->satellite_prn[i] = packet_part2->svid;
//if satellite information is healthy store the data
uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield
if (!unhealthy) {
if ((packet_part2->flags) & 1) { //flags is a bitfield
ubx_gps->satellite_used[i] = 1;
} else {
ubx_gps->satellite_used[i] = 0;
}
ubx_gps->satellite_snr[i] = packet_part2->cno;
ubx_gps->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
ubx_gps->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
} else {
ubx_gps->satellite_used[i] = 0;
ubx_gps->satellite_snr[i] = 0;
ubx_gps->satellite_elevation[i] = 0;
ubx_gps->satellite_azimuth[i] = 0;
}
}
for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused
/* Unused channels have to be set to zero for e.g. MAVLink */
ubx_gps->satellite_prn[i] = 0;
ubx_gps->satellite_used[i] = 0;
ubx_gps->satellite_snr[i] = 0;
ubx_gps->satellite_elevation[i] = 0;
ubx_gps->satellite_azimuth[i] = 0;
}
/* set flag if any sat info is available */
if (!packet_part1->numCh > 0) {
ubx_gps->satellite_info_available = 1;
} else {
ubx_gps->satellite_info_available = 0;
}
ubx_gps->timestamp = hrt_absolute_time();
ubx_gps->counter++;
//pthread_mutex_lock(ubx_mutex);
ubx_state->last_message_timestamps[NAV_SVINFO - 1] = hrt_absolute_time();
//pthread_mutex_unlock(ubx_mutex);
ret = 1;
} else {
if (gps_verbose) printf("\t[gps] NAV_SVINFO: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
case NAV_VELNED: {
// printf("GOT NAV_VELNED MESSAGE\n");
gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) gps_rx_buffer;
//Check if checksum is valid and the store the gps information
if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
ubx_gps->vel = (uint16_t)packet->speed;
ubx_gps->vel_n = packet->velN / 100.0f;
ubx_gps->vel_e = packet->velE / 100.0f;
ubx_gps->vel_d = packet->velD / 100.0f;
ubx_gps->vel_ned_valid = true;
ubx_gps->cog = (uint16_t)((float)(packet->heading) * 1e-3f);
ubx_gps->timestamp = hrt_absolute_time();
ubx_gps->counter++;
//pthread_mutex_lock(ubx_mutex);
ubx_state->last_message_timestamps[NAV_VELNED - 1] = hrt_absolute_time();
//pthread_mutex_unlock(ubx_mutex);
ret = 1;
} else {
if (gps_verbose) printf("[gps] NAV_VELNED: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
case RXM_SVSI: {
// printf("GOT RXM_SVSI MESSAGE\n");
const int length_part1 = 7;
char gps_rx_buffer_part1[length_part1];
memcpy(gps_rx_buffer_part1, gps_rx_buffer, length_part1);
gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) gps_rx_buffer_part1;
//Check if checksum is valid and the store the gps information
if (ubx_state->ck_a == gps_rx_buffer[ubx_state->rx_count - 1] && ubx_state->ck_b == gps_rx_buffer[ubx_state->rx_count]) {
ubx_gps->satellites_visible = packet->numVis;
ubx_gps->timestamp = hrt_absolute_time();
ubx_gps->counter++;
//pthread_mutex_lock(ubx_mutex);
ubx_state->last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time();
//pthread_mutex_unlock(ubx_mutex);
ret = 1;
} else {
if (gps_verbose) printf("[gps] RXM_SVSI: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
case ACK_ACK: {
// printf("GOT ACK_ACK\n");
gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) gps_rx_buffer;
//Check if checksum is valid
if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
switch (ubx_config_state) {
case UBX_CONFIG_STATE_PRT:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT)
ubx_config_state++;
break;
case UBX_CONFIG_STATE_NAV5:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_NAV5)
ubx_config_state++;
break;
case UBX_CONFIG_STATE_MSG_NAV_POSLLH:
case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC:
case UBX_CONFIG_STATE_MSG_NAV_DOP:
case UBX_CONFIG_STATE_MSG_NAV_SVINFO:
case UBX_CONFIG_STATE_MSG_NAV_SOL:
case UBX_CONFIG_STATE_MSG_NAV_VELNED:
case UBX_CONFIG_STATE_MSG_RXM_SVSI:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
ubx_config_state++;
break;
default:
break;
}
ret = 1;
} else {
if (gps_verbose) printf("[gps] ACK_ACK: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
case ACK_NAK: {
// printf("GOT ACK_NAK\n");
gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) gps_rx_buffer;
//Check if checksum is valid
if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
if (gps_verbose) printf("[gps] the ubx gps returned: not acknowledged\n");
ret = 1;
} else {
if (gps_verbose) printf("[gps] ACK_NAK: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
default: //something went wrong
ubx_decode_init();
break;
}
}
(ubx_state->rx_count)++;
}
return 0; // no valid packet found
}
