C++ comm_get_txspace函数代码示例

/*
  send RC_CHANNELS_RAW, and RC_CHANNELS messages
 */
void GCS_MAVLINK::send_radio_in(uint8_t receiver_rssi)
{
    uint32_t now = hal.scheduler->millis();

    uint16_t values[8];
    memset(values, 0, sizeof(values));
    hal.rcin->read(values, 8);

    mavlink_msg_rc_channels_raw_send(
        chan,
        now,
        0, // port
        values[0],
        values[1],
        values[2],
        values[3],
        values[4],
        values[5],
        values[6],
        values[7],
        receiver_rssi);

#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
    if (hal.rcin->num_channels() > 8 && 
        comm_get_txspace(chan) >= MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
        mavlink_msg_rc_channels_send(
            chan,
            now,
            hal.rcin->num_channels(),
            hal.rcin->read(CH_1),
            hal.rcin->read(CH_2),
            hal.rcin->read(CH_3),
            hal.rcin->read(CH_4),
            hal.rcin->read(CH_5),
            hal.rcin->read(CH_6),
            hal.rcin->read(CH_7),
            hal.rcin->read(CH_8),
            hal.rcin->read(CH_9),
            hal.rcin->read(CH_10),
            hal.rcin->read(CH_11),
            hal.rcin->read(CH_12),
            hal.rcin->read(CH_13),
            hal.rcin->read(CH_14),
            hal.rcin->read(CH_15),
            hal.rcin->read(CH_16),
            hal.rcin->read(CH_17),
            hal.rcin->read(CH_18),
            receiver_rssi);        
    }
#endif
}

void GCS_MAVLINK::send_home(const Location &home) const
{
    if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_HOME_POSITION_LEN) {
        const float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
        mavlink_msg_home_position_send(
            chan,
            home.lat,
            home.lng,
            home.alt / 100,
            0.0f, 0.0f, 0.0f,
            q,
            0.0f, 0.0f, 0.0f);
    }
}

bool try_send_statustext(mavlink_channel_t chan, const char *text, int len) {

    int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
    CHECK_PAYLOAD_SIZE(STATUSTEXT);

    char statustext[50] = { 0 };
    if (len < 50) {
        memcpy(statustext, text, len);
    }
    mavlink_msg_statustext_send(
            chan,
            1, /* SEVERITY_LOW */
            statustext);
    return true;
}

/*
  send a statustext message to all active MAVLink connections
 */
void GCS_MAVLINK::send_statustext_all(const prog_char_t *msg)
{
    for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
        if ((1U<<i) & mavlink_active) {
            mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
            if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) {
                char msg2[50];
                strncpy_P(msg2, msg, sizeof(msg2));
                mavlink_msg_statustext_send(chan,
                                            SEVERITY_HIGH,
                                            msg2);
            }
        }
    }
}

void
GCS_MAVLINK::send_text(gcs_severity severity, const char *str)
{
    if (severity != SEVERITY_LOW && 
        comm_get_txspace(chan) >= 
        MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_STATUSTEXT_LEN) {
        // send immediately
        mavlink_msg_statustext_send(chan, severity, str);
    } else {
        // send via the deferred queuing system
        mavlink_statustext_t *s = &pending_status;
        s->severity = (uint8_t)severity;
        strncpy((char *)s->text, str, sizeof(s->text));
        send_message(MSG_STATUSTEXT);
    }
}

void AP_InertialSensor_UserInteract_MAVLink::_printf_P(const prog_char* fmt, ...) 
{
    char msg[50];
    va_list ap;
    va_start(ap, fmt);
    hal.util->vsnprintf_P(msg, sizeof(msg), (const prog_char_t *)fmt, ap);
    va_end(ap);
    if (msg[strlen(msg)-1] == '\n') {
        // STATUSTEXT messages should not add linefeed
        msg[strlen(msg)-1] = 0;
    }
    while (comm_get_txspace(_chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES < sizeof(mavlink_statustext_t)) {
        hal.scheduler->delay(1);        
    }
    mavlink_msg_statustext_send(_chan, SEVERITY_USER_RESPONSE, msg);
}

/*
  send a parameter value message to all active MAVLink connections
 */
void GCS_MAVLINK::send_parameter_value_all(const char *param_name, ap_var_type param_type, float param_value)
{
    for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
        if ((1U<<i) & mavlink_active) {
            mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
            if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_PARAM_VALUE_LEN) {
                mavlink_msg_param_value_send(
                    chan,
                    param_name,
                    param_value,
                    mav_var_type(param_type),
                    AP_Param::count_parameters(),
                    -1);
            }
        }
    }
}

