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

GPS: replaced the GPS auto-detect mechanism 

the old mechanism wasted most of the input bytes, and chewed a lot of
CPU, making it impractical to do GPS detection for a GPS attached
after startup

The new code is async, and detects a GPS by looking for a fully formed
packet with the right checksum for each GPS type
This commit is contained in:
Andrew Tridgell 2012-09-17 14:43:07 +10:00
parent 7e8ef0ae95
commit 59b44816ec
12 changed files with 336 additions and 180 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

235
libraries/AP_GPS/AP_GPS_Auto.cpp
View File

@ -8,16 +8,7 @@
#include "AP_GPS.h" // includes AP_GPS_Auto.h
// Define this to add NMEA to the auto-detection cycle.
//
// Note that there is a potential race where NMEA data may overlap with
// the commands that switch a GPS out of NMEA mode that can cause
// the GPS to switch to binary mode at the same time that this code
// detects it as being in NMEA mode.
//
//#define WITH_NMEA_MODE 1
static unsigned int baudrates[] = {38400U, 57600U, 9600U, 4800U};
static const uint32_t baudrates[] PROGMEM = {38400U, 57600U, 9600U, 4800U};
const prog_char AP_GPS_Auto::_mtk_set_binary[] PROGMEM = MTK_SET_BINARY;
const prog_char AP_GPS_Auto::_sirf_set_binary[] PROGMEM = SIRF_SET_BINARY;
@ -36,7 +27,6 @@ void
AP_GPS_Auto::init(enum GPS_Engine_Setting nav_setting)
{
idleTimeout = 1200;
if (callback == NULL) callback = delay;
_nav_setting = nav_setting;
}
@ -46,48 +36,35 @@ AP_GPS_Auto::init(enum GPS_Engine_Setting nav_setting)
// We detect the real GPS, then update the pointer we have been called through
// and return.
//
/// @todo This routine spends a long time trying to detect a GPS. That's not strictly
/// desirable; it might be a good idea to rethink the logic here to make it
/// more asynchronous, so that other parts of the system can get a chance
/// to run while GPS detection is in progress.
///
bool
AP_GPS_Auto::read(void)
{
GPS *gps;
uint8_t i;
uint32_t then;
static uint32_t last_baud_change_ms;
static uint8_t last_baud;
GPS *gps;
uint32_t now = millis();
// Loop through possible baudrates trying to detect a GPS at one of them.
//
// Note that we need to have a FastSerial rather than a Stream here because
// Stream has no idea of line speeds. FastSerial is quite OK with us calling
// ::begin any number of times.
//
for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) {
if (now - last_baud_change_ms > 1200) {
// its been more than 1.2 seconds without detection on this
// GPS - switch to another baud rate
_fs->begin(pgm_read_dword(&baudrates[last_baud]), 256, 16);
last_baud++;
last_baud_change_ms = now;
if (last_baud == sizeof(baudrates) / sizeof(baudrates[0])) {
last_baud = 0;
}
// write config strings for the types of GPS we support
_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 the serial port has a large enough buffer for any protocol
_fs->begin(baudrates[i], 256, 16);
if (NULL != (gps = _detect())) {
// configure the detected GPS and give it a chance to listen to its device
gps->init(_nav_setting);
then = millis();
while ((millis() - then) < 1200) {
// if we get a successful update from the GPS, we are done
gps->new_data = false;
gps->update();
if (gps->new_data) {
Serial.println_P(PSTR("OK"));
*_gps = gps;
return true;
}
}
// GPS driver failed to parse any data from GPS,
// delete the driver and continue the process.
Serial.println_P(PSTR("failed, retrying"));
delete gps;
}
if (NULL != (gps = _detect())) {
// configure the detected GPS
gps->init(_nav_setting);
Serial.println_P(PSTR("OK"));
*_gps = gps;
return true;
}
return false;
}
@ -98,138 +75,40 @@ AP_GPS_Auto::read(void)
GPS *
AP_GPS_Auto::_detect(void)
{
uint32_t then;
uint8_t fingerprint[4];
uint8_t tries;
uint16_t charcount;
GPS *gps;
static uint32_t detect_started_ms;
//
// Loop attempting to detect a recognized GPS
//
Serial.print('G');
gps = NULL;
if (detect_started_ms == 0) {
detect_started_ms = millis();
}
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);
while (_port->available() > 0) {
uint8_t data = _port->read();
if (AP_GPS_UBLOX::_detect(data)) {
Serial.print_P(PSTR(" ublox "));
return new AP_GPS_UBLOX(_port);
}
if (AP_GPS_MTK16::_detect(data)) {
Serial.print_P(PSTR(" MTK16 "));
return new AP_GPS_MTK16(_port);
}
if (AP_GPS_MTK::_detect(data)) {
Serial.print_P(PSTR(" MTK "));
return new AP_GPS_MTK(_port);
}
if (AP_GPS_SIRF::_detect(data)) {
Serial.print_P(PSTR(" SIRF "));
return new AP_GPS_SIRF(_port);
}
if (millis() - detect_started_ms > 5000) {
// prevent false detection of NMEA mode in
// a MTK or UBLOX which has booted in NMEA mode
if (AP_GPS_NMEA::_detect(data)) {
Serial.print_P(PSTR(" NMEA "));
return new AP_GPS_NMEA(_port);
}
}
}
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);
return NULL;
}

