/*
update one GPS instance. This should be called at 10Hz or greater
*/
void
AP_GPS::update_instance(uint8_t instance)
{
if (_type[instance] == GPS_TYPE_HIL) {
// in HIL, leave info alone
return;
}
if (_type[instance] == GPS_TYPE_NONE) {
// not enabled
state[instance].status = NO_GPS;
state[instance].hdop = 9999;
return;
}
if (locked_ports & (1U<<instance)) {
// the port is locked by another driver
return;
}
if (drivers[instance] == NULL || state[instance].status == NO_GPS) {
// we don't yet know the GPS type of this one, or it has timed
// out and needs to be re-initialised
detect_instance(instance);
return;
}
send_blob_update(instance);
// we have an active driver for this instance
bool result = drivers[instance]->read();
uint32_t tnow = AP_HAL::millis();
// if we did not get a message, and the idle timer of 1.2 seconds
// has expired, re-initialise the GPS. This will cause GPS
// detection to run again
if (!result) {
if (tnow - timing[instance].last_message_time_ms > 1200) {
// free the driver before we run the next detection, so we
// don't end up with two allocated at any time
delete drivers[instance];
drivers[instance] = NULL;
memset(&state[instance], 0, sizeof(state[instance]));
state[instance].instance = instance;
state[instance].status = NO_GPS;
state[instance].hdop = 9999;
timing[instance].last_message_time_ms = tnow;
}
} else {
timing[instance].last_message_time_ms = tnow;
if (state[instance].status >= GPS_OK_FIX_2D) {
timing[instance].last_fix_time_ms = tnow;
}
}
}
/*
run detection step for one GPS instance. If this finds a GPS then it
will fill in drivers[instance] and change state[instance].status
from NO_GPS to NO_FIX.
*/
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];
if (port == NULL) {
// UART not available
return;
}
uint32_t now = hal.scheduler->millis();
state[instance].instance = instance;
state[instance].status = NO_GPS;
// record the time when we started detection. This is used to try
// to avoid initialising a uBlox as a NMEA GPS
if (dstate->detect_started_ms == 0) {
dstate->detect_started_ms = now;
}
if (now - dstate->last_baud_change_ms > 1200) {
// try the next baud rate
dstate->last_baud++;
if (dstate->last_baud == sizeof(_baudrates) / sizeof(_baudrates[0])) {
dstate->last_baud = 0;
}
uint32_t baudrate = pgm_read_dword(&_baudrates[dstate->last_baud]);
port->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();
/*
running a uBlox at less than 38400 will lead to packet
corruption, as we can't receive the packets in the 200ms
window for 5Hz fixes. The NMEA startup message should force
the uBlox into 38400 no matter what rate it is configured
for.
*/
if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_UBLOX) &&
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);
}
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);
}
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);
}
#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);
}
#endif // HAL_CPU_CLASS
#if !defined( __AVR_ATmega1280__ )
// save a bit of code space on a 1280
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);
}
else if (now - dstate->detect_started_ms > 5000) {
// prevent false detection of NMEA mode in
// a MTK or UBLOX which has booted in NMEA mode
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);
}
}
#endif
}
if (new_gps != NULL) {
state[instance].status = NO_FIX;
drivers[instance] = new_gps;
timing[instance].last_message_time_ms = now;
}
}
/*
run detection step for one GPS instance. If this finds a GPS then it
will fill in drivers[instance] and change state[instance].status
from NO_GPS to NO_FIX.
*/
void
AP_GPS::detect_instance(uint8_t instance)
{
AP_GPS_Backend *new_gps = NULL;
struct detect_state *dstate = &detect_state[instance];
uint32_t now = AP_HAL::millis();
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
if (_type[instance] == GPS_TYPE_PX4) {
// check for explicitely chosen PX4 GPS beforehand
// it is not possible to autodetect it, nor does it require a real UART
hal.console->print(" PX4 ");
new_gps = new AP_GPS_PX4(*this, state[instance], _port[instance]);
goto found_gps;
}
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
if (_type[instance] == GPS_TYPE_QURT) {
hal.console->print(" QURTGPS ");
new_gps = new AP_GPS_QURT(*this, state[instance], _port[instance]);
goto found_gps;
}
#endif
if (_port[instance] == NULL) {
// UART not available
return;
}
state[instance].instance = instance;
state[instance].status = NO_GPS;
state[instance].hdop = 9999;
// by default the sbf/trimble gps outputs no data on its port, until configured.
if (_type[instance] == GPS_TYPE_SBF) {
hal.console->print(" SBF ");
new_gps = new AP_GPS_SBF(*this, state[instance], _port[instance]);
} else if ((_type[instance] == GPS_TYPE_GSOF)) {
hal.console->print(" GSOF ");
new_gps = new AP_GPS_GSOF(*this, state[instance], _port[instance]);
}
// record the time when we started detection. This is used to try
// to avoid initialising a uBlox as a NMEA GPS
if (dstate->detect_started_ms == 0) {
dstate->detect_started_ms = now;
}
if (now - dstate->last_baud_change_ms > GPS_BAUD_TIME_MS) {
// try the next baud rate
dstate->last_baud++;
if (dstate->last_baud == ARRAY_SIZE(_baudrates)) {
dstate->last_baud = 0;
}
uint32_t baudrate = pgm_read_dword(&_baudrates[dstate->last_baud]);
_port[instance]->begin(baudrate);
_port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
dstate->last_baud_change_ms = now;
#if UBLOX_RXM_RAW_LOGGING
if(_raw_data != 0)
send_blob_start(instance, _initialisation_raw_blob, sizeof(_initialisation_raw_blob));
else
#endif
send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
}
send_blob_update(instance);
while (initblob_state[instance].remaining == 0 && _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
window for 5Hz fixes. The NMEA startup message should force
the uBlox into 38400 no matter what rate it is configured
for.
*/
if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_UBLOX) &&
pgm_read_dword(&_baudrates[dstate->last_baud]) >= 38400 &&
AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) {
hal.console->print(" ublox ");
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(" MTK19 ");
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(" MTK ");
new_gps = new AP_GPS_MTK(*this, state[instance], _port[instance]);
}
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) &&
AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) {
hal.console->print(" SBP ");
new_gps = new AP_GPS_SBP(*this, state[instance], _port[instance]);
}
// save a bit of code space on a 1280
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) &&
AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) {
hal.console->print(" SIRF ");
new_gps = new AP_GPS_SIRF(*this, state[instance], _port[instance]);
}
else if (now - dstate->detect_started_ms > (ARRAY_SIZE(_baudrates) * GPS_BAUD_TIME_MS)) {
// prevent false detection of NMEA mode in
// a MTK or UBLOX which has booted in NMEA mode
if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_NMEA) &&
AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) {
hal.console->print(" NMEA ");
new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]);
}
}
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_QURT
found_gps:
#endif
if (new_gps != NULL) {
state[instance].status = NO_FIX;
drivers[instance] = new_gps;
timing[instance].last_message_time_ms = now;
}
}
/*
run detection step for one GPS instance. If this finds a GPS then it
will fill in drivers[instance] and change state[instance].status
from NO_GPS to NO_FIX.
*/
void
AP_GPS::detect_instance(uint8_t instance)
{
AP_GPS_Backend *new_gps = NULL;
struct detect_state *dstate = &detect_state[instance];
uint32_t now = hal.scheduler->millis();
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
if (_type[instance] == GPS_TYPE_PX4) {
// 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[instance]);
goto found_gps;
}
#endif
if (_port[instance] == NULL) {
// UART not available
return;
}
state[instance].instance = instance;
state[instance].status = NO_GPS;
// record the time when we started detection. This is used to try
// to avoid initialising a uBlox as a NMEA GPS
if (dstate->detect_started_ms == 0) {
dstate->detect_started_ms = now;
}
if (now - dstate->last_baud_change_ms > 1200) {
// try the next baud rate
dstate->last_baud++;
if (dstate->last_baud == sizeof(_baudrates) / sizeof(_baudrates[0])) {
dstate->last_baud = 0;
}
uint32_t baudrate = pgm_read_dword(&_baudrates[dstate->last_baud]);
_port[instance]->begin(baudrate);
_port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
dstate->last_baud_change_ms = now;
send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
}
send_blob_update(instance);
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
window for 5Hz fixes. The NMEA startup message should force
the uBlox into 38400 no matter what rate it is configured
for.
*/
if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_UBLOX) &&
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[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[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[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[instance]);
}
#endif // HAL_CPU_CLASS
#if !defined(GPS_SKIP_SIRF_NMEA)
// save a bit of code space on a 1280
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[instance]);
}
else if (now - dstate->detect_started_ms > 5000) {
// prevent false detection of NMEA mode in
// a MTK or UBLOX which has booted in NMEA mode
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[instance]);
}
}
#endif
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
found_gps:
#endif
if (new_gps != NULL) {
state[instance].status = NO_FIX;
drivers[instance] = new_gps;
timing[instance].last_message_time_ms = now;
}
}
