aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GPS: use SerialManager for init

Browse Source
This commit is contained in:
Randy Mackay 2015-01-19 22:36:35 +09:00 committed by Andrew Tridgell
parent 0c3a8f585a
commit 2eddecb10d
2 changed files with 30 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AP_GPS/AP_GPS.cpp
View File

@ -83,14 +83,19 @@ const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = {
};
/// Startup initialisation.
void AP_GPS::init(DataFlash_Class *dataflash)
void AP_GPS::init(DataFlash_Class *dataflash, const AP_SerialManager& serial_manager)
{
_DataFlash = dataflash;
hal.uartB->begin(38400UL, 256, 16);
primary_instance = 0;
// search for serial ports with gps protocol
AP_SerialManager::serial_state gps_serial;
if (serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, gps_serial)) {
_port[0] = gps_serial.uart;
}
#if GPS_MAX_INSTANCES > 1
if (hal.uartE != NULL) {
hal.uartE->begin(38400UL, 256, 16);
if (serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS2, gps_serial)) {
_port[1] = gps_serial.uart;
}
#endif
}
@ -118,15 +123,19 @@ void AP_GPS::send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t
*/
void AP_GPS::send_blob_update(uint8_t instance)
{
// exit immediately if no uart for this instance
if (_port[instance] == NULL) {
return;
}
// see if we can write some more of the initialisation blob
if (initblob_state[instance].remaining > 0) {
AP_HAL::UARTDriver *port = instance==0?hal.uartB:hal.uartE;
int16_t space = port->txspace();
int16_t space = _port[instance]->txspace();
if (space > (int16_t)initblob_state[instance].remaining) {
space = initblob_state[instance].remaining;
}
while (space > 0) {
port->write(pgm_read_byte(initblob_state[instance].blob));
_port[instance]->write(pgm_read_byte(initblob_state[instance].blob));
initblob_state[instance].blob++;
space--;
initblob_state[instance].remaining--;
@ -143,7 +152,6 @@ void
AP_GPS::detect_instance(uint8_t instance)
{
AP_GPS_Backend *new_gps = NULL;
AP_HAL::UARTDriver *port = instance==0?hal.uartB:hal.uartE;
struct detect_state *dstate = &detect_state[instance];
uint32_t now = hal.scheduler->millis();
@ -152,12 +160,12 @@ AP_GPS::detect_instance(uint8_t instance)
// check for explicitely chosen PX4 GPS beforehand
// it is not possible to autodetect it, nor does it require a real UART
hal.console->print_P(PSTR(" PX4 "));
new_gps = new AP_GPS_PX4(*this, state[instance], port);
new_gps = new AP_GPS_PX4(*this, state[instance], _port[instance]);
goto found_gps;
}
#endif
if (port == NULL) {
if (_port[instance] == NULL) {
// UART not available
return;
}
@ -178,15 +186,15 @@ AP_GPS::detect_instance(uint8_t instance)
dstate->last_baud = 0;
}
uint32_t baudrate = pgm_read_dword(&_baudrates[dstate->last_baud]);
port->begin(baudrate, 256, 16);
_port[instance]->begin(baudrate, 256, 16);
dstate->last_baud_change_ms = now;
send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
}
send_blob_update(instance);
while (port->available() > 0 && new_gps == NULL) {
uint8_t data = port->read();
while (_port[instance]->available() > 0 && new_gps == NULL) {
uint8_t data = _port[instance]->read();
/*
running a uBlox at less than 38400 will lead to packet
corruption, as we can't receive the packets in the 200ms
@ -198,23 +206,23 @@ AP_GPS::detect_instance(uint8_t instance)
pgm_read_dword(&_baudrates[dstate->last_baud]) >= 38400 &&
AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) {
hal.console->print_P(PSTR(" ublox "));
new_gps = new AP_GPS_UBLOX(*this, state[instance], port);
new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance]);
}
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK19) &&
AP_GPS_MTK19::_detect(dstate->mtk19_detect_state, data)) {
hal.console->print_P(PSTR(" MTK19 "));
new_gps = new AP_GPS_MTK19(*this, state[instance], port);
new_gps = new AP_GPS_MTK19(*this, state[instance], _port[instance]);
}
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK) &&
AP_GPS_MTK::_detect(dstate->mtk_detect_state, data)) {
hal.console->print_P(PSTR(" MTK "));
new_gps = new AP_GPS_MTK(*this, state[instance], port);
new_gps = new AP_GPS_MTK(*this, state[instance], _port[instance]);
}
#if GPS_RTK_AVAILABLE
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) &&
AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) {
hal.console->print_P(PSTR(" SBP "));
new_gps = new AP_GPS_SBP(*this, state[instance], port);
new_gps = new AP_GPS_SBP(*this, state[instance], _port[instance]);
}
#endif // HAL_CPU_CLASS
#if !defined(GPS_SKIP_SIRF_NMEA)
@ -222,7 +230,7 @@ AP_GPS::detect_instance(uint8_t instance)
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) &&
AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) {
hal.console->print_P(PSTR(" SIRF "));
new_gps = new AP_GPS_SIRF(*this, state[instance], port);
new_gps = new AP_GPS_SIRF(*this, state[instance], _port[instance]);
}
else if (now - dstate->detect_started_ms > 5000) {
// prevent false detection of NMEA mode in
@ -230,7 +238,7 @@ AP_GPS::detect_instance(uint8_t instance)
if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_NMEA) &&
AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) {
hal.console->print_P(PSTR(" NMEA "));
new_gps = new AP_GPS_NMEA(*this, state[instance], port);
new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]);
}
}
#endif

4
libraries/AP_GPS/AP_GPS.h
View File

@ -26,6 +26,7 @@
#include <GCS_MAVLink.h>
#include <AP_Vehicle.h>
#include "GPS_detect_state.h"
#include <AP_SerialManager.h>
/**
maximum number of GPS instances available on this platform. If more
@ -66,7 +67,7 @@ public:
}
/// Startup initialisation.
void init(DataFlash_Class *dataflash);
void init(DataFlash_Class *dataflash, const AP_SerialManager& serial_manager);
/// Update GPS state based on possible bytes received from the module.
/// This routine must be called periodically (typically at 10Hz or
@ -370,6 +371,7 @@ private:
GPS_timing timing[GPS_MAX_INSTANCES];
GPS_State state[GPS_MAX_INSTANCES];
AP_GPS_Backend *drivers[GPS_MAX_INSTANCES];
AP_HAL::UARTDriver *_port[GPS_MAX_INSTANCES];
/// primary GPS instance
uint8_t primary_instance:2;