/*
  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;
            }
        }
    }
}

// try to send a message, return false if it won't fit in the serial tx buffer
bool GCS_MAVLINK::try_send_message(enum ap_message id)
{
    uint16_t txspace = comm_get_txspace(chan);
    switch (id) {
    case MSG_HEARTBEAT:
        CHECK_PAYLOAD_SIZE(HEARTBEAT);
        tracker.send_heartbeat(chan);
        return true;

    case MSG_ATTITUDE:
        CHECK_PAYLOAD_SIZE(ATTITUDE);
        tracker.send_attitude(chan);
        break;

    case MSG_LOCATION:
        CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
        tracker.send_location(chan);
        break;

    case MSG_LOCAL_POSITION:
        CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED);
        send_local_position(tracker.ahrs);
        break;

    case MSG_NAV_CONTROLLER_OUTPUT:
        CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
        tracker.send_nav_controller_output(chan);
        break;

    case MSG_GPS_RAW:
        CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
        tracker.gcs[chan-MAVLINK_COMM_0].send_gps_raw(tracker.gps);
        break;

    case MSG_RADIO_IN:
        CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
        tracker.gcs[chan-MAVLINK_COMM_0].send_radio_in(0);
        break;

    case MSG_RADIO_OUT:
        CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
        tracker.send_radio_out(chan);
        break;

    case MSG_RAW_IMU1:
        CHECK_PAYLOAD_SIZE(RAW_IMU);
        tracker.gcs[chan-MAVLINK_COMM_0].send_raw_imu(tracker.ins, tracker.compass);
        break;

    case MSG_RAW_IMU2:
        CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
        tracker.gcs[chan-MAVLINK_COMM_0].send_scaled_pressure(tracker.barometer);
        break;

    case MSG_RAW_IMU3:
        CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
        tracker.gcs[chan-MAVLINK_COMM_0].send_sensor_offsets(tracker.ins, tracker.compass, tracker.barometer);
        break;

    case MSG_NEXT_PARAM:
        CHECK_PAYLOAD_SIZE(PARAM_VALUE);
        tracker.gcs[chan-MAVLINK_COMM_0].queued_param_send();
        break;

    case MSG_NEXT_WAYPOINT:
        CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
        tracker.send_waypoint_request(chan);
        break;

    case MSG_STATUSTEXT:
        CHECK_PAYLOAD_SIZE(STATUSTEXT);
        tracker.send_statustext(chan);
        break;

    case MSG_AHRS:
        CHECK_PAYLOAD_SIZE(AHRS);
        tracker.gcs[chan-MAVLINK_COMM_0].send_ahrs(tracker.ahrs);
        break;

    case MSG_SIMSTATE:
        CHECK_PAYLOAD_SIZE(SIMSTATE);
        tracker.send_simstate(chan);
        break;

    case MSG_HWSTATUS:
        CHECK_PAYLOAD_SIZE(HWSTATUS);
        tracker.send_hwstatus(chan);
        break;

    case MSG_SERVO_OUT:
    case MSG_EXTENDED_STATUS1:
    case MSG_EXTENDED_STATUS2:
    case MSG_RETRY_DEFERRED:
    case MSG_CURRENT_WAYPOINT:
    case MSG_VFR_HUD:
    case MSG_SYSTEM_TIME:
    case MSG_LIMITS_STATUS:
    case MSG_FENCE_STATUS:
    case MSG_WIND:
//.........这里部分代码省略.........

/*
  handle an incoming mission item
 */
