Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GPS: fixed auto-config of UBlox setup with no UBX messages 

if a UBlox is configured for NMEA only, with no UBX messages at all
then it would never trigger the GPS_AUTO detection. This adds a UBX
config message to the init strings that enables the NAV_SOL message
This commit is contained in:
Andrew Tridgell 2012-06-15 15:53:14 +10:00
parent b7e807cd58
commit ae9883a8e2
6 changed files with 63 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_GPS/AP_GPS_Auto.cpp
View File

@ -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);
}

1
libraries/AP_GPS/AP_GPS_Auto.h
View File

@ -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;

36
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -11,8 +11,9 @@
#define UBLOX_DEBUGGING 0
#if UBLOX_DEBUGGING
#include <FastSerial.h>
#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 <stdint.h>
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);
}

14
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -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);

12
libraries/AP_GPS/GPS.cpp
View File

@ -2,7 +2,7 @@
#include <FastSerial.h>
#define GPS_DEBUGGING 1
#define GPS_DEBUGGING 0
#if GPS_DEBUGGING
#include <FastSerial.h>
@ -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++));
}
}

2
libraries/AP_GPS/GPS.h
View File

@ -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: