GPS: add stub settings for nav_setting in the other GPS drivers

only Ublox supports nav_setting so far
This commit is contained in:
Andrew Tridgell 2012-06-10 16:34:53 +10:00
parent 514be604a5
commit 33a2ad7e07
20 changed files with 38 additions and 22 deletions

View File

@ -25,12 +25,12 @@ AP_GPS_406::AP_GPS_406(Stream *s) : AP_GPS_SIRF(s)
} }
// Public Methods //////////////////////////////////////////////////////////////////// // Public Methods ////////////////////////////////////////////////////////////////////
void AP_GPS_406::init(void) void AP_GPS_406::init(enum GPS_Engine_Setting nav_setting)
{ {
_change_to_sirf_protocol(); // Changes to SIRF protocol and sets baud rate _change_to_sirf_protocol(); // Changes to SIRF protocol and sets baud rate
_configure_gps(); // Function to configure GPS, to output only the desired msg's _configure_gps(); // Function to configure GPS, to output only the desired msg's
AP_GPS_SIRF::init(); // let the superclass do anything it might need here AP_GPS_SIRF::init(nav_setting); // let the superclass do anything it might need here
idleTimeout = 1200; idleTimeout = 1200;
} }

View File

@ -21,7 +21,7 @@ class AP_GPS_406 : public AP_GPS_SIRF
public: public:
// Methods // Methods
AP_GPS_406(Stream *port); AP_GPS_406(Stream *port);
virtual void init(void); virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
private: private:
void _change_to_sirf_protocol(void); void _change_to_sirf_protocol(void);

View File

@ -34,10 +34,11 @@ AP_GPS_Auto::AP_GPS_Auto(FastSerial *s, GPS **gps) :
// Do nothing at init time - it may be too early to try detecting the GPS // Do nothing at init time - it may be too early to try detecting the GPS
// //
void void
AP_GPS_Auto::init(void) AP_GPS_Auto::init(enum GPS_Engine_Setting nav_setting)
{ {
idleTimeout = 1200; idleTimeout = 1200;
if (callback == NULL) callback = delay; if (callback == NULL) callback = delay;
_nav_setting = nav_setting;
} }
// Called the first time that a client tries to kick the GPS to update. // Called the first time that a client tries to kick the GPS to update.
@ -70,7 +71,7 @@ AP_GPS_Auto::read(void)
if (NULL != (gps = _detect())) { if (NULL != (gps = _detect())) {
// configure the detected GPS and give it a chance to listen to its device // configure the detected GPS and give it a chance to listen to its device
gps->init(); gps->init(_nav_setting);
then = millis(); then = millis();
while ((millis() - then) < 1200) { while ((millis() - then) < 1200) {
// if we get a successful update from the GPS, we are done // if we get a successful update from the GPS, we are done

View File

@ -24,7 +24,7 @@ public:
AP_GPS_Auto(FastSerial *s, GPS **gps); AP_GPS_Auto(FastSerial *s, GPS **gps);
/// Dummy init routine, does nothing /// Dummy init routine, does nothing
virtual void init(void); virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
/// Detect and initialise the attached GPS unit. Updates the /// Detect and initialise the attached GPS unit. Updates the
/// pointer passed into the constructor when a GPS is detected. /// pointer passed into the constructor when a GPS is detected.
@ -47,5 +47,6 @@ private:
static const prog_char _ublox_set_binary[]; static const prog_char _ublox_set_binary[];
static const prog_char _sirf_set_binary[]; static const prog_char _sirf_set_binary[];
enum GPS_Engine_Setting _nav_setting;
}; };
#endif #endif

View File

@ -24,7 +24,7 @@ AP_GPS_HIL::AP_GPS_HIL(Stream *s) : GPS(s)
} }
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void AP_GPS_HIL::init(void) void AP_GPS_HIL::init(enum GPS_Engine_Setting nav_setting)
{ {
idleTimeout = 1200; idleTimeout = 1200;
} }

View File

