diff --git a/libraries/AP_GPS/AP_GPS_Auto.cpp b/libraries/AP_GPS/AP_GPS_Auto.cpp index 4d23221420..e9780d072e 100644 --- a/libraries/AP_GPS/AP_GPS_Auto.cpp +++ b/libraries/AP_GPS/AP_GPS_Auto.cpp @@ -20,7 +20,6 @@ static unsigned int baudrates[] = {38400U, 57600U, 9600U, 4800U}; const prog_char AP_GPS_Auto::_mtk_set_binary[] PROGMEM = MTK_SET_BINARY; -const prog_char AP_GPS_Auto::_ublox_set_binary[] PROGMEM = UBLOX_SET_BINARY; const prog_char AP_GPS_Auto::_sirf_set_binary[] PROGMEM = SIRF_SET_BINARY; @@ -41,6 +40,7 @@ AP_GPS_Auto::init(enum GPS_Engine_Setting nav_setting) _nav_setting = nav_setting; } + // Called the first time that a client tries to kick the GPS to update. // // We detect the real GPS, then update the pointer we have been called through @@ -109,8 +109,8 @@ AP_GPS_Auto::_detect(void) // Serial.print('G'); gps = NULL; - for (tries = 0; tries < 2; tries++) { + for (tries = 0; tries < 2; tries++) { // // Empty the serial buffer and wait for 50ms of quiet. // @@ -128,6 +128,22 @@ AP_GPS_Auto::_detect(void) } } 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 // @@ -199,24 +215,6 @@ AP_GPS_Auto::_detect(void) break; } - // - // If we haven't spammed the various init strings, send them now - // and retry to avoid a false-positive on the NMEA detector. - // - if (0 == tries) { - Serial.print('*'); - // use the FastSerial port handle so that we can use PROGMEM strings - _fs->println_P((const prog_char_t *)_mtk_set_binary); - _fs->println_P((const prog_char_t *)_ublox_set_binary); - _fs->println_P((const prog_char_t *)_sirf_set_binary); - - // give the GPS time to react to the settings - callback(100); - continue; - } else { - Serial.print('?'); - } - #if WITH_NMEA_MODE // // Something talking NMEA @@ -230,6 +228,7 @@ AP_GPS_Auto::_detect(void) break; } #endif + Serial.printf("?"); } return(gps); } diff --git a/libraries/AP_GPS/AP_GPS_Auto.h b/libraries/AP_GPS/AP_GPS_Auto.h index 932ed2a990..59f59f5c8d 100644 --- a/libraries/AP_GPS/AP_GPS_Auto.h +++ b/libraries/AP_GPS/AP_GPS_Auto.h @@ -44,7 +44,6 @@ private: GPS *_detect(void); static const prog_char _mtk_set_binary[]; - static const prog_char _ublox_set_binary[]; static const prog_char _sirf_set_binary[]; enum GPS_Engine_Setting _nav_setting; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index e492dba91f..6b92daf135 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -11,8 +11,9 @@ #define UBLOX_DEBUGGING 0 -#if UBLOX_DEBUGGING #include + +#if UBLOX_DEBUGGING # define Debug(fmt, args...) do {Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__ , ##args); delay(0); } while(0) #else # define Debug(fmt, args...) @@ -21,6 +22,8 @@ #include "AP_GPS_UBLOX.h" #include +const prog_char AP_GPS_UBLOX::_ublox_set_binary[] PROGMEM = UBLOX_SET_BINARY; +const uint8_t AP_GPS_UBLOX::_ublox_set_binary_size = sizeof(AP_GPS_UBLOX::_ublox_set_binary); // Constructors //////////////////////////////////////////////////////////////// @@ -321,10 +324,17 @@ 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; - // this type 0x41 pubx sets us up for 38400 with - // NMEA+UBX input and UBX output - _send_pubx("$PUBX,41,1,0003,0001,38400,0"); + // 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; @@ -338,19 +348,11 @@ AP_GPS_UBLOX::_configure_gps(void) _configure_message_rate(CLASS_NAV, MSG_SOL, 1); _configure_message_rate(CLASS_NAV, MSG_VELNED, 1); + _configure_message_rate(CLASS_NAV, MSG_SOL, 1); + _configure_message_rate(CLASS_NAV, MSG_SOL, 1); + _configure_message_rate(CLASS_NAV, MSG_SOL, 1); + _configure_message_rate(CLASS_NAV, MSG_SOL, 1); + // ask for the current navigation settings _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0); } - -void -AP_GPS_UBLOX::_send_pubx(const char *msg) -{ - uint8_t csum = 0; - char suffix[4]; - for (uint8_t i=1; msg[i]; i++) { - csum ^= msg[i]; - } - _port->write(msg); - sprintf(suffix, "*%02x", (unsigned)csum); - _port->write((const uint8_t *)suffix, 3); -} diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 46553634ec..6bf0f71da0 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -13,7 +13,14 @@ #include "GPS.h" -#define UBLOX_SET_BINARY "$PUBX,41,1,0003,0001,38400,0*26" +/* + try to put a UBlox into binary mode. This is in two parts. First we + send a PUBX asking the UBlox to receive NMEA and UBX, and send UBX, + with a baudrate of 38400. Then we send a UBX message setting rate 1 + for the NAV_SOL message. The setup of NAV_SOL is to cope with + configurations where all UBX binary message types are disabled. + */ +#define UBLOX_SET_BINARY "$PUBX,41,1,0003,0001,38400,0*26\n\265\142\006\001\003\000\001\006\001\022\117" class AP_GPS_UBLOX : public GPS { @@ -23,6 +30,9 @@ public: virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE); virtual bool read(); + static const prog_char _ublox_set_binary[]; + static const uint8_t _ublox_set_binary_size; + private: // u-blox UBX protocol essentials // XXX this is being ignored by the compiler #pragma pack(1) @@ -123,6 +133,7 @@ private: MSG_STATUS = 0x3, MSG_SOL = 0x6, MSG_VELNED = 0x12, + MSG_CFG_PRT = 0x00, MSG_CFG_RATE = 0x08, MSG_CFG_SET_RATE = 0x01, MSG_CFG_NAV_SETTINGS = 0x24 @@ -175,7 +186,6 @@ private: // used to update fix between status and position packets bool next_fix; - void _send_pubx(const char *msg); void _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate); void _configure_gps(void); void _update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_t &ck_b); diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp index 1466d32108..7df3b0d673 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -2,7 +2,7 @@ #include -#define GPS_DEBUGGING 1 +#define GPS_DEBUGGING 0 #if GPS_DEBUGGING #include @@ -81,3 +81,13 @@ GPS::_error(const char *msg) { Serial.println(msg); } + +/// +/// write a block of configuration data to a GPS +/// +void GPS::_write_progstr_block(Stream *_fs, const prog_char *pstr, uint8_t size) +{ + while (size--) { + _fs->write(pgm_read_byte(pstr++)); + } +} diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index ebce5a0018..b002f4c65d 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -180,6 +180,8 @@ protected: enum GPS_Engine_Setting _nav_setting; + void _write_progstr_block(Stream *_fs, const prog_char *pstr, uint8_t size); + private: