/*
* 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);
}
