/* setup a UART, handling begin() and init() */ void GCS_MAVLINK::setup_uart(const AP_SerialManager& serial_manager, AP_SerialManager::SerialProtocol protocol, uint8_t instance) { // search for serial port AP_HAL::UARTDriver *uart; uart = serial_manager.find_serial(protocol, instance); if (uart == NULL) { // return immediately if not found return; } // get associated mavlink channel mavlink_channel_t mav_chan; if (!serial_manager.get_mavlink_channel(protocol, instance, mav_chan)) { // return immediately in unlikely case mavlink channel cannot be found return; } /* Now try to cope with SiK radios that may be stuck in bootloader mode because CTS was held while powering on. This tells the bootloader to wait for a firmware. It affects any SiK radio with CTS connected that is externally powered. To cope we send 0x30 0x20 at 115200 on startup, which tells the bootloader to reset and boot normally */ uart->begin(115200); AP_HAL::UARTDriver::flow_control old_flow_control = uart->get_flow_control(); uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); for (uint8_t i=0; i<3; i++) { hal.scheduler->delay(1); uart->write(0x30); uart->write(0x20); } // since tcdrain() and TCSADRAIN may not be implemented... hal.scheduler->delay(1); uart->set_flow_control(old_flow_control); // now change back to desired baudrate uart->begin(serial_manager.find_baudrate(protocol, instance)); // and init the gcs instance init(uart, mav_chan); }
/** 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; } }