diff --git a/libraries/AP_GPS/AP_GPS_Auto.cpp b/libraries/AP_GPS/AP_GPS_Auto.cpp index 63ee4cc040..727b1eac74 100644 --- a/libraries/AP_GPS/AP_GPS_Auto.cpp +++ b/libraries/AP_GPS/AP_GPS_Auto.cpp @@ -10,8 +10,18 @@ static unsigned int baudrates[] = {38400U, 57600U, 9600U, 4800U}; -GPS * -AP_GPS_Auto::detect(void) +void +AP_GPS_Auto::init(void) +{ +} + +// +// 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 +// and return. +void +AP_GPS_Auto::update(void) { GPS *gps; int i; @@ -20,10 +30,21 @@ AP_GPS_Auto::detect(void) for (;;) { // loop through possible baudrates for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) { - printf("autodetect at %d:%u\n", i, baudrates[i]); + printf("GPS autodetect at %d:%u\n", i, baudrates[i]); _port->begin(baudrates[i]); - if (NULL != (gps = _detect())) - return(gps); + if (NULL != (gps = _detect())) { + // make the detected GPS the default + *_gps = gps; + + // configure the detected GPS and run one update + gps->print_errors = true; // XXX + gps->init(); + gps->update(); + + // drop back to our caller - subsequent calls through + // the global will not come here + return; + } } } } @@ -129,12 +150,6 @@ AP_GPS_Auto::_detect(void) break; } } - - // If we detected a GPS, call its init routine - if (NULL != gps) { - gps->print_errors = true; // XXX - gps->init(); - } return(gps); } diff --git a/libraries/AP_GPS/AP_GPS_Auto.h b/libraries/AP_GPS/AP_GPS_Auto.h index 72b42cbdd9..a259add7c0 100644 --- a/libraries/AP_GPS/AP_GPS_Auto.h +++ b/libraries/AP_GPS/AP_GPS_Auto.h @@ -9,7 +9,7 @@ #include #include -class AP_GPS_Auto +class AP_GPS_Auto : public GPS { public: /// Constructor @@ -21,17 +21,26 @@ public: /// @param ptr Pointer to a GPS * that will be fixed up by ::init /// when the GPS type has been detected. /// - AP_GPS_Auto(FastSerial *port = NULL) : _port(port) {}; + AP_GPS_Auto(FastSerial *port, GPS **gps) : + _port(port), + _gps(gps) + {}; + + void init(void); /// Detect and initialise the attached GPS unit. Returns a /// pointer to the allocated & initialised GPS driver. /// - GPS *detect(void); + void update(void); private: /// Serial port connected to the GPS. FastSerial *_port; + /// global GPS driver pointer, updated by auto-detection + /// + GPS **_gps; + /// low-level auto-detect routine /// GPS *_detect(void); diff --git a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde index 2491d4697a..d60057e532 100644 --- a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde +++ b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde @@ -4,23 +4,30 @@ #include #include +#include FastSerialPort0(Serial); FastSerialPort1(Serial1); GPS *gps; -AP_GPS_Auto GPS(&Serial1); +AP_GPS_Auto GPS(&Serial1, &gps); #define T6 1000000 #define T7 10000000 +void * operator new(size_t size) +{ + return(calloc(size, 1)); +} + void setup() { Serial.begin(38400); Serial1.begin(38400); Serial.println("GPS AUTO library test"); - gps = GPS.detect(); // GPS Initialization + gps = &GPS; + gps->init(); delay(1000); } void loop()