mirror of https://github.com/ArduPilot/ardupilot
GPS: move _port setting to init()
this is needed to avoid constructor ordering dependencies
This commit is contained in:
parent
107ab1a694
commit
18db7b2efd
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
||||
/**
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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?
|
||||
|
|
|
@ -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 );
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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 );
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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,
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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.
|
||||
|
||||
|
|
|
@ -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 );
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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.
|
||||
|
|
Loading…
Reference in New Issue