int UBX::configure(unsigned &baudrate) { _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; } /* 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)); /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */ receive(UBX_CONFIG_TIMEOUT); if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE); baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; } /* 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)); _clsID_needed = UBX_CLASS_CFG; _msgID_needed = UBX_MESSAGE_CFG_RATE; 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; send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); if (receive(UBX_CONFIG_TIMEOUT) < 0) { /* try next baudrate */ continue; } /* 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)); _clsID_needed = UBX_CLASS_CFG; _msgID_needed = UBX_MESSAGE_CFG_NAV5; 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_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); if (receive(UBX_CONFIG_TIMEOUT) < 0) { /* try next baudrate */ continue; } type_gps_bin_cfg_msg_packet_t cfg_msg_packet; memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); _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::configure(unsigned &baudrate) { _configured = false; /* try different baudrates */ const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; int baud_i; for (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)); _message_class_needed = UBX_CLASS_CFG; _message_id_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 (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { /* try next baudrate */ continue; } /* 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)); /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */ wait_for_ack(UBX_CONFIG_TIMEOUT); if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE); baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; } /* at this point we have correct baudrate on both ends */ break; } if (baud_i >= 5) { return 1; } /* send a CFG-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)); _message_class_needed = UBX_CLASS_CFG; _message_id_needed = UBX_MESSAGE_CFG_RATE; 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_MEASINTERVAL; cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet)); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { warnx("CFG FAIL: RATE"); return 1; } /* 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)); _message_class_needed = UBX_CLASS_CFG; _message_id_needed = UBX_MESSAGE_CFG_NAV5; 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_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { warnx("CFG FAIL: NAV5"); return 1; } /* configure message rates */ /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { warnx("MSG CFG FAIL: NAV POSLLH"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { warnx("MSG CFG FAIL: NAV TIMEUTC"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { warnx("MSG CFG FAIL: NAV SOL"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { warnx("MSG CFG FAIL: NAV VELNED"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { warnx("MSG CFG FAIL: NAV SVINFO"); return 1; } configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { warnx("MSG CFG FAIL: MON HW"); return 1; } _configured = true; return 0; }