59
libraries/AP_GPS/AP_GPS_MTK.cpp
View File

@ -158,3 +158,62 @@ restart:
}
return parsed;
}
/*
detect a MTK GPS
*/
bool
AP_GPS_MTK::_detect(uint8_t data)
{
static uint8_t payload_counter;
static uint8_t step;
static uint8_t ck_a, ck_b;
switch(step) {
case 1:
if (PREAMBLE2 == data) {
step++;
break;
}
step = 0;
case 0:
ck_b = ck_a = payload_counter = 0;
if(PREAMBLE1 == data)
step++;
break;
case 2:
if (MESSAGE_CLASS == data) {
step++;
ck_b = ck_a = data;
} else {
step = 0;
}
break;
case 3:
if (MESSAGE_ID == data) {
step++;
ck_b += (ck_a += data);
payload_counter = 0;
} else {
step = 0;
}
break;
case 4:
ck_b += (ck_a += data);
if (++payload_counter == sizeof(struct diyd_mtk_msg))
step++;
break;
case 5:
step++;
if (ck_a != data) {
step = 0;
}
break;
case 6:
step = 0;
if (ck_b == data) {
return true;
}
}
return false;
}

1
libraries/AP_GPS/AP_GPS_MTK.h
View File

@ -23,6 +23,7 @@ public:
AP_GPS_MTK(Stream *s);
virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual bool read(void);
static bool _detect(uint8_t );
private:
// XXX this is being ignored by the compiler #pragma pack(1)

54
libraries/AP_GPS/AP_GPS_MTK16.cpp
View File

@ -11,6 +11,7 @@
// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1"
//
#include <FastSerial.h>
#include "AP_GPS_MTK16.h"
#include <stdint.h>
#if defined(ARDUINO) && ARDUINO >= 100
@ -180,3 +181,56 @@ restart:
return parsed;
}
/*
detect a MTK16 GPS
*/
bool
AP_GPS_MTK16::_detect(uint8_t data)
{
static uint8_t payload_counter;
static uint8_t step;
static uint8_t ck_a, ck_b;
switch (step) {
case 1:
if (PREAMBLE2 == data) {
step++;
break;
}
step = 0;
case 0:
ck_b = ck_a = payload_counter = 0;
if (PREAMBLE1 == data)
step++;
break;
case 2:
if (data == sizeof(struct diyd_mtk_msg)) {
step++;
ck_b = ck_a = data;
} else {
step = 0;
}
break;
case 3:
ck_b += (ck_a += data);
if (++payload_counter == sizeof(struct diyd_mtk_msg))
step++;
break;
case 4:
step++;
if (ck_a != data) {
Serial.printf("wrong ck_a\n");
step = 0;
}
break;
case 5:
step = 0;
if (ck_b == data) {
return true;
}
Serial.printf("wrong ck_b\n");
break;
}
return false;
}

1
libraries/AP_GPS/AP_GPS_MTK16.h
View File

@ -21,6 +21,7 @@ public:
AP_GPS_MTK16(Stream *s);
virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual bool read(void);
static bool _detect(uint8_t );
private:
// XXX this is being ignored by the compiler #pragma pack(1)

