/* * configure a UBlox GPS for the given message rate */ void AP_GPS_UBLOX::_configure_gps(void) { struct ubx_cfg_nav_rate msg; const unsigned baudrates[4] = {9600U, 19200U, 38400U, 57600U}; FastSerial *_fs = (FastSerial *)_port; // the GPS may be setup for a different baud rate. This ensures // it gets configured correctly for (uint8_t i=0; i<4; i++) { _fs->begin(baudrates[i]); _write_progstr_block(_fs, _ublox_set_binary, _ublox_set_binary_size); while (_fs->tx_pending()) delay(1); } _fs->begin(38400U); // ask for navigation solutions every 200ms msg.measure_rate_ms = 200; msg.nav_rate = 1; msg.timeref = 0; // UTC time _send_message(CLASS_CFG, MSG_CFG_RATE, &msg, sizeof(msg)); // ask for the messages we parse to be sent on every navigation solution _configure_message_rate(CLASS_NAV, MSG_POSLLH, 1); _configure_message_rate(CLASS_NAV, MSG_STATUS, 1); _configure_message_rate(CLASS_NAV, MSG_SOL, 1); _configure_message_rate(CLASS_NAV, MSG_VELNED, 1); // ask for the current navigation settings Debug("Asking for engine setting\n"); _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0); }
void GPS::_update_progstr(void) { struct progstr_queue *q = &progstr_state.queue[progstr_state.idx]; // quick return if nothing to do if (q->size == 0 || progstr_state.fs->tx_pending()) { return; } uint8_t nbytes = q->size - q->ofs; if (nbytes > 16) { nbytes = 16; } _write_progstr_block(progstr_state.fs, q->pstr+q->ofs, nbytes); q->ofs += nbytes; if (q->ofs == q->size) { q->size = 0; progstr_state.idx++; if (progstr_state.idx == PROGSTR_QUEUE_SIZE) { progstr_state.idx = 0; } } }
/* * configure a UBlox GPS for the given message rate */ void AP_GPS_UBLOX::_configure_gps(void) { const unsigned baudrates[4] = {9600U, 19200U, 38400U, 57600U}; // the GPS may be setup for a different baud rate. This ensures // it gets configured correctly for (uint8_t i=0; i<4; i++) { _port->begin(baudrates[i]); _write_progstr_block(_port, _ublox_set_binary, _ublox_set_binary_size); while (_port->tx_pending()) { hal.scheduler->delay(1); } } _port->begin(38400U); // start the process of updating the GPS rates need_rate_update = true; rate_update_step = 0; // ask for the current navigation settings Debug("Asking for engine setting\n"); _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0); }
// // Perform one iteration of the auto-detection process. // GPS * AP_GPS_Auto::_detect(void) { uint32_t then; uint8_t fingerprint[4]; uint8_t tries; uint16_t charcount; GPS *gps; // // Loop attempting to detect a recognized GPS // Serial.print('G'); gps = NULL; for (tries = 0; tries < 2; tries++) { // // Empty the serial buffer and wait for 50ms of quiet. // // XXX We can detect babble by counting incoming characters, but // what would we do about it? // charcount = 0; _port->flush(); then = millis(); do { if (_port->available()) { then = millis(); _port->read(); charcount++; } } while ((millis() - then) < 50 && charcount < 5000); if (tries == 0) { // write configuration strings to put the GPS into the preferred // mode _write_progstr_block(_fs, _mtk_set_binary, sizeof(_mtk_set_binary)); _write_progstr_block(_fs, AP_GPS_UBLOX::_ublox_set_binary, AP_GPS_UBLOX::_ublox_set_binary_size); _write_progstr_block(_fs, _sirf_set_binary, sizeof(_sirf_set_binary)); // ensure its all been written while (_fs->tx_pending()) { callback(10); } // give the GPS time to react to the settings callback(100); } // // Collect four characters to fingerprint a device // // If we take more than 1200ms to receive four characters, abort. // This will normally only be the case where there is no GPS attached. // while (_port->available() < 4) { callback(1); if ((millis() - then) > 1200) { Serial.print('!'); return NULL; } } fingerprint[0] = _port->read(); fingerprint[1] = _port->read(); fingerprint[2] = _port->read(); fingerprint[3] = _port->read(); // // ublox or MTK in DIYD binary mode (whose smart idea was // it to make the MTK look sort-of like it was talking UBX?) // if ((0xb5 == fingerprint[0]) && (0x62 == fingerprint[1]) && (0x01 == fingerprint[2])) { // message 5 is MTK pretending to talk UBX if (0x05 == fingerprint[3]) { gps = new AP_GPS_MTK(_port); Serial.print_P(PSTR(" MTK1.4 ")); break; } // any other message is ublox gps = new AP_GPS_UBLOX(_port); Serial.print_P(PSTR(" ublox ")); break; } // new style 3DR UBlox (April 2012)x if (0xb5 == fingerprint[0] && 0x62 == fingerprint[1] && 0x0d == fingerprint[2] && 0x01 == fingerprint[3]) { // new style Ublox gps = new AP_GPS_UBLOX(_port); Serial.print_P(PSTR(" ublox ")); break; } // // MTK v1.6 // if ((0xd0 == fingerprint[0]) && (0xdd == fingerprint[1]) && (0x20 == fingerprint[2])) { gps = new AP_GPS_MTK16(_port); Serial.print_P(PSTR(" MTK1.6 ")); break; } // // SIRF in binary mode // if ((0xa0 == fingerprint[0]) && (0xa2 == fingerprint[1])) { gps = new AP_GPS_SIRF(_port); Serial.print_P(PSTR(" SiRF ")); break; } #if WITH_NMEA_MODE // // Something talking NMEA // if (('$' == fingerprint[0]) && (('G' == fingerprint[1]) || ('P' == fingerprint[1]))) { // XXX this may be a bit presumptive, might want to give the GPS a couple of // iterations around the loop to react to init strings? gps = new AP_GPS_NMEA(_port); break; } #endif Serial.printf("?"); } return(gps); }