/**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /* @file U-Blox protocol implementation */ #include #include #include #include #include #include #include #include #include #include "ubx.h" UBX::UBX() : _config_state(UBX_CONFIG_STATE_PRT), _waiting_for_ack(false) { reset(); } UBX::~UBX() { } void UBX::reset() { decodeInit(); _config_state = UBX_CONFIG_STATE_PRT; _waiting_for_ack = false; } void UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate) { /* make sure the buffer, where the message is written to, is long enough */ assert(sizeof(type_gps_bin_cfg_prt_packet_t)+2 <= max_length); /* Only send a new config message when we got the ACK of the last one, * otherwise we might not configure all the messages because the ACK comes from an older/previos CFD command * reason being that the ACK only includes CFG-MSG but not to which NAV MSG it belongs. */ if (!_waiting_for_ack) { _waiting_for_ack = true; if (_config_state == UBX_CONFIG_STATE_CONFIGURED) { /* This should never happen, the parser should set the flag, * if it should be reconfigured, reset() should be called first. */ warnx("ubx: already configured"); _waiting_for_ack = false; return; } else if (_config_state == UBX_CONFIG_STATE_PRT) { /* Send a CFG-PRT message to set the UBX protocol for in and out * and leave the baudrate as it is, we just want an ACK-ACK from this */ type_gps_bin_cfg_prt_packet_t cfg_prt_packet; /* Set everything else of the packet to 0, otherwise the module wont accept it */ memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); /* Define the package contents, don't change the baudrate */ cfg_prt_packet.clsID = UBX_CLASS_CFG; cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; cfg_prt_packet.baudRate = baudrate; cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; /* Calculate the checksum now */ addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); /* Start with the two sync bytes */ buffer[0] = UBX_SYNC1; buffer[1] = UBX_SYNC2; /* Copy it to the buffer that will be written back in the main gps driver */ memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet)); /* Set the length of the packet (plus the 2 sync bytes) */ length = sizeof(cfg_prt_packet)+2; } else if (_config_state == UBX_CONFIG_STATE_PRT_NEW_BAUDRATE) { /* Send a CFG-PRT message again, this time change the baudrate */ type_gps_bin_cfg_prt_packet_t cfg_prt_packet; memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); cfg_prt_packet.clsID = UBX_CLASS_CFG; cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); buffer[0] = UBX_SYNC1; buffer[1] = UBX_SYNC2; memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet)); length = sizeof(cfg_prt_packet)+2; /* If the new baudrate will be different from the current one, we should report that back to the driver */ if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { baudrate=UBX_CFG_PRT_PAYLOAD_BAUDRATE; baudrate_changed = true; /* Don't wait for an ACK, we're switching baudrate and we might never get, * after that, start fresh */ reset(); } } else if (_config_state == UBX_CONFIG_STATE_RATE) { /* send a CFT-RATE message to define update rate */ type_gps_bin_cfg_rate_packet_t cfg_rate_packet; memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); cfg_rate_packet.clsID = UBX_CLASS_CFG; cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE; cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; addChecksumToMessage((uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); buffer[0] = UBX_SYNC1; buffer[1] = UBX_SYNC2; memcpy(&(buffer[2]), &cfg_rate_packet, sizeof(cfg_rate_packet)); length = sizeof(cfg_rate_packet)+2; } else if (_config_state == UBX_CONFIG_STATE_NAV5) { /* send a NAV5 message to set the options for the internal filter */ type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); cfg_nav5_packet.clsID = UBX_CLASS_CFG; cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; addChecksumToMessage((uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); buffer[0] = UBX_SYNC1; buffer[1] = UBX_SYNC2; memcpy(&(buffer[2]), &cfg_nav5_packet, sizeof(cfg_nav5_packet)); length = sizeof(cfg_nav5_packet)+2; } else { /* Catch the remaining config states here, they all need the same packet type */ type_gps_bin_cfg_msg_packet_t cfg_msg_packet; memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); cfg_msg_packet.clsID = UBX_CLASS_CFG; cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; /* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */ cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ; switch (_config_state) { case UBX_CONFIG_STATE_MSG_NAV_POSLLH: cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; break; case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; break; // case UBX_CONFIG_STATE_MSG_NAV_DOP: // cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; // cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; // break; case UBX_CONFIG_STATE_MSG_NAV_SVINFO: cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; /* For satelites info 1Hz is enough */ cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ; break; case UBX_CONFIG_STATE_MSG_NAV_SOL: cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; break; case UBX_CONFIG_STATE_MSG_NAV_VELNED: cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; break; // case UBX_CONFIG_STATE_MSG_RXM_SVSI: // cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; // cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; // break; default: break; } addChecksumToMessage((uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); buffer[0] = UBX_SYNC1; buffer[1] = UBX_SYNC2; memcpy(&(buffer[2]), &cfg_msg_packet, sizeof(cfg_msg_packet)); length = sizeof(cfg_msg_packet)+2; } } } void UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_needed, bool &pos_updated) { switch (_decode_state) { /* First, look for sync1 */ case UBX_DECODE_UNINIT: if (b == UBX_SYNC1) { _decode_state = UBX_DECODE_GOT_SYNC1; } break; /* Second, look for sync2 */ case UBX_DECODE_GOT_SYNC1: if (b == UBX_SYNC2) { _decode_state = UBX_DECODE_GOT_SYNC2; } else { /* Second start symbol was wrong, reset state machine */ decodeInit(); } break; /* Now look for class */ case UBX_DECODE_GOT_SYNC2: /* everything except sync1 and sync2 needs to be added to the checksum */ addByteToChecksum(b); /* check for known class */ switch (b) { case UBX_CLASS_ACK: _decode_state = UBX_DECODE_GOT_CLASS; _message_class = ACK; break; case UBX_CLASS_NAV: _decode_state = UBX_DECODE_GOT_CLASS; _message_class = NAV; break; // case UBX_CLASS_RXM: // _decode_state = UBX_DECODE_GOT_CLASS; // _message_class = RXM; // break; case UBX_CLASS_CFG: _decode_state = UBX_DECODE_GOT_CLASS; _message_class = CFG; break; default: //unknown class: reset state machine decodeInit(); break; } break; case UBX_DECODE_GOT_CLASS: addByteToChecksum(b); switch (_message_class) { case NAV: switch (b) { case UBX_MESSAGE_NAV_POSLLH: _decode_state = UBX_DECODE_GOT_MESSAGEID; _message_id = NAV_POSLLH; break; case UBX_MESSAGE_NAV_SOL: _decode_state = UBX_DECODE_GOT_MESSAGEID; _message_id = NAV_SOL; break; case UBX_MESSAGE_NAV_TIMEUTC: _decode_state = UBX_DECODE_GOT_MESSAGEID; _message_id = NAV_TIMEUTC; break; // case UBX_MESSAGE_NAV_DOP: // _decode_state = UBX_DECODE_GOT_MESSAGEID; // _message_id = NAV_DOP; // break; case UBX_MESSAGE_NAV_SVINFO: _decode_state = UBX_DECODE_GOT_MESSAGEID; _message_id = NAV_SVINFO; break; case UBX_MESSAGE_NAV_VELNED: _decode_state = UBX_DECODE_GOT_MESSAGEID; _message_id = NAV_VELNED; break; default: //unknown class: reset state machine, should not happen decodeInit(); break; } break; // case RXM: // switch (b) { // case UBX_MESSAGE_RXM_SVSI: // _decode_state = UBX_DECODE_GOT_MESSAGEID; // _message_id = RXM_SVSI; // break; // // default: //unknown class: reset state machine, should not happen // decodeInit(); // break; // } // break; case CFG: switch (b) { case UBX_MESSAGE_CFG_NAV5: _decode_state = UBX_DECODE_GOT_MESSAGEID; _message_id = CFG_NAV5; break; default: //unknown class: reset state machine, should not happen decodeInit(); break; } break; case ACK: switch (b) { case UBX_MESSAGE_ACK_ACK: _decode_state = UBX_DECODE_GOT_MESSAGEID; _message_id = ACK_ACK; break; case UBX_MESSAGE_ACK_NAK: _decode_state = UBX_DECODE_GOT_MESSAGEID; _message_id = ACK_NAK; break; default: //unknown class: reset state machine, should not happen decodeInit(); break; } break; default: //should not happen because we set the class warnx("UBX Error, we set a class that we don't know"); decodeInit(); config_needed = true; break; } break; case UBX_DECODE_GOT_MESSAGEID: addByteToChecksum(b); _payload_size = b; //this is the first length byte _decode_state = UBX_DECODE_GOT_LENGTH1; break; case UBX_DECODE_GOT_LENGTH1: addByteToChecksum(b); _payload_size += b << 8; // here comes the second byte of length _decode_state = UBX_DECODE_GOT_LENGTH2; break; case UBX_DECODE_GOT_LENGTH2: /* Add to checksum if not yet at checksum byte */ if (_rx_count < _payload_size) addByteToChecksum(b); _rx_buffer[_rx_count] = b; /* once the payload has arrived, we can process the information */ if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes switch (_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 *) _rx_buffer; //Check if checksum is valid and the store the gps information if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { gps_position->lat = packet->lat; gps_position->lon = packet->lon; gps_position->alt = packet->height_msl; gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m /* Add timestamp to finish the report */ gps_position->timestamp_position = hrt_absolute_time(); /* set flag to trigger publishing of new position */ pos_updated = true; } else { warnx("NAV_POSLLH: checksum invalid"); } // Reset state machine to decode next packet decodeInit(); break; } case NAV_SOL: { // printf("GOT NAV_SOL MESSAGE\n"); gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; //Check if checksum is valid and the store the gps information if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { gps_position->fix_type = packet->gpsFix; gps_position->s_variance_m_s = packet->sAcc; gps_position->p_variance_m = packet->pAcc; gps_position->timestamp_variance = hrt_absolute_time(); } else { warnx("NAV_SOL: checksum invalid"); } // Reset state machine to decode next packet decodeInit(); break; } // case NAV_DOP: { //// printf("GOT NAV_DOP MESSAGE\n"); // gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; // // //Check if checksum is valid and the store the gps information // if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { // // gps_position->eph_m = packet->hDOP; // gps_position->epv = packet->vDOP; // // gps_position->timestamp_posdilution = hrt_absolute_time(); // // _new_nav_dop = true; // // } else { // warnx("NAV_DOP: checksum invalid"); // } // // // Reset state machine to decode next packet // decodeInit(); // break; // } case NAV_TIMEUTC: { // printf("GOT NAV_TIMEUTC MESSAGE\n"); gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; //Check if checksum is valid and the store the gps information if (_rx_ck_a == packet->ck_a && _rx_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); gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); gps_position->timestamp_time = hrt_absolute_time(); } else { warnx("NAV_TIMEUTC: checksum invalid"); } // Reset state machine to decode next packet decodeInit(); 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 _rx_buffer_part1[length_part1]; memcpy(_rx_buffer_part1, _rx_buffer, length_part1); gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1; //read checksum const int length_part3 = 2; char _rx_buffer_part3[length_part3]; memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3); gps_bin_nav_svinfo_part3_packet_t *packet_part3 = (gps_bin_nav_svinfo_part3_packet_t *) _rx_buffer_part3; //Check if checksum is valid and then store the gps information if (_rx_ck_a == packet_part3->ck_a && _rx_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 _rx_buffer_part2[length_part2]; //for temporal storage uint8_t satellites_used = 0; int i; for (i = 0; i < packet_part1->numCh; i++) { //for each channel /* Get satellite information from the buffer */ memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2); packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2; /* Write satellite information in the global storage */ gps_position->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 gps_position->satellite_used[i] = 1; satellites_used++; } else { gps_position->satellite_used[i] = 0; } gps_position->satellite_snr[i] = packet_part2->cno; gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); } else { gps_position->satellite_used[i] = 0; gps_position->satellite_snr[i] = 0; gps_position->satellite_elevation[i] = 0; gps_position->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 */ gps_position->satellite_prn[i] = 0; gps_position->satellite_used[i] = 0; gps_position->satellite_snr[i] = 0; gps_position->satellite_elevation[i] = 0; gps_position->satellite_azimuth[i] = 0; } gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones /* set timestamp if any sat info is available */ if (packet_part1->numCh > 0) { gps_position->satellite_info_available = true; } else { gps_position->satellite_info_available = false; } gps_position->timestamp_satellites = hrt_absolute_time(); } else { warnx("NAV_SVINFO: checksum invalid"); } // Reset state machine to decode next packet decodeInit(); break; } case NAV_VELNED: { // printf("GOT NAV_VELNED MESSAGE\n"); gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; //Check if checksum is valid and the store the gps information if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { gps_position->vel_m_s = (float)packet->speed * 1e-2f; gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; gps_position->vel_ned_valid = true; gps_position->timestamp_velocity = hrt_absolute_time(); } else { warnx("NAV_VELNED: checksum invalid"); } // Reset state machine to decode next packet decodeInit(); break; } // case RXM_SVSI: { // printf("GOT RXM_SVSI MESSAGE\n"); // const int length_part1 = 7; // char _rx_buffer_part1[length_part1]; // memcpy(_rx_buffer_part1, _rx_buffer, length_part1); // gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1; // // //Check if checksum is valid and the store the gps information // if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { // // gps_position->satellites_visible = packet->numVis; // gps_position->counter++; // _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); // ret = 1; // // } else { // warnx("RXM_SVSI: checksum invalid\n"); // } // // // Reset state machine to decode next packet // decodeInit(); // break; // } case ACK_ACK: { // printf("GOT ACK_ACK\n"); gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; //Check if checksum is valid if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { _waiting_for_ack = false; switch (_config_state) { case UBX_CONFIG_STATE_PRT: if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) _config_state = UBX_CONFIG_STATE_PRT_NEW_BAUDRATE; break; case UBX_CONFIG_STATE_PRT_NEW_BAUDRATE: if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) _config_state = UBX_CONFIG_STATE_RATE; break; case UBX_CONFIG_STATE_RATE: if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_RATE) _config_state = UBX_CONFIG_STATE_NAV5; break; case UBX_CONFIG_STATE_NAV5: if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_NAV5) _config_state = UBX_CONFIG_STATE_MSG_NAV_POSLLH; break; case UBX_CONFIG_STATE_MSG_NAV_POSLLH: if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) _config_state = UBX_CONFIG_STATE_MSG_NAV_TIMEUTC; break; case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) _config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO; break; // case UBX_CONFIG_STATE_MSG_NAV_DOP: // if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) // _config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO; // break; case UBX_CONFIG_STATE_MSG_NAV_SVINFO: if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) _config_state = UBX_CONFIG_STATE_MSG_NAV_SOL; break; case UBX_CONFIG_STATE_MSG_NAV_SOL: if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) _config_state = UBX_CONFIG_STATE_MSG_NAV_VELNED; break; case UBX_CONFIG_STATE_MSG_NAV_VELNED: if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) _config_state = UBX_CONFIG_STATE_CONFIGURED; /* set the flag to tell the driver that configuration was successful */ config_needed = false; break; // case UBX_CONFIG_STATE_MSG_RXM_SVSI: // if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) // _config_state = UBX_CONFIG_STATE_CONFIGURED; // break; default: break; } } else { warnx("ACK_ACK: checksum invalid"); } // Reset state machine to decode next packet decodeInit(); break; } case ACK_NAK: { // printf("GOT ACK_NAK\n"); gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) _rx_buffer; //Check if checksum is valid if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { warnx("UBX: Received: Not Acknowledged"); /* configuration obviously not successful */ config_needed = true; } else { warnx("ACK_NAK: checksum invalid\n"); } // Reset state machine to decode next packet decodeInit(); break; } default: //we don't know the message warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); decodeInit(); break; } } // end if _rx_count high enough else if (_rx_count < RECV_BUFFER_SIZE) { _rx_count++; } else { warnx("buffer full, restarting"); decodeInit(); config_needed = true; } break; default: break; } return; } void UBX::decodeInit(void) { _rx_ck_a = 0; _rx_ck_b = 0; _rx_count = 0; _decode_state = UBX_DECODE_UNINIT; _message_class = CLASS_UNKNOWN; _message_id = ID_UNKNOWN; _payload_size = 0; } void UBX::addByteToChecksum(uint8_t b) { _rx_ck_a = _rx_ck_a + b; _rx_ck_b = _rx_ck_b + _rx_ck_a; } void UBX::addChecksumToMessage(uint8_t* message, const unsigned length) { uint8_t ck_a = 0; uint8_t ck_b = 0; unsigned i; for (i = 0; i < length-2; i++) { ck_a = ck_a + message[i]; ck_b = ck_b + ck_a; } /* The checksum is written to the last to bytes of a message */ message[length-2] = ck_a; message[length-1] = ck_b; }