diff options
Diffstat (limited to 'apps/drivers/gps/ubx.cpp')
-rw-r--r-- | apps/drivers/gps/ubx.cpp | 930 |
1 files changed, 454 insertions, 476 deletions
diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 8e2396564..74cbc5aaf 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -37,6 +37,7 @@ #include <unistd.h> #include <stdio.h> +#include <poll.h> #include <math.h> #include <string.h> #include <assert.h> @@ -47,172 +48,251 @@ #include "ubx.h" +#define UBX_CONFIG_TIMEOUT 100 -UBX::UBX() : -_config_state(UBX_CONFIG_STATE_PRT), +UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : +_fd(fd), +_gps_position(gps_position), _waiting_for_ack(false) { - reset(); + decode_init(); } UBX::~UBX() { } -void -UBX::reset() +int +UBX::configure(unsigned &baudrate) { - decodeInit(); - _config_state = UBX_CONFIG_STATE_PRT; - _waiting_for_ack = false; -} + _waiting_for_ack = true; + + /* try different baudrates */ + const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; + + for (int baud_i = 0; baud_i < 5; baud_i++) { + baudrate = baudrates_to_try[baud_i]; + set_baudrate(_fd, baudrate); + + /* 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)); + + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_PRT; + + /* 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; + + send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } -void -UBX::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) -{ - /* 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; - - sendConfigPacket(fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); - - } 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; - - sendConfigPacket(fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); - - /* 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(); - } + /* Send a CFG-PRT message again, this time 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 = UBX_CFG_PRT_PAYLOAD_BAUDRATE; + cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; + cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + + send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { + set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE); + baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; + } - } else if (_config_state == UBX_CONFIG_STATE_RATE) { + /* no ack is ecpected here, keep going configuring */ - /* 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)); + /* 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; + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_RATE; - sendConfigPacket(fd, (uint8_t*)&cfg_rate_packet, 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; - } 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)); + send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } - 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; + /* 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)); - sendConfigPacket(fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_NAV5; - } else { - /* Catch the remaining config states here, they all need the same packet type */ + 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; - type_gps_bin_cfg_msg_packet_t cfg_msg_packet; - memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); + send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } - 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; + type_gps_bin_cfg_msg_packet_t cfg_msg_packet; + memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); - 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; + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_MSG; + + 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; + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + 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; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } +// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; +// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; + +// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; +// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; + + _waiting_for_ack = false; + return 0; + } + return -1; +} + +int +UBX::receive(unsigned timeout) +{ + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = _fd; + fds[0].events = POLLIN; + + uint8_t buf[32]; + + /* timeout additional to poll */ + uint64_t time_started = hrt_absolute_time(); + + int j = 0; + ssize_t count = 0; + + while (true) { + + /* pass received bytes to the packet decoder */ + while (j < count) { + if (parse_char(buf[j]) > 0) { + /* return to configure during configuration or to the gps driver during normal work + * if a packet has arrived */ + if (handle_message() > 0) + return 1; + } + /* in case we keep trying but only get crap from GPS */ + if (time_started + timeout*1000 < hrt_absolute_time() ) { + return -1; } + j++; + } - sendConfigPacket(fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + /* everything is read */ + j = count = 0; + + /* then poll for new data */ + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); + + if (ret < 0) { + /* something went wrong when polling */ + return -1; + + } else if (ret == 0) { + /* Timeout */ + return -1; + + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. If more bytes are + * available, we'll go back to poll() again... + */ + count = ::read(_fd, buf, sizeof(buf)); + } } } } -void -UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_needed, bool &pos_updated) +int +UBX::parse_char(uint8_t b) { switch (_decode_state) { /* First, look for sync1 */ @@ -227,13 +307,13 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ _decode_state = UBX_DECODE_GOT_SYNC2; } else { /* Second start symbol was wrong, reset state machine */ - decodeInit(); + decode_init(); } break; /* Now look for class */ case UBX_DECODE_GOT_SYNC2: /* everything except sync1 and sync2 needs to be added to the checksum */ - addByteToChecksum(b); + add_byte_to_checksum(b); /* check for known class */ switch (b) { case UBX_CLASS_ACK: @@ -256,12 +336,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ _message_class = CFG; break; default: //unknown class: reset state machine - decodeInit(); + decode_init(); break; } break; case UBX_DECODE_GOT_CLASS: - addByteToChecksum(b); + add_byte_to_checksum(b); switch (_message_class) { case NAV: switch (b) { @@ -296,7 +376,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ break; default: //unknown class: reset state machine, should not happen - decodeInit(); + decode_init(); break; } break; @@ -308,7 +388,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ // break; // // default: //unknown class: reset state machine, should not happen -// decodeInit(); +// decode_init(); // break; // } // break; @@ -321,7 +401,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ break; default: //unknown class: reset state machine, should not happen - decodeInit(); + decode_init(); break; } break; @@ -337,384 +417,282 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ _message_id = ACK_NAK; break; default: //unknown class: reset state machine, should not happen - decodeInit(); + decode_init(); 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; + decode_init(); +// config_needed = true; break; } break; case UBX_DECODE_GOT_MESSAGEID: - addByteToChecksum(b); + add_byte_to_checksum(b); _payload_size = b; //this is the first length byte _decode_state = UBX_DECODE_GOT_LENGTH1; break; case UBX_DECODE_GOT_LENGTH1: - addByteToChecksum(b); + add_byte_to_checksum(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); + add_byte_to_checksum(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 + /* compare checksum */ + if (_rx_ck_a == _rx_buffer[_rx_count-1] && _rx_ck_b == _rx_buffer[_rx_count]) { + return 1; + } else { + decode_init(); + return -1; + warnx("ubx: Checksum wrong"); + } - /* Add timestamp to finish the report */ - gps_position->timestamp_position = hrt_absolute_time(); + return 1; + } else if (_rx_count < RECV_BUFFER_SIZE) { + _rx_count++; + } else { + warnx("ubx: buffer full"); + decode_init(); + return -1; + } + break; + default: + break; + } + return 0; //XXX ? +} - /* set flag to trigger publishing of new position */ - pos_updated = true; - } else { - warnx("NAV_POSLLH: checksum invalid"); - } +int +UBX::handle_message() +{ + int ret = 0; - // Reset state machine to decode next packet - decodeInit(); - break; - } + 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"); + if (!_waiting_for_ack) { + gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; - case NAV_SOL: { -// printf("GOT NAV_SOL MESSAGE\n"); - gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; + _gps_position->lat = packet->lat; + _gps_position->lon = packet->lon; + _gps_position->alt = packet->height_msl; - //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 = (float)packet->hAcc * 1e-3f; // from mm to m + _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m - gps_position->fix_type = packet->gpsFix; - gps_position->s_variance_m_s = packet->sAcc; - gps_position->p_variance_m = packet->pAcc; + /* Add timestamp to finish the report */ + _gps_position->timestamp_position = hrt_absolute_time(); + /* only return 1 when new position is available */ + ret = 1; + } + break; + } - gps_position->timestamp_variance = hrt_absolute_time(); + case NAV_SOL: { +// printf("GOT NAV_SOL MESSAGE\n"); + if (!_waiting_for_ack) { + gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; - } else { - warnx("NAV_SOL: checksum invalid"); - } + _gps_position->fix_type = packet->gpsFix; + _gps_position->s_variance_m_s = packet->sAcc; + _gps_position->p_variance_m = packet->pAcc; - // Reset state machine to decode next packet - decodeInit(); - break; - } + _gps_position->timestamp_variance = hrt_absolute_time(); + } + 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; +// case NAV_DOP: { +//// printf("GOT NAV_DOP MESSAGE\n"); +// gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; // -// } else { -// warnx("NAV_DOP: checksum invalid"); -// } +// _gps_position->eph_m = packet->hDOP; +// _gps_position->epv = packet->vDOP; // -// // 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; +// _gps_position->timestamp_posdilution = hrt_absolute_time(); // -// //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]) { +// _new_nav_dop = true; // -// 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; - } +// break; +// } - case ACK_NAK: { -// printf("GOT ACK_NAK\n"); - gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) _rx_buffer; + case NAV_TIMEUTC: { +// printf("GOT NAV_TIMEUTC MESSAGE\n"); - //Check if checksum is valid - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + if (!_waiting_for_ack) { + gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; - warnx("UBX: Received: Not Acknowledged"); - /* configuration obviously not successful */ - config_needed = true; - } else { - warnx("ACK_NAK: checksum invalid\n"); - } + //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; - // Reset state machine to decode next packet - decodeInit(); - break; - } + time_t epoch = mktime(&timeinfo); - default: //we don't know the message - warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); - decodeInit(); + _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(); + } + break; + } - break; + case NAV_SVINFO: { +// printf("GOT NAV_SVINFO MESSAGE\n"); + + if (!_waiting_for_ack) { + //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); + + //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; } - } // end if _rx_count high enough - else if (_rx_count < RECV_BUFFER_SIZE) { - _rx_count++; + + _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 { - warnx("buffer full, restarting"); - decodeInit(); - config_needed = true; + _gps_position->satellite_used[i] = 0; + _gps_position->satellite_snr[i] = 0; + _gps_position->satellite_elevation[i] = 0; + _gps_position->satellite_azimuth[i] = 0; } - break; - default: + + } + + 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(); + } + break; - } - return; + } + + case NAV_VELNED: { +// printf("GOT NAV_VELNED MESSAGE\n"); + + if (!_waiting_for_ack) { + gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; + + _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(); + } + + 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; +// +// _gps_position->satellites_visible = packet->numVis; +// _gps_position->counter++; +// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); +// +// break; +// } + case ACK_ACK: { +// printf("GOT ACK_ACK\n"); + gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; + + if (_waiting_for_ack) { + if (packet->clsID == _clsID_needed && packet->msgID == _msgID_needed) { + ret = 1; + } + } + } + break; + + case ACK_NAK: { +// printf("GOT ACK_NAK\n"); + warnx("UBX: Received: Not Acknowledged"); + /* configuration obviously not successful */ + ret = -1; + break; + } + + default: //we don't know the message + warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); + ret = -1; + break; + } + // end if _rx_count high enough + decode_init(); + return ret; //XXX? } void -UBX::decodeInit(void) +UBX::decode_init(void) { _rx_ck_a = 0; _rx_ck_b = 0; @@ -726,14 +704,14 @@ UBX::decodeInit(void) } void -UBX::addByteToChecksum(uint8_t b) +UBX::add_byte_to_checksum(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) +UBX::add_checksum_to_message(uint8_t* message, const unsigned length) { uint8_t ck_a = 0; uint8_t ck_b = 0; @@ -749,12 +727,12 @@ UBX::addChecksumToMessage(uint8_t* message, const unsigned length) } void -UBX::sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length) +UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) { ssize_t ret = 0; /* Calculate the checksum now */ - addChecksumToMessage(packet, length); + add_checksum_to_message(packet, length); const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2}; |