From de119e07bb858f927f633aa40be9bc861bcced8f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 17 Mar 2014 16:50:27 +0900 Subject: [PATCH] Rover: remove do_takeoff, do_change_alt support --- APMrover2/APMrover2.pde | 2 -- APMrover2/GCS_Mavlink.pde | 10 --------- APMrover2/commands_logic.pde | 43 ------------------------------------ 3 files changed, 55 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index e4d477f119..11a6715a96 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -502,8 +502,6 @@ static int32_t condition_value; // A starting value used to check the status of a conditional command. // For example in a delay command the condition_start records that start time for the delay static int32_t condition_start; -// A value used in condition commands. For example the rate at which to change altitude. -static int16_t condition_rate; //////////////////////////////////////////////////////////////////////////////// // 3D Location vectors diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index d976443b52..02bec0ee5e 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -1293,11 +1293,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // 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, @@ -1505,11 +1500,6 @@ mission_item_send_failed: 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 = cmd.content.location; diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde index 729dd648ce..3f1a703204 100644 --- a/APMrover2/commands_logic.pde +++ b/APMrover2/commands_logic.pde @@ -1,11 +1,9 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // forward declarations to make compiler happy -static void do_takeoff(const AP_Mission::Mission_Command& cmd); static void do_nav_wp(const AP_Mission::Mission_Command& cmd); static void do_wait_delay(const AP_Mission::Mission_Command& cmd); static void do_within_distance(const AP_Mission::Mission_Command& cmd); -static void do_change_alt(const AP_Mission::Mission_Command& cmd); static void do_change_speed(const AP_Mission::Mission_Command& cmd); static void do_set_home(const AP_Mission::Mission_Command& cmd); static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); @@ -24,10 +22,6 @@ start_command(const AP_Mission::Mission_Command& cmd) gcs_send_text_fmt(PSTR("Executing command ID #%i"),cmd.id); switch(cmd.id){ - case MAV_CMD_NAV_TAKEOFF: - do_takeoff(cmd); - break; - case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint do_nav_wp(cmd); break; @@ -45,10 +39,6 @@ start_command(const AP_Mission::Mission_Command& cmd) do_within_distance(cmd); break; - case MAV_CMD_CONDITION_CHANGE_ALT: - do_change_alt(cmd); - break; - // Do commands case MAV_CMD_DO_CHANGE_SPEED: do_change_speed(cmd); @@ -148,9 +138,6 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd) switch(cmd.id) { - case MAV_CMD_NAV_TAKEOFF: - return verify_takeoff(); - case MAV_CMD_NAV_WAYPOINT: return verify_nav_wp(cmd); @@ -165,10 +152,6 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd) return verify_within_distance(); break; - case MAV_CMD_CONDITION_CHANGE_ALT: - return verify_change_alt(); - break; - default: gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Unsupported command")); return true; @@ -188,11 +171,6 @@ static void do_RTL(void) next_WP = home; } -static void do_takeoff(const AP_Mission::Mission_Command& cmd) -{ - set_next_WP(cmd.content.location); -} - static void do_nav_wp(const AP_Mission::Mission_Command& cmd) { set_next_WP(cmd.content.location); @@ -201,10 +179,6 @@ static void do_nav_wp(const AP_Mission::Mission_Command& cmd) /********************************************************************************/ // Verify Nav (Must) commands /********************************************************************************/ -static bool verify_takeoff() -{ return true; -} - static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd) { if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) { @@ -253,14 +227,6 @@ static void do_wait_delay(const AP_Mission::Mission_Command& cmd) condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds } -static void do_change_alt(const AP_Mission::Mission_Command& cmd) -{ - condition_rate = abs((int)cmd.content.location.lat); - condition_value = cmd.content.location.alt; - if(condition_value < current_loc.alt) condition_rate = -condition_rate; - next_WP.alt = condition_value; // For future nav calculations -} - static void do_within_distance(const AP_Mission::Mission_Command& cmd) { condition_value = cmd.content.distance.meters; @@ -279,15 +245,6 @@ static bool verify_wait_delay() return false; } -static bool verify_change_alt() -{ - if( (condition_rate>=0 && current_loc.alt >= condition_value) || (condition_rate<=0 && current_loc.alt <= condition_value)) { - condition_value = 0; - return true; - } - return false; -} - static bool verify_within_distance() { if (wp_distance < condition_value){