void GCS_MAVLINK::handle_mission_item(mavlink_message_t *msg, AP_Mission &mission)
{
    mavlink_mission_item_t packet;
    uint8_t result = MAV_MISSION_ACCEPTED;
    struct AP_Mission::Mission_Command cmd = {};

    mavlink_msg_mission_item_decode(msg, &packet);
    if (mavlink_check_target(packet.target_system,packet.target_component)) {
        return;
    }

    // convert mavlink packet to mission command
    if (!AP_Mission::mavlink_to_mission_cmd(packet, cmd)) {
        result = MAV_MISSION_INVALID;
        goto mission_ack;
    }

    if (packet.current == 2) {                                               
        // current = 2 is a flag to tell us this is a "guided mode"
        // waypoint and not for the mission
        handle_guided_request(cmd);

        // verify we received the command
        result = 0;
        goto mission_ack;
    }

    if (packet.current == 3) {
        //current = 3 is a flag to tell us this is a alt change only
        // add home alt if needed
        handle_change_alt_request(cmd);

        // verify we recevied the command
        result = 0;
        goto mission_ack;
    }

    // Check if receiving waypoints (mission upload expected)
    if (!waypoint_receiving) {
        result = MAV_MISSION_ERROR;
        goto mission_ack;
    }

    // check if this is the requested waypoint
    if (packet.seq != waypoint_request_i) {
        result = MAV_MISSION_INVALID_SEQUENCE;
        goto mission_ack;
    }
    
    // if command index is within the existing list, replace the command
    if (packet.seq < mission.num_commands()) {
        if (mission.replace_cmd(packet.seq,cmd)) {
            result = MAV_MISSION_ACCEPTED;
        }else{
            result = MAV_MISSION_ERROR;
            goto mission_ack;
        }
        // if command is at the end of command list, add the command
    } else if (packet.seq == mission.num_commands()) {
        if (mission.add_cmd(cmd)) {
            result = MAV_MISSION_ACCEPTED;
        }else{
            result = MAV_MISSION_ERROR;
            goto mission_ack;
        }
        // if beyond the end of the command list, return an error
    } else {
        result = MAV_MISSION_ERROR;
        goto mission_ack;
    }
    
    // update waypoint receiving state machine
    waypoint_timelast_receive = hal.scheduler->millis();
    waypoint_request_i++;
    
    if (waypoint_request_i >= waypoint_request_last) {
        mavlink_msg_mission_ack_send_buf(
            msg,
            chan,
            msg->sysid,
            msg->compid,
            MAV_MISSION_ACCEPTED);
        
        send_text_P(SEVERITY_LOW,PSTR("flight plan received"));
        waypoint_receiving = false;
        // XXX ignores waypoint radius for individual waypoints, can
        // only set WP_RADIUS parameter
    } else {
        waypoint_timelast_request = hal.scheduler->millis();
        // if we have enough space, then send the next WP immediately
        if (comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES >= MAVLINK_MSG_ID_MISSION_ITEM_LEN) {
            queued_waypoint_send();
        } else {
            send_message(MSG_NEXT_WAYPOINT);
        }
    }
    return;
//.........这里部分代码省略.........

void
GCS_MAVLINK::send_text(MAV_SEVERITY severity, const char *str)
{
    if (severity < MAV_SEVERITY_WARNING &&
            comm_get_txspace(chan) >=
            MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_STATUSTEXT_LEN) {
        // send immediately
        char msg[50] {};
        strncpy(msg, str, sizeof(msg));
        mavlink_msg_statustext_send(chan, severity, msg);
    } 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);
    }