@ -17,7 +17,7 @@
class AP_GPS_HIL : public GPS { class AP_GPS_HIL : public GPS {
public: public:
AP_GPS_HIL(Stream *s); AP_GPS_HIL(Stream *s);
virtual void init(void); virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual bool read(void); virtual bool read(void);
/** /**

View File

@ -43,7 +43,7 @@ AP_GPS_IMU::AP_GPS_IMU(Stream *s) : GPS(s)
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void void
AP_GPS_IMU::init(void) AP_GPS_IMU::init(enum GPS_Engine_Setting nav_setting)
{ {
// we expect the stream to already be open at the corret bitrate // we expect the stream to already be open at the corret bitrate
idleTimeout = 1200; idleTimeout = 1200;

View File

@ -11,7 +11,7 @@ public:
// Methods // Methods
AP_GPS_IMU(Stream *s); AP_GPS_IMU(Stream *s);
virtual void init(void); virtual void init(enum GPS_Engine_Setting nav_setting);
virtual bool read(void); virtual bool read(void);
// Properties // Properties

View File

@ -21,7 +21,7 @@ AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s)
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void void
AP_GPS_MTK::init(void) AP_GPS_MTK::init(enum GPS_Engine_Setting nav_setting)
{ {
_port->flush(); _port->flush();
// initialize serial port for binary protocol use // initialize serial port for binary protocol use

View File

@ -21,7 +21,7 @@
class AP_GPS_MTK : public GPS { class AP_GPS_MTK : public GPS {
public: public:
AP_GPS_MTK(Stream *s); AP_GPS_MTK(Stream *s);
virtual void init(void); virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual bool read(void); virtual bool read(void);
private: private:

View File

@ -26,7 +26,7 @@ AP_GPS_MTK16::AP_GPS_MTK16(Stream *s) : GPS(s)
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void void
AP_GPS_MTK16::init(void) AP_GPS_MTK16::init(enum GPS_Engine_Setting nav_setting)
{ {
_port->flush(); _port->flush();

View File

@ -19,7 +19,7 @@
class AP_GPS_MTK16 : public GPS { class AP_GPS_MTK16 : public GPS {
public: public:
AP_GPS_MTK16(Stream *s); AP_GPS_MTK16(Stream *s);
virtual void init(void); virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual bool read(void); virtual bool read(void);
private: private:

View File

@ -97,7 +97,7 @@ AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) :
} }
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void AP_GPS_NMEA::init(void) void AP_GPS_NMEA::init(enum GPS_Engine_Setting nav_setting)
{ {
BetterStream *bs = (BetterStream *)_port; BetterStream *bs = (BetterStream *)_port;

View File

@ -58,7 +58,7 @@ public:
/// Perform a (re)initialisation of the GPS; sends the /// Perform a (re)initialisation of the GPS; sends the
/// protocol configuration messages. /// protocol configuration messages.
/// ///
virtual void init(); virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
/// Checks the serial receive buffer for characters, /// Checks the serial receive buffer for characters,
/// attempts to parse NMEA data and updates internal state /// attempts to parse NMEA data and updates internal state

View File

@ -9,7 +9,7 @@ class AP_GPS_None : public GPS
{ {
public: public:
AP_GPS_None(Stream *s) : GPS(s) {} AP_GPS_None(Stream *s) : GPS(s) {}
virtual void init(void) {}; virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE) {};
virtual bool read(void) { virtual bool read(void) {
return false; return false;
}; };

View File

@ -30,7 +30,7 @@ AP_GPS_SIRF::AP_GPS_SIRF(Stream *s) : GPS(s)
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void void
AP_GPS_SIRF::init(void) AP_GPS_SIRF::init(enum GPS_Engine_Setting nav_setting)
{ {
_port->flush(); _port->flush();

View File

@ -19,7 +19,7 @@ class AP_GPS_SIRF : public GPS {
public: public:
AP_GPS_SIRF(Stream *s); AP_GPS_SIRF(Stream *s);
virtual void init(); virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual bool read(); virtual bool read();
private: private:

View File

@ -19,7 +19,7 @@ class AP_GPS_Shim : public GPS
public: public:
AP_GPS_Shim() : GPS(NULL) {} AP_GPS_Shim() : GPS(NULL) {}
virtual void init(void) {}; virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE) {};
virtual bool read(void) { virtual bool read(void) {
bool updated = _updated; bool updated = _updated;
_updated = false; _updated = false;

View File

@ -35,7 +35,7 @@ GPS::update(void)
Debug("gps read timeout %lu %lu", (unsigned long)tnow, (unsigned long)_idleTimer); Debug("gps read timeout %lu %lu", (unsigned long)tnow, (unsigned long)_idleTimer);
_status = NO_GPS; _status = NO_GPS;
init(); init(_nav_setting);
// reset the idle timer // reset the idle timer
_idleTimer = tnow; _idleTimer = tnow;
} }

View File

@ -36,6 +36,18 @@ public:
GPS_OK = 2 ///< Receiving valid messages and locked GPS_OK = 2 ///< Receiving valid messages and locked
}; };
// GPS navigation engine settings. Not all GPS receivers support
// this
enum GPS_Engine_Setting {
GPS_ENGINE_NONE = -1,
GPS_ENGINE_PEDESTRIAN = 3,
GPS_ENGINE_AUTOMOTIVE = 4,
GPS_ENGINE_SEA = 5,
GPS_ENGINE_AIRBORNE_1G = 6,
GPS_ENGINE_AIRBORNE_2G = 7,
GPS_ENGINE_AIRBORNE_4G = 8
};
/// Query GPS status /// Query GPS status
/// ///
/// The 'valid message' status indicates that a recognised message was /// The 'valid message' status indicates that a recognised message was
@ -72,7 +84,7 @@ public:
/// ///
/// Must be implemented by the GPS driver. /// Must be implemented by the GPS driver.
/// ///
virtual void init(void) = 0; virtual void init(enum GPS_Engine_Setting engine_setting = GPS_ENGINE_NONE) = 0;
// Properties // Properties
uint32_t time; ///< GPS time (milliseconds from epoch) uint32_t time; ///< GPS time (milliseconds from epoch)
@ -166,6 +178,8 @@ protected:
/// Time epoch code for the gps in use /// Time epoch code for the gps in use
GPS_Time_Epoch _epoch; GPS_Time_Epoch _epoch;
enum GPS_Engine_Setting _nav_setting;
private: private: