Exemplo n.º 1
0
void AP_AccelCal::_printf(const char* fmt, ...)
{
    if (!_gcs) {
        return;
    }
    char msg[50];
    va_list ap;
    va_start(ap, fmt);
    hal.util->vsnprintf(msg, sizeof(msg), fmt, ap);
    va_end(ap);
    if (msg[strlen(msg)-1] == '\n') {
        // STATUSTEXT messages should not add linefeed
        msg[strlen(msg)-1] = 0;
    }
    AP_HAL::UARTDriver *uart = _gcs->get_uart();
    /*
     *     to ensure these messages get to the user we need to wait for the
     *     port send buffer to have enough room
     */
    while (uart->txspace() < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_STATUSTEXT_LEN) {
        hal.scheduler->delay(1);
    }

#if !APM_BUILD_TYPE(APM_BUILD_Replay)
    _gcs->send_text(MAV_SEVERITY_CRITICAL, msg);
#endif
}
Exemplo n.º 2
0
/*
  send some more initialisation string bytes if there is room in the
  UART transmit buffer
 */
void AP_GPS::send_blob_update(uint8_t instance)
{
    // 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();
        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));
            initblob_state[instance].blob++;
            space--;
            initblob_state[instance].remaining--;
        }
    }
}
void AP_InertialSensor_UserInteract_MAVLink::printf(const char* fmt, ...)
{
    char msg[50];
    va_list ap;
    va_start(ap, fmt);
    hal.util->vsnprintf(msg, sizeof(msg), fmt, ap);
    va_end(ap);
    if (msg[strlen(msg)-1] == '\n') {
        // STATUSTEXT messages should not add linefeed
        msg[strlen(msg)-1] = 0;
    }
    AP_HAL::UARTDriver *uart = _gcs->get_uart();
    /*
      to ensure these messages get to the user we need to wait for the
      port send buffer to have enough room
     */
    while (uart->txspace() < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_STATUSTEXT_LEN) {
        hal.scheduler->delay(1);
    }
    _gcs->send_text(MAV_SEVERITY_CRITICAL, msg);
}
/**
   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;
    }
}