#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN && APM_BUILD_TYPE(APM_BUILD_ArduCopter)
    if (!strcmp(str, "compass disabled"))                       // Messages defined in compassmot.pde (10x)
        textId = 1;
    else if (!strcmp(str, "check compass"))
        textId = 2;
    else if (!strcmp(str, "RC not calibrated"))
        textId = 3;
    else if (!strcmp(str, "thr not zero"))
        textId = 4;
    else if (!strcmp(str, "Not landed"))
        textId = 5;
    else if (!strcmp(str, "STARTING CALIBRATION"))
        textId = 6;
    else if (!strcmp(str, "CURRENT"))
        textId = 7;
    else if (!strcmp(str, "THROTTLE"))
        textId = 8;
    else if (!strcmp(str, "Calibration Successful"))
        textId = 9;
    else if (!strcmp(str, "Failed"))
        textId = 10;
    else if (!strcmp(str, "AutoTune: Started"))                 // Messages defined in control_autotune.pde (4x)
        textId = 21;
    else if (!strcmp(str, "AutoTune: Stopped"))
        textId = 22;
    else if (!strcmp(str, "AutoTune: Success"))
        textId = 23;
    else if (!strcmp(str, "AutoTune: Failed"))
        textId = 24;
    else if (!strcmp(str, "Crash: Disarming"))                  // Messages defined in crash_check.pde (3x)
        textId = 35;
    else if (!strcmp(str, "Parachute: Released"))
        textId = 36;
    else if (!strcmp(str, "Parachute: Too Low"))
        textId = 37;
    else if (!strcmp(str, "EKF variance"))                      // Messages defined in ekf_check.pde (2x)
        textId = 48;
    else if (!strcmp(str, "DCM bad heading"))
        textId = 49;
    else if (!strcmp(str, "Low Battery"))                       // Messages defined in events.pde (2x)
        textId = 60;
    else if (!strcmp(str, "Lost GPS"))
        textId = 61;
    else if (!strcmp(str, "bad rally point message ID"))        // Messages defined in GCS_Mavlink.pde (6x)
        textId = 72;
    else if (!strcmp(str, "bad rally point message count"))
        textId = 73;
    else if (!strcmp(str, "error setting rally point"))
        textId = 74;
    else if (!strcmp(str, "bad rally point index"))
        textId = 75;
    else if (!strcmp(str, "failed to set rally point"))
        textId = 76;
    else if (!strcmp(str, "Initialising APM..."))
        textId = 77;
    else if (!strcmp(str, "Erasing logs"))                      // Messages defined in Log.pde (2x)
        textId = 88;
    else if (!strcmp(str, "Log erase complete"))
        textId = 89;
    else if (!strcmp(str, "ARMING MOTORS"))                     // Messages defined in motors.pde  (30x)
        textId = 100;
    else if (!strcmp(str, "Arm: Gyro cal failed"))
        textId = 101;
    else if (!strcmp(str, "PreArm: RC not calibrated"))
        textId = 102;
    else if (!strcmp(str, "PreArm: Baro not healthy"))
        textId = 103;
    else if (!strcmp(str, "PreArm: Alt disparity"))
        textId = 104;
    else if (!strcmp(str, "PreArm: Compass not healthy"))
        textId = 105;
    else if (!strcmp(str, "PreArm: Compass not calibrated"))
        textId = 106;
    else if (!strcmp(str, "PreArm: Compass offsets too high"))
        textId = 107;
    else if (!strcmp(str, "PreArm: Check mag field"))
        textId = 108;
    else if (!strcmp(str, "PreArm: compasses inconsistent"))
        textId = 109;
    else if (!strcmp(str, "PreArm: INS not calibrated"))
        textId = 110;
    else if (!strcmp(str, "PreArm: Accels not healthy"))
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    2b) the message has a target_system of zero

    2c) the message does not have the flight controllers target_system
        and the flight controller has seen a message from the messages
        target_system on the link

    2d) the message has the flight controllers target_system and has a
        target_component field and the flight controllers has seen a
        message from the target_system/target_component combination on
        the link

Note: This proposal assumes that ground stations will not send command
packets to a non-broadcast destination (sysid/compid combination)
until they have received at least one package from that destination
over the link. This is essential to prevent a flight controller from
acting on a message that is not meant for it. For example, a PARAM_SET
cannot be sent to a specific sysid/compid combination until the GCS
has seen a packet from that sysid/compid combination on the link. 

The GCS must also reset what sysid/compid combinations it has seen on
a link when it sees a SYSTEM_TIME message with a decrease in
time_boot_ms from a particular sysid/compid. That is essential to
detect a reset of the flight controller, which implies a reset of its
routing table.