/*
  special handling for heartbeat messages. To ensure routing
  propagation heartbeat messages need to be forwarded on all channels
  except channels where the sysid/compid of the heartbeat could come from
*/
void MAVLink_routing::handle_heartbeat(mavlink_channel_t in_channel, const mavlink_message_t* msg)
{
    if (msg->compid == MAV_COMP_ID_GIMBAL) {
        return;
    }

    uint16_t mask = GCS_MAVLINK::active_channel_mask();
    
    // don't send on the incoming channel. This should only matter if
    // the routing table is full
    mask &= ~(1U<<(in_channel-MAVLINK_COMM_0));
    
    // mask out channels that do not want the heartbeat to be forwarded
    mask &= ~no_route_mask;
    
    // mask out channels that are known sources for this sysid/compid
    for (uint8_t i=0; i<num_routes; i++) {
        if (routes[i].sysid == msg->sysid && routes[i].compid == msg->compid) {
            mask &= ~(1U<<((unsigned)(routes[i].channel-MAVLINK_COMM_0)));
        }
    }

    if (mask == 0) {
        // nothing to send to
        return;
    }

    // send on the remaining channels
    for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
        if (mask & (1U<<i)) {
            mavlink_channel_t channel = (mavlink_channel_t)(MAVLINK_COMM_0 + i);
            if (comm_get_txspace(channel) >= ((uint16_t)msg->len) +
                GCS_MAVLINK::packet_overhead_chan(channel)) {
#if ROUTING_DEBUG
                ::printf("fwd HB from chan %u on chan %u from sysid=%u compid=%u\n",
                         (unsigned)in_channel,
                         (unsigned)channel,
                         (unsigned)msg->sysid,
                         (unsigned)msg->compid);
#endif
                _mavlink_resend_uart(channel, msg);
            }
        }
    }
}

/*
  send a statustext message to all active MAVLink connections
 */
void GCS_MAVLINK::send_statustext_all(MAV_SEVERITY severity, const char *fmt, ...)
{
    for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
        if ((1U<<i) & mavlink_active) {
            mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
            if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) {
                char msg2[50] {};
                va_list arg_list;
                va_start(arg_list, fmt);
                hal.util->vsnprintf((char *)msg2, sizeof(msg2), fmt, arg_list);
                va_end(arg_list);
                mavlink_msg_statustext_send(chan,
                                            severity,
                                            msg2);
            }
        }
    }
}

/*
    send a statustext message to specific MAVLink connections in a bitmask
 */
void GCS_MAVLINK::service_statustext(void)
{
    // create bitmask of what mavlink ports we should send this text to.
    // note, if sending to all ports, we only need to store the bitmask for each and the string only once.
    // once we send over a link, clear the port but other busy ports bit may stay allowing for faster links
    // to clear the bit and send quickly but slower links to still store the string. Regardless of mixed
    // bitrates of ports, a maximum of GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY strings can be buffered. Downside
    // is if you have a super slow link mixed with a faster port, if there are GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY
    // strings in the slow queue then the next item can not be queued for the faster link

    if (_statustext_queue.empty()) {
        // nothing to do
        return;
    }

    for (uint8_t idx=0; idx<GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY; ) {
        statustext_t *statustext = _statustext_queue[idx];
        if (statustext == nullptr) {
            break;
        }

        // try and send to all active mavlink ports listed in the statustext.bitmask
        for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
            uint8_t chan_bit = (1U<<i);
            // logical AND (&) to mask them together
            if (statustext->bitmask & chan_bit) {
                // something is queued on a port and that's the port index we're looped at
                mavlink_channel_t chan_index = (mavlink_channel_t)(MAVLINK_COMM_0+i);
                if (comm_get_txspace(chan_index) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) {
                    // we have space so send then clear that channel bit on the mask
                    mavlink_msg_statustext_send(chan_index, statustext->msg.severity, statustext->msg.text);
                    statustext->bitmask &= ~chan_bit;
                }
            }
        }

        if (statustext->bitmask == 0) {
            _statustext_queue.remove(idx);
        } else {
            // move to next index
            idx++;
        }
    }
}

void GCS_MAVLINK::send_home_all(const Location &home)
{
    const float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
    for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
        if ((1U<<i) & mavlink_active) {
            mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
            if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_HOME_POSITION_LEN) {
                mavlink_msg_home_position_send(
                    chan,
                    home.lat,
                    home.lng,
                    home.alt / 100,
                    0.0f, 0.0f, 0.0f,
                    q,
                    0.0f, 0.0f, 0.0f);
            }
        }
    }
}

