/*
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;
mission_ack:
// we are rejecting the mission/waypoint
mavlink_msg_mission_ack_send_buf(
msg,
chan,
msg->sysid,
msg->compid,
result);
}