*/
bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavlink_message_t* msg)
{
    // handle the case of loopback of our own messages, due to
    // incorrect serial configuration.
    if (msg->sysid == mavlink_system.sysid && 
        msg->compid == mavlink_system.compid) {
        return true;
    }

    // learn new routes
    learn_route(in_channel, msg);

    if (msg->msgid == MAVLINK_MSG_ID_RADIO ||
        msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
        // don't forward RADIO packets
        return true;
    }
    
    if (msg->msgid == MAVLINK_MSG_ID_HEARTBEAT) {
        // heartbeat needs special handling
        handle_heartbeat(in_channel, msg);
        return true;
    }

    // extract the targets for this packet
    int16_t target_system = -1;
    int16_t target_component = -1;
    get_targets(msg, target_system, target_component);

    bool broadcast_system = (target_system == 0 || target_system == -1);
    bool broadcast_component = (target_component == 0 || target_component == -1);
    bool match_system = broadcast_system || (target_system == mavlink_system.sysid);
    bool match_component = match_system && (broadcast_component || 
                                            (target_component == mavlink_system.compid));
    bool process_locally = match_system && match_component;

    if (process_locally && !broadcast_system && !broadcast_component) {
        // nothing more to do - it can only be for us
        return true;
    }

    // forward on any channels matching the targets
    bool forwarded = false;
    bool sent_to_chan[MAVLINK_COMM_NUM_BUFFERS];
    memset(sent_to_chan, 0, sizeof(sent_to_chan));
    for (uint8_t i=0; i<num_routes; i++) {
        if (broadcast_system || (target_system == routes[i].sysid &&
                                 (broadcast_component || 
                                  target_component == routes[i].compid ||
                                  !match_system))) {
            if (in_channel != routes[i].channel && !sent_to_chan[routes[i].channel]) {
                if (comm_get_txspace(routes[i].channel) >= ((uint16_t)msg->len) +
                    GCS_MAVLINK::packet_overhead_chan(routes[i].channel)) {
#if ROUTING_DEBUG
                    ::printf("fwd msg %u from chan %u on chan %u sysid=%d compid=%d\n",
                             msg->msgid,
                             (unsigned)in_channel,
                             (unsigned)routes[i].channel,
                             (int)target_system,
                             (int)target_component);
#endif
                    _mavlink_resend_uart(routes[i].channel, msg);
                }
                sent_to_chan[routes[i].channel] = true;
                forwarded = true;
            }
        }
    }
    if (!forwarded && match_system) {
        process_locally = true;
    }

    return process_locally;
}

// try to send a message, return false if it won't fit in the serial tx buffer
bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
{
    if (telemetry_delayed()) {
        return false;
    }

    // if we don't have at least 0.2ms remaining before the main loop
    // wants to fire then don't send a mavlink message. We want to
    // prioritise the main flight control loop over communications
    if (!hal.scheduler->in_delay_callback() &&
        rover.scheduler.time_available_usec() < 200) {
        gcs().set_out_of_time(true);
        return false;
    }

    switch (id) {

    case MSG_EXTENDED_STATUS1:
        // send extended status only once vehicle has been initialised
        // to avoid unnecessary errors being reported to user
        if (initialised) {
            if (PAYLOAD_SIZE(chan, SYS_STATUS) +
                PAYLOAD_SIZE(chan, POWER_STATUS) > comm_get_txspace(chan)) {
                return false;
            }
            rover.send_sys_status(chan);
            send_power_status();
        }
        break;

    case MSG_NAV_CONTROLLER_OUTPUT:
        CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
        rover.send_nav_controller_output(chan);
        break;

    case MSG_SERVO_OUT:
        CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
        rover.send_servo_out(chan);
        break;

    case MSG_RANGEFINDER:
        CHECK_PAYLOAD_SIZE(RANGEFINDER);
        rover.send_rangefinder(chan);
        send_distance_sensor();
        send_proximity();
        break;

    case MSG_RPM:
        CHECK_PAYLOAD_SIZE(RPM);
        rover.send_wheel_encoder(chan);
        break;

    case MSG_FENCE_STATUS:
        CHECK_PAYLOAD_SIZE(FENCE_STATUS);
        rover.send_fence_status(chan);
        break;

    case MSG_WIND:
        CHECK_PAYLOAD_SIZE(WIND);
        rover.g2.windvane.send_wind(chan);
        break;

    case MSG_PID_TUNING:
        CHECK_PAYLOAD_SIZE(PID_TUNING);
        rover.send_pid_tuning(chan);
        break;

    default:
        return GCS_MAVLINK::try_send_message(id);
    }
    return true;
}