From df1b2e1192b9379b939629f99b4752e817751e85 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 10 Mar 2014 17:44:05 +0900 Subject: [PATCH] Rover: Mission integration with GCS_Mavlink.pde --- APMrover2/GCS_Mavlink.pde | 345 +++++++++++++------------------------- 1 file changed, 112 insertions(+), 233 deletions(-) diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index f60e8b41ad..b09bc393bd 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -481,9 +481,13 @@ static void NOINLINE send_rangefinder(mavlink_channel_t chan) static void NOINLINE send_current_waypoint(mavlink_channel_t chan) { - mavlink_msg_mission_current_send( - chan, - g.command_index); + uint16_t current_cmd_index; + if (mission.state() == AP_Mission::MISSION_RUNNING) { + current_cmd_index = mission.get_current_nav_cmd().index; + }else{ + current_cmd_index = AP_MISSION_CMD_INDEX_NONE; + } + mavlink_msg_mission_current_send(chan, current_cmd_index); } static void NOINLINE send_statustext(mavlink_channel_t chan) @@ -1045,7 +1049,7 @@ GCS_MAVLINK::send_text_P(gcs_severity severity, const prog_char_t *str) void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) { - struct Location tell_command = {}; // command for telemetry + struct AP_Mission::Mission_Command cmd = {}; // general purpose mission command switch (msg->msgid) { @@ -1248,7 +1252,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_mission_count_send( chan,msg->sysid, msg->compid, - g.command_total + 1); // + home + mission.num_commands()); waypoint_receiving = false; waypoint_dest_sysid = msg->sysid; @@ -1267,102 +1271,52 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (mavlink_check_target(packet.target_system, packet.target_component)) break; - // send waypoint - tell_command = get_cmd_with_index(packet.seq); - - // set frame of waypoint - uint8_t frame; - - if (tell_command.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT) { - frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame - } else { - frame = MAV_FRAME_GLOBAL; // reference frame - } - - float param1 = 0, param2 = 0 , param3 = 0, param4 = 0; - - // time that the mav should loiter in milliseconds - uint8_t current = 0; // 1 (true), 0 (false) - - if (packet.seq == (uint16_t)g.command_index) - current = 1; - - uint8_t autocontinue = 1; // 1 (true), 0 (false) - - float x = 0, y = 0, z = 0; - - if (tell_command.id < MAV_CMD_NAV_LAST || tell_command.id == MAV_CMD_CONDITION_CHANGE_ALT) { - // command needs scaling - x = tell_command.lat/1.0e7; // local (x), global (latitude) - y = tell_command.lng/1.0e7; // local (y), global (longitude) - z = tell_command.alt/1.0e2; - } - - switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields - - case MAV_CMD_NAV_TAKEOFF: - case MAV_CMD_DO_SET_HOME: - param1 = tell_command.p1; - break; - - case MAV_CMD_CONDITION_CHANGE_ALT: - x=0; // Clear fields loaded above that we don't want sent for this command - y=0; - case MAV_CMD_CONDITION_DELAY: - case MAV_CMD_CONDITION_DISTANCE: - param1 = tell_command.lat; - break; - - case MAV_CMD_DO_JUMP: - param2 = tell_command.lat; - param1 = tell_command.p1; - break; - - case MAV_CMD_DO_REPEAT_SERVO: - param4 = tell_command.lng*0.001f; // time - param3 = tell_command.lat; // repeat - param2 = tell_command.alt; // pwm - param1 = tell_command.p1; // channel - break; - - case MAV_CMD_DO_REPEAT_RELAY: - param3 = tell_command.lat*0.001f; // time - param2 = tell_command.alt; // count - param1 = tell_command.p1; // relay number - break; - - case MAV_CMD_DO_CHANGE_SPEED: - param3 = tell_command.lat; - param2 = tell_command.alt; - param1 = tell_command.p1; - break; - - case MAV_CMD_DO_SET_PARAMETER: - case MAV_CMD_DO_SET_RELAY: - case MAV_CMD_DO_SET_SERVO: - param2 = tell_command.alt; - param1 = tell_command.p1; - break; - - case MAV_CMD_DO_SET_CAM_TRIGG_DIST: - param1 = tell_command.alt; - break; + // 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) + + // rover specific overrides to packet values + if (cmd.id == MAV_CMD_CONDITION_CHANGE_ALT) { + ret_packet.param1 = cmd.content.location.lat; // Copter and Plane set param1 = cmd.p1/100 } mavlink_msg_mission_item_send(chan,msg->sysid, - msg->compid, - packet.seq, - frame, - tell_command.id, - current, - autocontinue, - param1, - param2, - param3, - param4, - x, - y, - z); + msg->compid, + packet.seq, + ret_packet.frame, + cmd.id, + ret_packet.current, + ret_packet.autocontinue, + ret_packet.param1, + ret_packet.param2, + ret_packet.param3, + ret_packet.param4, + ret_packet.x, + ret_packet.y, + ret_packet.z); + break; + +mission_item_send_failed: + // send failure message + mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ERROR); break; } @@ -1449,13 +1403,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_mission_clear_all_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) break; - // clear all commands - g.command_total.set_and_save(0); - - // note that we don't send multiple acks, as otherwise a - // GCS that is doing a clear followed by a set may see - // the additional ACKs as ACKs of the set operations - mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED); + // clear all commands + if (mission.clear()) { + // note that we don't send multiple acks, as otherwise a + // GCS that is doing a clear followed by a set may see + // the additional ACKs as ACKs of the set operations + mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED); + }else{ + // we failed to clear the mission (perhaps because we're currently running it) so send NAK + mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ERROR); + } break; } @@ -1467,9 +1424,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (mavlink_check_target(packet.target_system,packet.target_component)) break; // set current command - change_command(packet.seq); - - mavlink_msg_mission_current_send(chan, g.command_index); + if (mission.set_current_cmd(packet.seq)) { + mavlink_msg_mission_current_send(chan, mission.get_current_nav_cmd().index); + } break; } @@ -1481,16 +1438,24 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (mavlink_check_target(packet.target_system,packet.target_component)) break; // start waypoint receiving - if (packet.count > MAX_WAYPOINTS) { - packet.count = MAX_WAYPOINTS; + if (packet.count > AP_MISSION_MAX_COMMANDS) { + // send NAK + mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_NO_SPACE); + break; + } + + // new mission arriving, clear current mission + if (!mission.clear()) { + // return error if we were unable to clear the mission (possibly because we're currently flying the mission) + mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ERROR); + break; } - g.command_total.set_and_save(packet.count - 1); waypoint_timelast_receive = millis(); waypoint_timelast_request = 0; waypoint_receiving = true; waypoint_request_i = 0; - waypoint_request_last= g.command_total; + waypoint_request_last= packet.count; // record how many commands we expect to receive break; } @@ -1502,8 +1467,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (mavlink_check_target(packet.target_system,packet.target_component)) break; // start waypoint receiving - if (packet.start_index > g.command_total || - packet.end_index > g.command_total || + if (packet.start_index > mission.num_commands() || + packet.end_index > mission.num_commands() || packet.end_index < packet.start_index) { send_text_P(SEVERITY_LOW,PSTR("flight plan update rejected")); break; @@ -1538,128 +1503,22 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_mission_item_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; - // defaults - tell_command.id = packet.command; - - switch (packet.frame) - { - case MAV_FRAME_MISSION: - case MAV_FRAME_GLOBAL: - { - tell_command.lat = 1.0e7f*packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7f*packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z*1.0e2f; // in as m converted to cm - tell_command.options = 0; // absolute altitude - break; - } - -#ifdef MAV_FRAME_LOCAL_NED - case MAV_FRAME_LOCAL_NED: // local (relative to home position) - { - tell_command.lat = 1.0e7f*ToDeg(packet.x/ - (radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; - tell_command.lng = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lng; - tell_command.alt = -packet.z*1.0e2f; - tell_command.options = LOCATION_MASK_OPTIONS_RELATIVE_ALT; - break; - } -#endif - -#ifdef MAV_FRAME_LOCAL - case MAV_FRAME_LOCAL: // local (relative to home position) - { - tell_command.lat = 1.0e7f*ToDeg(packet.x/ - (radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; - tell_command.lng = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lng; - tell_command.alt = packet.z*1.0e2f; - tell_command.options = LOCATION_MASK_OPTIONS_RELATIVE_ALT; - break; - } -#endif - - case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude - { - tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z * 1.0e2f; - tell_command.options = LOCATION_MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! - break; - } - - default: - result = MAV_MISSION_UNSUPPORTED_FRAME; - break; - } - - - if (result != MAV_MISSION_ACCEPTED) goto mission_failed; - - switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields - case MAV_CMD_NAV_WAYPOINT: - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - break; - - case MAV_CMD_NAV_TAKEOFF: - case MAV_CMD_DO_SET_HOME: - tell_command.p1 = packet.param1; - break; - - case MAV_CMD_CONDITION_CHANGE_ALT: - tell_command.lat = packet.param1; - break; - - case MAV_CMD_CONDITION_DELAY: - case MAV_CMD_CONDITION_DISTANCE: - tell_command.lat = packet.param1; - break; - - case MAV_CMD_DO_JUMP: - tell_command.lat = packet.param2; - tell_command.p1 = packet.param1; - break; - - case MAV_CMD_DO_REPEAT_SERVO: - tell_command.lng = packet.param4*1000; // time - tell_command.lat = packet.param3; // count - tell_command.alt = packet.param2; // PWM - tell_command.p1 = packet.param1; // channel - break; - - case MAV_CMD_DO_REPEAT_RELAY: - tell_command.lat = packet.param3*1000; // time - tell_command.alt = packet.param2; // count - tell_command.p1 = packet.param1; // relay number - break; - - case MAV_CMD_DO_CHANGE_SPEED: - tell_command.lat = packet.param3; - tell_command.alt = packet.param2; - tell_command.p1 = packet.param1; - break; - - case MAV_CMD_DO_SET_PARAMETER: - case MAV_CMD_DO_SET_RELAY: - case MAV_CMD_DO_SET_SERVO: - tell_command.alt = packet.param2; - tell_command.p1 = packet.param1; - break; - - case MAV_CMD_DO_SET_CAM_TRIGG_DIST: - tell_command.alt = packet.param1; - break; - - default: - result = MAV_MISSION_UNSUPPORTED; - break; + // convert mavlink packet to mission command + if (!AP_Mission::mavlink_to_mission_cmd(packet, cmd)) { + result = MAV_MISSION_ERROR; + goto mission_failed; } - if (result != MAV_MISSION_ACCEPTED) goto mission_failed; + // rover specific overrides to mavlink to mission command conversion + if (cmd.id == MAV_CMD_CONDITION_CHANGE_ALT) { + cmd.content.location.lat = packet.param1; + } if(packet.current == 2){ //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission - guided_WP = tell_command; + guided_WP = cmd.content.location; // add home alt if needed - if (guided_WP.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT){ + if (guided_WP.flags.relative_alt){ guided_WP.alt += home.alt; } @@ -1688,19 +1547,39 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) goto mission_failed; } - set_cmd_with_index(tell_command, packet.seq); + // 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_failed; + } + // 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_failed; + } + // if beyond the end of the command list, return an error + }else{ + result = MAV_MISSION_ERROR; + goto mission_failed; + } // update waypoint receiving state machine waypoint_timelast_receive = millis(); waypoint_timelast_request = 0; waypoint_request_i++; - if (waypoint_request_i > waypoint_request_last) { + if (waypoint_request_i >= waypoint_request_last) { mavlink_msg_mission_ack_send( chan, msg->sysid, msg->compid, - result); + MAV_MISSION_ACCEPTED); send_text_P(SEVERITY_LOW,PSTR("flight plan received")); waypoint_receiving = false;