AP_GPS: Raise the baud rate on a ublox GPS if using RAW logging

the implementation leaves an easy path forward for providing a different startup blob for all the GPS's if raw logging is enabled
This commit is contained in:
Michael du Breuil 2015-07-14 11:45:09 -07:00 committed by Andrew Tridgell
parent 19bb96b9cb
commit 47592a1953
4 changed files with 8 additions and 2 deletions

View File

@ -130,6 +130,7 @@ const uint32_t AP_GPS::_baudrates[] PROGMEM = {4800U, 38400U, 115200U, 57600U, 9
// initialisation blobs to send to the GPS to try to get it into the
// right mode
const prog_char AP_GPS::_initialisation_blob[] PROGMEM = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY;
const prog_char AP_GPS::_initialisation_raw_blob[] PROGMEM = UBLOX_SET_BINARY_RAW_BAUD MTK_SET_BINARY SIRF_SET_BINARY;
/*
send some more initialisation string bytes if there is room in the
@ -213,6 +214,11 @@ AP_GPS::detect_instance(uint8_t instance)
_port[instance]->begin(baudrate);
_port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
dstate->last_baud_change_ms = now;
#if UBLOX_RXM_RAW_LOGGING
if(_raw_data != 0)
send_blob_start(instance, _initialisation_raw_blob, sizeof(_initialisation_raw_blob));
else
#endif
send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
}

View File

@ -407,6 +407,7 @@ private:
static const uint32_t _baudrates[];
static const prog_char _initialisation_blob[];
static const prog_char _initialisation_raw_blob[];
void detect_instance(uint8_t instance);
void update_instance(uint8_t instance);

View File

@ -747,8 +747,6 @@ AP_GPS_UBLOX::_configure_navigation_rate(uint16_t rate_ms)
void
AP_GPS_UBLOX::_configure_gps(void)
{
port->begin(38400U);
// start the process of updating the GPS rates
need_rate_update = true;
_last_5hz_time = hal.scheduler->millis();

View File

@ -39,6 +39,7 @@
* would mean we would never detect it.
*/
#define UBLOX_SET_BINARY "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0003,0001,38400,0*26\r\n"
#define UBLOX_SET_BINARY_RAW_BAUD "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0003,0001,115200,0*1E\r\n"
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
#define UBLOX_RXM_RAW_LOGGING 1