// locks onto a particular target sysid and sets it's position data stream to at least 1hz
void Tracker::mavlink_check_target(const mavlink_message_t* msg)
{
    // exit immediately if the target has already been set
    if (target_set) {
        return;
    }

    // decode
    mavlink_heartbeat_t packet;
    mavlink_msg_heartbeat_decode(msg, &packet);

    // exit immediately if this is not a vehicle we would track
    if ((packet.type == MAV_TYPE_ANTENNA_TRACKER) ||
        (packet.type == MAV_TYPE_GCS) ||
        (packet.type == MAV_TYPE_ONBOARD_CONTROLLER) ||
        (packet.type == MAV_TYPE_GIMBAL)) {
        return;
    }

    // set our sysid to the target, this ensures we lock onto a single vehicle
    if (g.sysid_target == 0) {
        g.sysid_target = msg->sysid;
    }

    // send data stream request to target on all channels
    //  Note: this doesn't check success for all sends meaning it's not guaranteed the vehicle's positions will be sent at 1hz
    for (uint8_t i=0; i < num_gcs; i++) {
        if (gcs[i].initialised) {
            if (comm_get_txspace((mavlink_channel_t)i) - MAVLINK_NUM_NON_PAYLOAD_BYTES >= MAVLINK_MSG_ID_DATA_STREAM_LEN) {
                mavlink_msg_request_data_stream_send(
                    (mavlink_channel_t)i,
                    msg->sysid,
                    msg->compid,
                    MAV_DATA_STREAM_POSITION,
                    1,  // 1hz
                    1); // start streaming
            }
        }
    }

    // flag target has been set
    target_set = true;
}

/**
   trigger sending of log data if there are some pending
 */
bool GCS_MAVLINK::handle_log_send_data(DataFlash_Class &dataflash)
{
    uint16_t txspace = comm_get_txspace(chan);
    if (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_LOG_DATA_LEN) {
        // no space
        return false;
    }
    if (hal.scheduler->millis() - last_heartbeat_time > 3000) {
        // give a heartbeat a chance
        return false;
    }

    int16_t ret = 0;
    uint32_t len = _log_data_remaining;
	mavlink_log_data_t packet;

    if (len > 90) {
        len = 90;
    }
    ret = dataflash.get_log_data(_log_num_data, _log_data_page, _log_data_offset, len, packet.data);
    if (ret < 0) {
        // report as EOF on error
        ret = 0;
    }
    if (ret < 90) {
        memset(&packet.data[ret], 0, 90-ret);
    }

    packet.ofs = _log_data_offset;
    packet.id = _log_num_data;
    packet.count = ret;
    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)&packet, 
                                    MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC);

    _log_data_offset += len;
    _log_data_remaining -= len;
    if (ret < 90 || _log_data_remaining == 0) {
        _log_sending = false;
    }
    return true;
}

/**
   trigger sending of log messages if there are some pending
 */
void GCS_MAVLINK::handle_log_send_listing(DataFlash_Class &dataflash)
{
    int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
    if (payload_space < MAVLINK_MSG_ID_LOG_ENTRY_LEN) {
        // no space
        return;
    }
    if (hal.scheduler->millis() - last_heartbeat_time > 3000) {
        // give a heartbeat a chance
        return;
    }

    uint32_t size, time_utc;
    dataflash.get_log_info(_log_next_list_entry, size, time_utc);
    mavlink_msg_log_entry_send(chan, _log_next_list_entry, _log_num_logs, _log_last_list_entry, time_utc, size);
    if (_log_next_list_entry == _log_last_list_entry) {
        _log_listing = false;
    } else {
        _log_next_list_entry++;
    }
}

/*
  send a MAVLink message to all components with this vehicle's system id

  This is a no-op if no routes to components have been learned
*/
void MAVLink_routing::send_to_components(const mavlink_message_t* msg)
{
    bool sent_to_chan[MAVLINK_COMM_NUM_BUFFERS];
    memset(sent_to_chan, 0, sizeof(sent_to_chan));

    // check learned routes
    for (uint8_t i=0; i<num_routes; i++) {
        if ((routes[i].sysid == mavlink_system.sysid) && !sent_to_chan[routes[i].channel]) {
            if (comm_get_txspace(routes[i].channel) >= ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
#if ROUTING_DEBUG
                ::printf("send msg %u on chan %u sysid=%u compid=%u\n",
                         msg->msgid,
                         (unsigned)routes[i].channel,
                         (unsigned)routes[i].sysid,
                         (unsigned)routes[i].compid);
#endif
                _mavlink_resend_uart(routes[i].channel, msg);
                sent_to_chan[routes[i].channel] = true;
            }
        }
    }
}