/*
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;
}
}
/**
handle a SERIAL_CONTROL message
*/
void GCS_MAVLINK::handle_serial_control(mavlink_message_t *msg, AP_GPS &gps)
{
mavlink_serial_control_t packet;
mavlink_msg_serial_control_decode(msg, &packet);
AP_HAL::UARTDriver *port = NULL;
if (packet.flags & SERIAL_CONTROL_FLAG_REPLY) {
// how did this packet get to us?
return;
}
bool exclusive = (packet.flags & SERIAL_CONTROL_FLAG_EXCLUSIVE) != 0;
switch (packet.device) {
case SERIAL_CONTROL_DEV_TELEM1:
port = hal.uartC;
lock_channel(MAVLINK_COMM_1, exclusive);
break;
case SERIAL_CONTROL_DEV_TELEM2:
port = hal.uartD;
lock_channel(MAVLINK_COMM_2, exclusive);
break;
case SERIAL_CONTROL_DEV_GPS1:
port = hal.uartB;
gps.lock_port(0, exclusive);
break;
case SERIAL_CONTROL_DEV_GPS2:
port = hal.uartE;
gps.lock_port(1, exclusive);
break;
default:
// not supported yet
return;
}
if (exclusive) {
// force flow control off for exclusive access. This protocol
// is used to talk to bootloaders which may not have flow
// control support
port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
}
// optionally change the baudrate
if (packet.baudrate != 0) {
port->begin(packet.baudrate);
}
// write the data
if (packet.count != 0) {
if ((packet.flags & SERIAL_CONTROL_FLAG_BLOCKING) == 0) {
port->write(packet.data, packet.count);
} else {
const uint8_t *data = &packet.data[0];
uint8_t count = packet.count;
while (count > 0) {
while (port->txspace() <= 0) {
hal.scheduler->delay(5);
}
uint16_t n = port->txspace();
if (n > packet.count) {
n = packet.count;
}
port->write(data, n);
data += n;
count -= n;
}
}
}
if ((packet.flags & SERIAL_CONTROL_FLAG_RESPOND) == 0) {
// no response expected
return;
}
uint8_t flags = packet.flags;
more_data:
// sleep for the timeout
while (packet.timeout != 0 &&
port->available() < (int16_t)sizeof(packet.data)) {
hal.scheduler->delay(1);
packet.timeout--;
}
packet.flags = SERIAL_CONTROL_FLAG_REPLY;
// work out how many bytes are available
int16_t available = port->available();
if (available < 0) {
available = 0;
}
if (available > (int16_t)sizeof(packet.data)) {
available = sizeof(packet.data);
}
// read any reply data
packet.count = 0;
memset(packet.data, 0, sizeof(packet.data));
while (available > 0) {
packet.data[packet.count++] = (uint8_t)port->read();
available--;
}
// and send the reply
_mav_finalize_message_chan_send(chan,
MAVLINK_MSG_ID_SERIAL_CONTROL,
(const char *)&packet,
MAVLINK_MSG_ID_SERIAL_CONTROL_LEN,
MAVLINK_MSG_ID_SERIAL_CONTROL_CRC);
if ((flags & SERIAL_CONTROL_FLAG_MULTI) && packet.count != 0) {
hal.scheduler->delay(1);
goto more_data;
}
}