From 0191355488c17d5a16f7ab84a9e010ce5fe84e28 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 31 Mar 2014 20:48:22 +1100 Subject: [PATCH] AP_GPS: fixed init strings to all be in progmem and sent async ... --- libraries/AP_GPS/AP_GPS.cpp | 63 ++++++++++++++++++++----------- libraries/AP_GPS/AP_GPS.h | 14 ++++++- libraries/AP_GPS/AP_GPS_MTK.cpp | 25 ++++++------ libraries/AP_GPS/AP_GPS_MTK.h | 3 ++ libraries/AP_GPS/AP_GPS_MTK19.cpp | 16 +------- libraries/AP_GPS/AP_GPS_NMEA.cpp | 54 +++++++++++--------------- libraries/AP_GPS/AP_GPS_NMEA.h | 2 + libraries/AP_GPS/AP_GPS_SIRF.cpp | 5 ++- libraries/AP_GPS/AP_GPS_SIRF.h | 2 + 9 files changed, 99 insertions(+), 85 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 617e4407e0..18997904bc 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -22,14 +22,6 @@ extern const AP_HAL::HAL& hal; -#define GPS_DEBUGGING 0 - -#if GPS_DEBUGGING - # define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(0); } while(0) -#else - # define Debug(fmt, args ...) -#endif - // table of user settable parameters const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = { // @Param: TYPE @@ -70,6 +62,38 @@ const uint16_t AP_GPS::_baudrates[] PROGMEM = {4800U, 38400U, 57600U, 9600U}; // right mode const prog_char AP_GPS::_initialisation_blob[] PROGMEM = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY; +/* + send some more initialisation string bytes if there is room in the + UART transmit buffer + */ +void AP_GPS::send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t size) +{ + initblob_state[instance].blob = _blob; + initblob_state[instance].remaining = size; +} + +/* + send some more initialisation string bytes if there is room in the + UART transmit buffer + */ +void AP_GPS::send_blob_update(uint8_t instance) +{ + // see if we can write some more of the initialisation blob + if (initblob_state[instance].remaining > 0) { + AP_HAL::UARTDriver *port = instance==0?hal.uartB:hal.uartE; + int16_t space = port->txspace(); + if (space > (int16_t)initblob_state[instance].remaining) { + space = initblob_state[instance].remaining; + } + while (space > 0) { + port->write(pgm_read_byte(initblob_state[instance].blob)); + initblob_state[instance].blob++; + space--; + initblob_state[instance].remaining--; + } + } +} + /* run detection step for one GPS instance. If this finds a GPS then it will fill in drivers[instance] and change state[instance].status @@ -104,22 +128,10 @@ AP_GPS::detect_instance(uint8_t instance) uint16_t baudrate = pgm_read_word(&_baudrates[dstate->last_baud]); port->begin(baudrate, 256, 16); dstate->last_baud_change_ms = now; - dstate->init_blob_offset = 0; - } - - // see if we can write some more of the initialisation blob - if (dstate->init_blob_offset < sizeof(_initialisation_blob)) { - int16_t space = port->txspace(); - if (space > (int16_t)sizeof(_initialisation_blob) - dstate->init_blob_offset) { - space = sizeof(_initialisation_blob) - dstate->init_blob_offset; - } - while (space > 0) { - port->write(pgm_read_byte(&_initialisation_blob[dstate->init_blob_offset])); - dstate->init_blob_offset++; - space--; - } + send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); } + send_blob_update(instance); while (port->available() > 0 && new_gps == NULL) { uint8_t data = port->read(); @@ -174,7 +186,7 @@ AP_GPS::detect_instance(uint8_t instance) /* - update one GPS instance + update one GPS instance. This should be called at 10Hz or greater */ void AP_GPS::update_instance(uint8_t instance) @@ -191,6 +203,8 @@ AP_GPS::update_instance(uint8_t instance) return; } + send_blob_update(instance); + // we have an active driver for this instance bool result = drivers[instance]->read(); uint32_t tnow = hal.scheduler->millis(); @@ -222,6 +236,9 @@ AP_GPS::update(void) } } +/* + set HIL (hardware in the loop) status for a GPS instance + */ void AP_GPS::setHIL(GPS_Status _status, uint64_t time_epoch_ms, Location &_location, Vector3f &_velocity, uint8_t _num_sats) diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 30095b0585..137cfab27d 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -89,6 +89,10 @@ public: GPS_ENGINE_AIRBORNE_4G = 8 }; + /* + The GPS_State structure is filled in by the backend driver as it + parses each message from the GPS. + */ struct GPS_State { uint8_t instance; // the instance number of this GPS @@ -242,6 +246,10 @@ public: AP_Int8 _type[GPS_MAX_INSTANCES]; AP_Int8 _navfilter; + // handle sending of initialisation strings to the GPS + void send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t size); + void send_blob_update(uint8_t instance); + private: struct GPS_timing { // the time we got our last fix in system milliseconds @@ -262,7 +270,6 @@ private: uint32_t detect_started_ms; uint32_t last_baud_change_ms; uint8_t last_baud; - uint8_t init_blob_offset; struct UBLOX_detect_state ublox_detect_state; struct MTK_detect_state mtk_detect_state; struct MTK19_detect_state mtk19_detect_state; @@ -270,6 +277,11 @@ private: struct NMEA_detect_state nmea_detect_state; } detect_state[GPS_MAX_INSTANCES]; + struct { + const prog_char *blob; + uint16_t remaining; + } initblob_state[GPS_MAX_INSTANCES]; + static const uint16_t _baudrates[]; static const prog_char _initialisation_blob[]; diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp index 8b0edb4c74..0a0b9aabcf 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK.cpp @@ -24,25 +24,24 @@ #include #include "AP_GPS_MTK.h" +// initialisation blobs to send to the GPS to try to get it into the +// right mode +const prog_char AP_GPS_MTK::_initialisation_blob[] PROGMEM = MTK_OUTPUT_5HZ SBAS_ON WAAS_ON MTK_NAVTHRES_OFF; + AP_GPS_MTK::AP_GPS_MTK(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : AP_GPS_Backend(_gps, _state, _port), _step(0), _payload_counter(0) { - // initialize serial port for binary protocol use - port->print(MTK_SET_BINARY); - - // set 5Hz update rate - port->print(MTK_OUTPUT_5HZ); - - // set SBAS on - port->print(SBAS_ON); - - // set WAAS on - port->print(WAAS_ON); + gps.send_blob_start(state.instance, _initialisation_blob, sizeof(_initialisation_blob)); +} - // Set Nav Threshold to 0 m/s - port->print(MTK_NAVTHRES_OFF); +/* + send an initialisation blob to configure the GPS + */ +void AP_GPS_MTK::send_init_blob(uint8_t instance, AP_GPS &gps) +{ + gps.send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); } diff --git a/libraries/AP_GPS/AP_GPS_MTK.h b/libraries/AP_GPS/AP_GPS_MTK.h index 1dc78631cd..504a93fb58 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.h +++ b/libraries/AP_GPS/AP_GPS_MTK.h @@ -35,6 +35,7 @@ public: bool read(void); static bool _detect(struct MTK_detect_state &state, uint8_t data); + static void send_init_blob(uint8_t instance, AP_GPS &gps); private: struct PACKED diyd_mtk_msg { @@ -76,6 +77,8 @@ private: // Buffer parse & GPS state update void _parse_gps(); + + static const prog_char _initialisation_blob[]; }; #endif // __AP_GPS_MTK_H__ diff --git a/libraries/AP_GPS/AP_GPS_MTK19.cpp b/libraries/AP_GPS/AP_GPS_MTK19.cpp index 336f76bbad..c937cab4f0 100644 --- a/libraries/AP_GPS/AP_GPS_MTK19.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK19.cpp @@ -34,21 +34,7 @@ AP_GPS_MTK19::AP_GPS_MTK19(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART _mtk_revision(0), _fix_counter(0) { - // initialize serial port for binary protocol use - // XXX should assume binary, let GPS_AUTO handle dynamic config? - port->print(MTK_SET_BINARY); - - // set 5Hz update rate - port->print(MTK_OUTPUT_5HZ); - - // set SBAS on - port->print(SBAS_ON); - - // set WAAS on - port->print(WAAS_ON); - - // Set Nav Threshold to 0 m/s - port->print(MTK_NAVTHRES_OFF); + AP_GPS_MTK::send_init_blob(_state.instance, _gps); } // Process bytes available from the stream diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index f3690559a8..d4732cafdc 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -46,30 +46,28 @@ extern const AP_HAL::HAL& hal; // for NMEA. GPS_AUTO will try to set any SiRF unit to binary mode as part of // the autodetection process. // -const prog_char AP_GPS_NMEA::_SiRF_init_string[] PROGMEM = - "$PSRF103,0,0,1,1*25\r\n" // GGA @ 1Hz - "$PSRF103,1,0,0,1*25\r\n" // GLL off - "$PSRF103,2,0,0,1*26\r\n" // GSA off - "$PSRF103,3,0,0,1*27\r\n" // GSV off - "$PSRF103,4,0,1,1*20\r\n" // RMC off - "$PSRF103,5,0,1,1*20\r\n" // VTG @ 1Hz - "$PSRF103,6,0,0,1*22\r\n" // MSS off - "$PSRF103,8,0,0,1*2C\r\n" // ZDA off - "$PSRF151,1*3F\r\n" // WAAS on (not always supported) - "$PSRF106,21*0F\r\n" // datum = WGS84 - ""; +#define SIRF_INIT_MSG \ + "$PSRF103,0,0,1,1*25\r\n" /* GGA @ 1Hz */ \ + "$PSRF103,1,0,0,1*25\r\n" /* GLL off */ \ + "$PSRF103,2,0,0,1*26\r\n" /* GSA off */ \ + "$PSRF103,3,0,0,1*27\r\n" /* GSV off */ \ + "$PSRF103,4,0,1,1*20\r\n" /* RMC off */ \ + "$PSRF103,5,0,1,1*20\r\n" /* VTG @ 1Hz */ \ + "$PSRF103,6,0,0,1*22\r\n" /* MSS off */ \ + "$PSRF103,8,0,0,1*2C\r\n" /* ZDA off */ \ + "$PSRF151,1*3F\r\n" /* WAAS on (not always supported) */ \ + "$PSRF106,21*0F\r\n" /* datum = WGS84 */ // MediaTek init messages ////////////////////////////////////////////////////// // // Note that we may see a MediaTek in NMEA mode if we are connected to a non-DIYDrones // MediaTek-based GPS. // -const prog_char AP_GPS_NMEA::_MTK_init_string[] PROGMEM = - "$PMTK314,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" // GGA & VTG once every fix - "$PMTK330,0*2E\r\n" // datum = WGS84 - "$PMTK313,1*2E\r\n" // SBAS on - "$PMTK301,2*2E\r\n" // use SBAS data for DGPS - ""; +#define MTK_INIT_MSG \ + "$PMTK314,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" /* GGA & VTG once every fix */ \ + "$PMTK330,0*2E\r\n" /* datum = WGS84 */ \ + "$PMTK313,1*2E\r\n" /* SBAS on */ \ + "$PMTK301,2*2E\r\n" /* use SBAS data for DGPS */ // ublox init messages ///////////////////////////////////////////////////////// // @@ -80,11 +78,12 @@ const prog_char AP_GPS_NMEA::_MTK_init_string[] PROGMEM = // We don't attempt to send $PUBX,41 as the unit must already be talking NMEA // and we don't know the baudrate. // -const prog_char AP_GPS_NMEA::_ublox_init_string[] PROGMEM = - "$PUBX,40,gga,0,1,0,0,0,0*7B\r\n" // GGA on at one per fix - "$PUBX,40,vtg,0,1,0,0,0,0*7F\r\n" // VTG on at one per fix - "$PUBX,40,rmc,0,0,0,0,0,0*67\r\n" // RMC off (XXX suppress other message types?) - ""; +#define UBLOX_INIT_MSG \ + "$PUBX,40,gga,0,1,0,0,0,0*7B\r\n" /* GGA on at one per fix */ \ + "$PUBX,40,vtg,0,1,0,0,0,0*7F\r\n" /* VTG on at one per fix */ \ + "$PUBX,40,rmc,0,0,0,0,0,0*67\r\n" /* RMC off (XXX suppress other message types?) */ + +const prog_char AP_GPS_NMEA::_initialisation_blob[] PROGMEM = SIRF_INIT_MSG MTK_INIT_MSG UBLOX_INIT_MSG; // NMEA message identifiers //////////////////////////////////////////////////// // @@ -105,14 +104,7 @@ AP_GPS_NMEA::AP_GPS_NMEA(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDr _term_offset(0), _gps_data_good(false) { - // send the SiRF init strings - port->print_P((const prog_char_t *)_SiRF_init_string); - - // send the MediaTek init strings - port->print_P((const prog_char_t *)_MTK_init_string); - - // send the ublox init strings - port->print_P((const prog_char_t *)_ublox_init_string); + gps.send_blob_start(state.instance, _initialisation_blob, sizeof(_initialisation_blob)); } bool AP_GPS_NMEA::read(void) diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index 0fd4a29736..f429bde6ae 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -152,6 +152,8 @@ private: static const prog_char _gpgga_string[]; static const prog_char _gpvtg_string[]; //@} + + static const prog_char _initialisation_blob[]; }; #endif // __AP_GPS_NMEA_H__ diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp index cfa4426086..ad5a2e30ed 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -28,9 +28,9 @@ // // XXX the bytes show up on the wire, but at least my test unit (EM-411) seems to ignore them. // -static const uint8_t init_messages[] PROGMEM = { +const uint8_t AP_GPS_SIRF::_initialisation_blob[] PROGMEM = { 0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0xb0, 0xb3, - 0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3 + 0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3 }; AP_GPS_SIRF::AP_GPS_SIRF(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : @@ -41,6 +41,7 @@ AP_GPS_SIRF::AP_GPS_SIRF(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDr _payload_counter(0), _msg_id(0) { + gps.send_blob_start(state.instance, (const prog_char *)_initialisation_blob, sizeof(_initialisation_blob)); } diff --git a/libraries/AP_GPS/AP_GPS_SIRF.h b/libraries/AP_GPS/AP_GPS_SIRF.h index b24c371dd1..c69176a02d 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.h +++ b/libraries/AP_GPS/AP_GPS_SIRF.h @@ -102,6 +102,8 @@ private: bool _parse_gps(void); void _accumulate(uint8_t val); + + static const uint8_t _initialisation_blob[]; }; #endif // AP_GPS_SIRF_h