自学教程：C++ AP_Mission类代码示例

/*  handle a MISSION_COUNT mavlink packet */void GCS_MAVLINK::handle_mission_count(AP_Mission &mission, mavlink_message_t *msg){    // decode    mavlink_mission_count_t packet;    mavlink_msg_mission_count_decode(msg, &packet);    // exit immediately if this command is not meant for this vehicle    if (mavlink_check_target(packet.target_system,packet.target_component)) {        return;    }    // start waypoint receiving    if (packet.count > mission.num_commands_max()) {        // send NAK        mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_NO_SPACE);        return;    }    // new mission arriving, truncate mission to be the same length    mission.truncate(packet.count);    // set variables to help handle the expected receiving of commands from the GCS    waypoint_timelast_receive = hal.scheduler->millis();    // set time we last received commands to now    waypoint_receiving = true;              // record that we expect to receive commands    waypoint_request_i = 0;                 // reset the next expected command number to zero    waypoint_request_last = packet.count;   // record how many commands we expect to receive    waypoint_timelast_request = 0;          // set time we last requested commands to zero}

/*  handle a MISSION_WRITE_PARTIAL_LIST mavlink packet */void GCS_MAVLINK::handle_mission_write_partial_list(AP_Mission &mission, mavlink_message_t *msg){    // decode    mavlink_mission_write_partial_list_t packet;    mavlink_msg_mission_write_partial_list_decode(msg, &packet);    // exit immediately if this command is not meant for this vehicle    if (mavlink_check_target(packet.target_system,packet.target_component)) {        return;    }    // start waypoint receiving    if ((unsigned)packet.start_index > mission.num_commands() ||        (unsigned)packet.end_index > mission.num_commands() ||        packet.end_index < packet.start_index) {        send_text_P(SEVERITY_LOW,PSTR("flight plan update rejected"));        return;    }    waypoint_timelast_receive = hal.scheduler->millis();    waypoint_timelast_request = 0;    waypoint_receiving   = true;    waypoint_request_i   = packet.start_index;    waypoint_request_last= packet.end_index;}

bool AP_Arming::mission_checks(bool report){    if (((checks_to_perform & ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_MISSION)) &&        _required_mission_items) {        AP_Mission *mission = AP::mission();        if (mission == nullptr) {            check_failed(ARMING_CHECK_MISSION, report, "No mission library present");            #if CONFIG_HAL_BOARD == HAL_BOARD_SITL                AP_HAL::panic("Mission checks requested, but no mission was allocated");            #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL            return false;        }        AP_Rally *rally = AP::rally();        if (rally == nullptr) {            check_failed(ARMING_CHECK_MISSION, report, "No rally library present");            #if CONFIG_HAL_BOARD == HAL_BOARD_SITL                AP_HAL::panic("Mission checks requested, but no rally was allocated");            #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL            return false;        }        const struct MisItemTable {          MIS_ITEM_CHECK check;          MAV_CMD mis_item_type;          const char *type;        } misChecks[5] = {          {MIS_ITEM_CHECK_LAND,          MAV_CMD_NAV_LAND,           "land"},          {MIS_ITEM_CHECK_VTOL_LAND,     MAV_CMD_NAV_VTOL_LAND,      "vtol land"},          {MIS_ITEM_CHECK_DO_LAND_START, MAV_CMD_DO_LAND_START,      "do land start"},          {MIS_ITEM_CHECK_TAKEOFF,       MAV_CMD_NAV_TAKEOFF,        "takeoff"},          {MIS_ITEM_CHECK_VTOL_TAKEOFF,  MAV_CMD_NAV_VTOL_TAKEOFF,   "vtol takeoff"},        };        for (uint8_t i = 0; i < ARRAY_SIZE(misChecks); i++) {            if (_required_mission_items & misChecks[i].check) {                if (!mission->contains_item(misChecks[i].mis_item_type)) {                    check_failed(ARMING_CHECK_MISSION, report, "Missing mission item: %s", misChecks[i].type);                    return false;                }            }        }        if (_required_mission_items & MIS_ITEM_CHECK_RALLY) {            Location ahrs_loc;            if (!AP::ahrs().get_position(ahrs_loc)) {                check_failed(ARMING_CHECK_MISSION, report, "Can't check rally without position");                return false;            }            RallyLocation rally_loc = {};            if (!rally->find_nearest_rally_point(ahrs_loc, rally_loc)) {                check_failed(ARMING_CHECK_MISSION, report, "No sufficently close rally point located");                return false;            }          }    }    return true;}

void RC_Channel::do_aux_function_clear_wp(const aux_switch_pos_t ch_flag){    AP_Mission *mission = AP::mission();    if (mission == nullptr) {        return;    }    if (ch_flag == HIGH) {        mission->clear();    }}

/*  handle a MISSION_REQUEST mavlink packet */void GCS_MAVLINK::handle_mission_request(AP_Mission &mission, mavlink_message_t *msg){    AP_Mission::Mission_Command cmd;    // decode    mavlink_mission_request_t packet;    mavlink_msg_mission_request_decode(msg, &packet);    // exit immediately if this command is not meant for this vehicle    if (mavlink_check_target(packet.target_system, packet.target_component)) {        return;    }    // retrieve mission from eeprom    if (!mission.read_cmd_from_storage(packet.seq, cmd)) {        goto mission_item_send_failed;    }    // convert mission command to mavlink mission item packet    mavlink_mission_item_t ret_packet;    memset(&ret_packet, 0, sizeof(ret_packet));    if (!AP_Mission::mission_cmd_to_mavlink(cmd, ret_packet)) {        goto mission_item_send_failed;    }    // set packet's current field to 1 if this is the command being executed    if (cmd.id == (uint16_t)mission.get_current_nav_cmd().index) {        ret_packet.current = 1;    } else {        ret_packet.current = 0;    }    // set auto continue to 1    ret_packet.autocontinue = 1;     // 1 (true), 0 (false)    /*      avoid the _send() function to save memory on APM2, as it avoids      the stack usage of the _send() function by using the already      declared ret_packet above     */    ret_packet.target_system = msg->sysid;    ret_packet.target_component = msg->compid;    ret_packet.seq = packet.seq;    ret_packet.command = cmd.id;    _mav_finalize_message_chan_send(chan,                                     MAVLINK_MSG_ID_MISSION_ITEM,                                    (const char *)&ret_packet,                                    MAVLINK_MSG_ID_MISSION_ITEM_LEN,                                    MAVLINK_MSG_ID_MISSION_ITEM_CRC);    return;mission_item_send_failed:    // send failure message    mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ERROR);}

/*  handle a MISSION_SET_CURRENT mavlink packet */void GCS_MAVLINK::handle_mission_set_current(AP_Mission &mission, mavlink_message_t *msg){    // decode    mavlink_mission_set_current_t packet;    mavlink_msg_mission_set_current_decode(msg, &packet);    // set current command    if (mission.set_current_cmd(packet.seq)) {        mavlink_msg_mission_current_send(chan, mission.get_current_nav_cmd().index);    }}

/*  handle a MISSION_SET_CURRENT mavlink packet */void GCS_MAVLINK::handle_mission_set_current(AP_Mission &mission, mavlink_message_t *msg){    // decode    mavlink_mission_set_current_t packet;    mavlink_msg_mission_set_current_decode(msg, &packet);    // exit immediately if this command is not meant for this vehicle    if (mavlink_check_target(packet.target_system,packet.target_component)) {        return;    }    // set current command    if (mission.set_current_cmd(packet.seq)) {        mavlink_msg_mission_current_send(chan, mission.get_current_nav_cmd().index);    }}

/*  handle a MISSION_WRITE_PARTIAL_LIST mavlink packet */void GCS_MAVLINK::handle_mission_write_partial_list(AP_Mission &mission, mavlink_message_t *msg){    // decode    mavlink_mission_write_partial_list_t packet;    mavlink_msg_mission_write_partial_list_decode(msg, &packet);    // start waypoint receiving    if ((unsigned)packet.start_index > mission.num_commands() ||        (unsigned)packet.end_index > mission.num_commands() ||        packet.end_index < packet.start_index) {        send_text_P(SEVERITY_LOW,PSTR("flight plan update rejected"));        return;    }    waypoint_timelast_receive = hal.scheduler->millis();    waypoint_timelast_request = 0;    waypoint_receiving   = true;    waypoint_request_i   = packet.start_index;    waypoint_request_last= packet.end_index;}

/*  handle a MISSION_REQUEST_LIST mavlink packet */void GCS_MAVLINK::handle_mission_request_list(AP_Mission &mission, mavlink_message_t *msg){    // decode    mavlink_mission_request_list_t packet;    mavlink_msg_mission_request_list_decode(msg, &packet);    // reply with number of commands in the mission.  The GCS will then request each command separately    mavlink_msg_mission_count_send(chan,msg->sysid, msg->compid, mission.num_commands());    // set variables to help handle the expected sending of commands to the GCS    waypoint_receiving = false;             // record that we are sending commands (i.e. not receiving)    waypoint_dest_sysid = msg->sysid;       // record system id of GCS who has requested the commands    waypoint_dest_compid = msg->compid;     // record component id of GCS who has requested the commands}

/*  handle a MISSION_CLEAR_ALL mavlink packet */void GCS_MAVLINK::handle_mission_clear_all(AP_Mission &mission, mavlink_message_t *msg){    // decode    mavlink_mission_clear_all_t packet;    mavlink_msg_mission_clear_all_decode(msg, &packet);    // clear all waypoints    if (mission.clear()) {        // send ack        mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_RESULT_ACCEPTED);    }else{        // send nack        mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, 1);    }}

/*  handle a MISSION_CLEAR_ALL mavlink packet */void GCS_MAVLINK::handle_mission_clear_all(AP_Mission &mission, mavlink_message_t *msg){    // decode    mavlink_mission_clear_all_t packet;    mavlink_msg_mission_clear_all_decode(msg, &packet);    // exit immediately if this command is not meant for this vehicle    if (mavlink_check_target(packet.target_system, packet.target_component)) {        return;    }    // clear all waypoints    if (mission.clear()) {        // send ack        mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_RESULT_ACCEPTED);    }else{        // send nack        mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, 1);    }}

/*  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;//.........这里部分代码省略.........