GPS: move _port setting to init()

this is needed to avoid constructor ordering dependencies
This commit is contained in:
Andrew Tridgell 2012-12-18 13:31:05 +11:00
parent 107ab1a694
commit 18db7b2efd
23 changed files with 56 additions and 110 deletions

View File

@ -179,7 +179,7 @@ static void init_ardupilot()
// Do GPS init
g_gps = &g_gps_driver;
// GPS Initialization
g_gps->init(GPS::GPS_ENGINE_AIRBORNE_4G);
g_gps->init(hal.uartB, GPS::GPS_ENGINE_AIRBORNE_4G);
//mavlink_system.sysid = MAV_SYSTEM_ID; // Using g.sysid_this_mav
mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id

View File

@ -17,18 +17,13 @@ extern const AP_HAL::HAL& hal;
static const char init_str[] = "$PSRF100,0,57600,8,1,0*37";
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_406::AP_GPS_406(AP_HAL::UARTDriver *s) : AP_GPS_SIRF(s)
{
}
// Public Methods ////////////////////////////////////////////////////////////////////
void AP_GPS_406::init(enum GPS_Engine_Setting nav_setting)
void AP_GPS_406::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
{
_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
AP_GPS_SIRF::init(nav_setting); // let the superclass do anything it might need here
AP_GPS_SIRF::init(s, nav_setting); // let the superclass do anything it might need here
idleTimeout = 1200;
}

View File

@ -22,8 +22,7 @@ class AP_GPS_406 : public AP_GPS_SIRF
{
public:
// Methods
AP_GPS_406(AP_HAL::UARTDriver *port);
virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
private:
void _change_to_sirf_protocol(void);

View File

@ -16,8 +16,7 @@ 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;
AP_GPS_Auto::AP_GPS_Auto(AP_HAL::UARTDriver *u, GPS **gps) :
GPS(u),
AP_GPS_Auto::AP_GPS_Auto(GPS **gps) :
_gps(gps)
{
}
@ -25,8 +24,9 @@ AP_GPS_Auto::AP_GPS_Auto(AP_HAL::UARTDriver *u, GPS **gps) :
// Do nothing at init time - it may be too early to try detecting the GPS
//
void
AP_GPS_Auto::init(enum GPS_Engine_Setting nav_setting)
AP_GPS_Auto::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
{
_port = s;
idleTimeout = 1200;
_nav_setting = nav_setting;
}
@ -64,7 +64,7 @@ AP_GPS_Auto::read(void)
if (NULL != (gps = _detect())) {
// configure the detected GPS
gps->init(_nav_setting);
gps->init(_port, _nav_setting);
hal.console->println_P(PSTR("OK"));
*_gps = gps;
return true;
@ -79,6 +79,7 @@ GPS *
AP_GPS_Auto::_detect(void)
{
static uint32_t detect_started_ms;
GPS *new_gps = NULL;
if (detect_started_ms == 0 && _port->available() > 0) {
detect_started_ms = hal.scheduler->millis();
@ -88,34 +89,38 @@ AP_GPS_Auto::_detect(void)
uint8_t data = _port->read();
if (AP_GPS_UBLOX::_detect(data)) {
hal.console->print_P(PSTR(" ublox "));
return new AP_GPS_UBLOX(_port);
}
if (AP_GPS_MTK16::_detect(data)) {
new_gps = new AP_GPS_UBLOX();
}
else if (AP_GPS_MTK16::_detect(data)) {
hal.console->print_P(PSTR(" MTK16 "));
return new AP_GPS_MTK16(_port);
}
if (AP_GPS_MTK::_detect(data)) {
new_gps = new AP_GPS_MTK16();
}
else if (AP_GPS_MTK::_detect(data)) {
hal.console->print_P(PSTR(" MTK "));
return new AP_GPS_MTK(_port);
new_gps = new AP_GPS_MTK();
}
#if !defined( __AVR_ATmega1280__ )
// save a bit of code space on a 1280
if (AP_GPS_SIRF::_detect(data)) {
else if (AP_GPS_SIRF::_detect(data)) {
hal.console->print_P(PSTR(" SIRF "));
return new AP_GPS_SIRF(_port);
new_gps = new AP_GPS_SIRF();
}
if (hal.scheduler->millis() - detect_started_ms > 5000) {
else if (hal.scheduler->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)) {
hal.console->print_P(PSTR(" NMEA "));
return new AP_GPS_NMEA(_port);
new_gps = new AP_GPS_NMEA();
}
}
#endif
}
return NULL;
if (new_gps != NULL) {
new_gps->init(_port);
}
return new_gps;
}

View File

@ -21,10 +21,10 @@ public:
/// @param ptr Pointer to a GPS * that will be fixed up by ::init
/// when the GPS type has been detected.
///
AP_GPS_Auto(AP_HAL::UARTDriver *s, GPS **gps);
AP_GPS_Auto(GPS **gps);
/// Dummy init routine, does nothing
virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
/// Detect and initialise the attached GPS unit. Updates the
/// pointer passed into the constructor when a GPS is detected.

View File

@ -14,14 +14,10 @@
#include <AP_HAL.h>
#include "AP_GPS_HIL.h"
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_HIL::AP_GPS_HIL(AP_HAL::UARTDriver* s) : GPS(s)
{
}
// Public Methods //////////////////////////////////////////////////////////////
void AP_GPS_HIL::init(enum GPS_Engine_Setting nav_setting)
void AP_GPS_HIL::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
{
_port = s;
idleTimeout = 1200;
}

View File

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

View File

@ -31,17 +31,12 @@
#include <AP_HAL.h>
#include "AP_GPS_IMU.h"
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_IMU::AP_GPS_IMU(AP_HAL::UARTDriver *s) : GPS(s)
{
}
// Public Methods //////////////////////////////////////////////////////////////
void
AP_GPS_IMU::init(enum GPS_Engine_Setting nav_setting)
AP_GPS_IMU::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
{
// we expect the stream to already be open at the corret bitrate
_port = s;
idleTimeout = 1200;
}

View File

@ -11,8 +11,7 @@ class AP_GPS_IMU : public GPS {
public:
// Methods
AP_GPS_IMU(AP_HAL::UARTDriver *s);
virtual void init(enum GPS_Engine_Setting nav_setting);
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting);
virtual bool read(void);
// Properties

View File

@ -17,15 +17,11 @@
#include "AP_GPS_MTK.h"
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_MTK::AP_GPS_MTK(AP_HAL::UARTDriver *s) : GPS(s)
{
}
// Public Methods //////////////////////////////////////////////////////////////
void
AP_GPS_MTK::init(enum GPS_Engine_Setting nav_setting)
AP_GPS_MTK::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
{
_port = s;
_port->flush();
// initialize serial port for binary protocol use
// XXX should assume binary, let GPS_AUTO handle dynamic config?

View File

@ -20,8 +20,7 @@
class AP_GPS_MTK : public GPS {
public:
AP_GPS_MTK(AP_HAL::UARTDriver *s);
virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual bool read(void);
static bool _detect(uint8_t );

View File

@ -16,15 +16,11 @@
extern const AP_HAL::HAL& hal;
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_MTK16::AP_GPS_MTK16(AP_HAL::UARTDriver *s) : GPS(s)
{
}
// Public Methods //////////////////////////////////////////////////////////////
void
AP_GPS_MTK16::init(enum GPS_Engine_Setting nav_setting)
AP_GPS_MTK16::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
{
_port = s;
_port->flush();
// initialize serial port for binary protocol use

View File

@ -19,8 +19,7 @@
class AP_GPS_MTK16 : public GPS {
public:
AP_GPS_MTK16(AP_HAL::UARTDriver *s);
virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual bool read(void);
static bool _detect(uint8_t );

View File

@ -90,15 +90,11 @@ const char AP_GPS_NMEA::_gpvtg_string[] PROGMEM = "GPVTG";
//
#define DIGIT_TO_VAL(_x) (_x - '0')
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_NMEA::AP_GPS_NMEA(AP_HAL::UARTDriver *s) :
GPS(s)
{
}
// Public Methods //////////////////////////////////////////////////////////////
void AP_GPS_NMEA::init(enum GPS_Engine_Setting nav_setting)
void AP_GPS_NMEA::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
{
_port = s;
// send the SiRF init strings
_port->print_P((const prog_char_t *)_SiRF_init_string);

View File

@ -52,14 +52,10 @@
class AP_GPS_NMEA : public GPS
{
public:
/// Constructor
///
AP_GPS_NMEA(AP_HAL::UARTDriver* s);
/// Perform a (re)initialisation of the GPS; sends the
/// protocol configuration messages.
///
virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
/// Checks the serial receive buffer for characters,
/// attempts to parse NMEA data and updates internal state

View File

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

View File

@ -23,15 +23,11 @@ static uint8_t init_messages[] = {
0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3
};
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_SIRF::AP_GPS_SIRF(AP_HAL::UARTDriver *s) : GPS(s)
{
}
// Public Methods //////////////////////////////////////////////////////////////
void
AP_GPS_SIRF::init(enum GPS_Engine_Setting nav_setting)
AP_GPS_SIRF::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
{
_port = s;
_port->flush();
// For modules that default to something other than SiRF binary,

View File

@ -18,9 +18,7 @@
class AP_GPS_SIRF : public GPS {
public:
AP_GPS_SIRF(AP_HAL::UARTDriver *s);
virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual bool read();
static bool _detect(uint8_t data);

View File

@ -17,10 +17,8 @@
class AP_GPS_Shim : public GPS
{
public:
AP_GPS_Shim() : GPS(NULL) {
}
virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE) {
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE) {
_port = s;
};
virtual bool read(void) {
bool updated = _updated;

View File

@ -28,17 +28,13 @@ extern const AP_HAL::HAL& hal;
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 ////////////////////////////////////////////////////////////////
AP_GPS_UBLOX::AP_GPS_UBLOX(AP_HAL::UARTDriver *s) : GPS(s)
{
}
// Public Methods //////////////////////////////////////////////////////////////
void
AP_GPS_UBLOX::init(enum GPS_Engine_Setting nav_setting)
AP_GPS_UBLOX::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
{
_port = s;
// XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the
// right reporting configuration.

View File

@ -28,8 +28,7 @@ class AP_GPS_UBLOX : public GPS
{
public:
// Methods
AP_GPS_UBLOX(AP_HAL::UARTDriver *s);
virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
virtual bool read();
static bool _detect(uint8_t );

View File

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

View File

@ -86,7 +86,7 @@ public:
///
/// Must be implemented by the GPS driver.
///
virtual void init(enum GPS_Engine_Setting engine_setting = GPS_ENGINE_NONE) = 0;
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting engine_setting = GPS_ENGINE_NONE) = 0;
// Properties
uint32_t time; ///< GPS time (milliseconds from epoch)
@ -153,16 +153,6 @@ public:
protected:
AP_HAL::UARTDriver *_port; ///< port the GPS is attached to
/// Constructor
///
/// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called.
///
/// @param u UARTDriver connected to the GPS module.
///
GPS(AP_HAL::UARTDriver *u) : _port(u) {
};
/// read from the GPS stream and update properties
///
/// Must be implemented by the GPS driver.