From f068a8c913dd8b745193959695e93075950d40a3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 1 Jan 2016 20:42:10 +1100 Subject: [PATCH] Plane: added support for VTOL_TAKEOFF and VTOL_LAND commands --- ArduPlane/Plane.h | 4 ++ ArduPlane/commands_logic.cpp | 14 ++++++ ArduPlane/quadplane.cpp | 86 ++++++++++++++++++++++++++++++++++++ ArduPlane/quadplane.h | 7 +++ 4 files changed, 111 insertions(+) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 389176157f..720f7bc09f 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -813,6 +813,8 @@ private: 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); + bool verify_vtol_land(void); void do_loiter_at_location(); void do_take_picture(); void log_picture(); @@ -987,6 +989,8 @@ private: void do_altitude_wait(const AP_Mission::Mission_Command& cmd); void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd); void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd); + void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd); + 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); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index c795ab8b2e..d5f90184f9 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -82,6 +82,14 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) do_altitude_wait(cmd); break; + case MAV_CMD_NAV_VTOL_TAKEOFF: + crash_state.is_crashed = false; + return quadplane.do_vtol_takeoff(cmd); + + case MAV_CMD_NAV_VTOL_LAND: + crash_state.is_crashed = false; + return quadplane.do_vtol_land(cmd); + // Conditional commands case MAV_CMD_CONDITION_DELAY: @@ -272,6 +280,12 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret break; #endif + case MAV_CMD_NAV_VTOL_TAKEOFF: + return quadplane.verify_vtol_takeoff(cmd); + + case MAV_CMD_NAV_VTOL_LAND: + return quadplane.verify_vtol_land(); + // do commands (always return true) case MAV_CMD_DO_CHANGE_SPEED: case MAV_CMD_DO_SET_HOME: diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 69d0438f03..37006755cd 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -630,6 +630,7 @@ void QuadPlane::update_transition(void) transition_state = TRANSITION_TIMER; GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Transition airspeed reached %.1f", aspeed); } + assisted_flight = true; hold_hover(assist_climb_rate_cms()); attitude_control->rate_controller_run(); motors->output(); @@ -825,5 +826,90 @@ void QuadPlane::control_auto(const Location &loc) plane.nav_roll_cd = wp_nav->get_roll(); plane.nav_pitch_cd = wp_nav->get_pitch(); + switch (plane.mission.get_current_nav_cmd().id) { + case MAV_CMD_NAV_VTOL_LAND: + pos_control->set_alt_target_from_climb_rate_ff(-50, plane.G_Dt, true); + break; + case MAV_CMD_NAV_VTOL_TAKEOFF: + pos_control->set_alt_target_with_slew(wp_nav->get_loiter_target().z, plane.ins.get_loop_delta_t()); + break; + default: + pos_control->set_alt_target_from_climb_rate_ff(assist_climb_rate_cms(), plane.G_Dt, false); + break; + } + pos_control->update_z_controller(); } + +/* + start a VTOL takeoff + */ +bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd) +{ + if (!available()) { + return false; + } + plane.prev_WP_loc = plane.current_loc; + plane.next_WP_loc = cmd.content.location; + plane.next_WP_loc.alt = plane.current_loc.alt + cmd.content.location.alt; + plane.auto_state.vtol_mode = true; + throttle_wait = false; + + // also update nav_controller for status output + plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc); + return true; +} + + +/* + start a VTOL landing + */ +bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd) +{ + if (!available()) { + return false; + } + plane.prev_WP_loc = plane.current_loc; + plane.next_WP_loc = cmd.content.location; + plane.auto_state.vtol_mode = true; + throttle_wait = false; + land_complete = false; + + // also update nav_controller for status output + plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc); + return true; +} + +/* + check if a VTOL takeoff has completed + */ +bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd) +{ + if (plane.auto_state.wp_distance > 2) { + return false; + } + const int32_t threshold = 30; + if (plane.current_loc.alt+threshold < plane.next_WP_loc.alt) { + return false; + } + plane.auto_state.vtol_mode = false; + transition_state = TRANSITION_AIRSPEED_WAIT; + return true; +} + +/* + check if a VTOL landing has completed + */ +bool QuadPlane::verify_vtol_land(void) +{ + if (!should_relax()) { + return false; + } + wp_nav->loiter_soften_for_landing(); + if (millis() - motors_lower_limit_start_ms > 5000) { + plane.disarm_motors(); + land_complete = true; + plane.gcs_send_text(MAV_SEVERITY_INFO,"Land complete"); + } + return false; +} diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index f2d9de8bdf..6e3070c543 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -42,6 +42,11 @@ public: bool handle_do_vtol_transition(const mavlink_command_long_t &packet); + bool do_vtol_takeoff(const AP_Mission::Mission_Command& cmd); + bool do_vtol_land(const AP_Mission::Mission_Command& cmd); + bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd); + bool verify_vtol_land(void); + private: AP_AHRS_NavEKF &ahrs; AP_Vehicle::MultiCopter aparm; @@ -148,4 +153,6 @@ private: // time we last set the loiter target uint32_t last_loiter_ms; + + bool land_complete:1; };