diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 47ce76dc80..81810a779f 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -841,7 +841,6 @@ private: bool verify_RTL(); bool verify_continue_and_change_alt(); bool verify_wait_delay(); - bool verify_change_alt(); bool verify_within_distance(); bool verify_altitude_wait(const AP_Mission::Mission_Command &cmd); bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd); @@ -1027,7 +1026,6 @@ private: void do_vtol_land(const AP_Mission::Mission_Command& cmd); bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); void do_wait_delay(const AP_Mission::Mission_Command& cmd); - void do_change_alt(const AP_Mission::Mission_Command& cmd); void do_within_distance(const AP_Mission::Mission_Command& cmd); void do_change_speed(const AP_Mission::Mission_Command& cmd); void do_set_home(const AP_Mission::Mission_Command& cmd); diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 21f2ff5503..2349007aef 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -48,7 +48,7 @@ void Plane::adjust_altitude_target() // stay within the range of the start and end locations in altitude constrain_target_altitude_location(next_WP_loc, prev_WP_loc); - } else if (mission.get_current_do_cmd().id != MAV_CMD_CONDITION_CHANGE_ALT) { + } else { set_target_altitude_location(next_WP_loc); } diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index be70214d9f..a92a4debd0 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -103,10 +103,6 @@ bool Plane::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: @@ -277,9 +273,6 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret case MAV_CMD_CONDITION_DISTANCE: return verify_within_distance(); - case MAV_CMD_CONDITION_CHANGE_ALT: - return verify_change_alt(); - #if PARACHUTE == ENABLED case MAV_CMD_DO_PARACHUTE: // assume parachute was released successfully @@ -792,23 +785,6 @@ void Plane::do_wait_delay(const AP_Mission::Mission_Command& cmd) condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds } -/* - process a DO_CHANGE_ALT request - */ -void Plane::do_change_alt(const AP_Mission::Mission_Command& cmd) -{ - condition_rate = labs((int)cmd.content.location.lat); // climb rate in cm/s - condition_value = cmd.content.location.alt; // To-Do: ensure this altitude is an absolute altitude? - if (condition_value < adjusted_altitude_cm()) { - condition_rate = -condition_rate; - } - set_target_altitude_current_adjusted(); - change_target_altitude(condition_rate/10); - next_WP_loc.alt = condition_value; // For future nav calculations - reset_offset_altitude(); - setup_glide_slope(); -} - void Plane::do_within_distance(const AP_Mission::Mission_Command& cmd) { condition_value = cmd.content.distance.meters; @@ -827,19 +803,6 @@ bool Plane::verify_wait_delay() return false; } -bool Plane::verify_change_alt() -{ - if( (condition_rate>=0 && adjusted_altitude_cm() >= condition_value) || - (condition_rate<=0 && adjusted_altitude_cm() <= condition_value)) { - condition_value = 0; - return true; - } - // condition_rate is climb rate in cm/s. - // We divide by 10 because this function is called at 10hz - change_target_altitude(condition_rate/10); - return false; -} - bool Plane::verify_within_distance() { if (auto_state.wp_distance < MAX(condition_value,0)) {