43
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -371,3 +371,46 @@ bool AP_GPS_NMEA::_term_complete()
return false;
}
#define hexdigit(x) ((x)>9?'A'+(x):'0'+(x))
/*
detect a NMEA GPS. Adds one byte, and returns true if the stream
matches a NMEA string
*/
bool
AP_GPS_NMEA::_detect(uint8_t data)
{
static uint8_t step;
static uint8_t ck;
switch (step) {
case 0:
ck = 0;
if ('$' == data) {
step++;
}
break;
case 1:
if ('*' == data) {
step++;
} else {
ck ^= data;
}
break;
case 2:
if (hexdigit(ck>>4) == data) {
step++;
} else {
step = 0;
}
break;
case 3:
if (hexdigit(ck&0xF) == data) {
return true;
}
step = 0;
break;
}
return false;
}

2
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -66,6 +66,8 @@ public:
///
virtual bool read();
static bool _detect(uint8_t data);
private:
/// Coding for the GPS sentences that the parser handles
enum _sentence_types { //there are some more than 10 fields in some sentences , thus we have to increase these value.

54
libraries/AP_GPS/AP_GPS_SIRF.cpp
View File

@ -195,3 +195,57 @@ AP_GPS_SIRF::_accumulate(uint8_t val)
{
_checksum = (_checksum + val) & 0x7fff;
}
/*
detect a SIRF GPS
*/
bool
AP_GPS_SIRF::_detect(uint8_t data)
{
uint16_t checksum;
uint8_t step, payload_length, payload_counter;
switch (step) {
case 1:
if (PREAMBLE2 == data) {
step++;
break;
}
step = 0;
case 0:
payload_length = payload_counter = checksum = 0;
if (PREAMBLE1 == data)
step++;
break;
case 2:
step++;
if (data != 0) {
// only look for short messages
step = 0;
}
break;
case 3:
step++;
payload_length = data;
break;
case 4:
checksum = (checksum + data) & 0x7fff;
if (++payload_counter == payload_length)
step++;
break;
case 5:
step++;
if ((checksum >> 8) != data) {
step = 0;
}
break;
case 6:
step = 0;
if ((checksum & 0xff) == data) {
return true;
}
}
return false;
}

1
libraries/AP_GPS/AP_GPS_SIRF.h
View File

@ -21,6 +21,7 @@ public:
virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual bool read();
static bool _detect(uint8_t data);
private:
// XXX this is being ignored by the compiler #pragma pack(1)

62
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -355,3 +355,65 @@ AP_GPS_UBLOX::_configure_gps(void)
// ask for the current navigation settings
_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
}
/*
detect a Ublox GPS. Adds one byte, and returns true if the stream
matches a UBlox
*/
bool
AP_GPS_UBLOX::_detect(uint8_t data)
{
static uint8_t payload_length, payload_counter;
static uint8_t step;
static uint8_t ck_a, ck_b;
switch (step) {
case 1:
if (PREAMBLE2 == data) {
step++;
break;
}
step = 0;
case 0:
if (PREAMBLE1 == data)
step++;
break;
case 2:
step++;
ck_b = ck_a = data;
break;
case 3:
step++;
ck_b += (ck_a += data);
break;
case 4:
step++;
ck_b += (ck_a += data);
payload_length = data;
break;
case 5:
step++;
ck_b += (ck_a += data);
payload_counter = 0;
break;
case 6:
ck_b += (ck_a += data);
if (++payload_counter == payload_length)
step++;
break;
case 7:
step++;
if (ck_a != data) {
step = 0;
}
break;
case 8:
step = 0;
if (ck_b == data) {
// a valid UBlox packet
return true;
}
}
return false;
}

2
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -20,6 +20,7 @@
* 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
@ -29,6 +30,7 @@ public:
AP_GPS_UBLOX(Stream *s);
virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual bool read();
static bool _detect(uint8_t );
static const prog_char _ublox_set_binary[];
static const uint8_t _ublox_set_binary_size;

2
libraries/AP_GPS/GPS.h
View File

@ -25,8 +25,6 @@ public:
///
void update(void);
void (*callback)(unsigned long t);
/// GPS status codes
///
/// \note Non-intuitive ordering for legacy reasons