From 418464ab8cd2ded378fbd27042db26277f2d469e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 1 Jan 2016 21:39:36 +1100 Subject: [PATCH] Plane: added quadplane version of is_flying() --- ArduPlane/ArduPlane.cpp | 6 +++--- ArduPlane/Attitude.cpp | 7 ++++++- ArduPlane/is_flying.cpp | 4 ++++ ArduPlane/quadplane.cpp | 46 ++++++++++++++++++++++++++++++++++++----- ArduPlane/quadplane.h | 4 ++++ 5 files changed, 58 insertions(+), 9 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 1673e00408..c6e2097f86 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -486,7 +486,7 @@ void Plane::handle_auto_mode(void) nav_cmd_id = auto_rtl_command.id; } - if (auto_state.vtol_mode) { + if (quadplane.in_vtol_auto()) { quadplane.control_auto(next_WP_loc); } else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF || (nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) { @@ -541,7 +541,7 @@ void Plane::update_flight_mode(void) if (effective_mode == QSTABILIZE || effective_mode == QHOVER || effective_mode == QLOITER || - (effective_mode == AUTO && auto_state.vtol_mode)) { + quadplane.in_vtol_auto()) { ahrs.set_fly_forward(false); } else { ahrs.set_fly_forward(true); @@ -867,7 +867,7 @@ void Plane::update_flight_stage(void) // Update the speed & height controller states if (auto_throttle_mode && !throttle_suppressed) { if (control_mode==AUTO) { - if (auto_state.vtol_mode) { + if (quadplane.in_vtol_auto()) { set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL); } else if (auto_state.takeoff_complete == false) { set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF); diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 8ebddcd595..792d8e9833 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -638,6 +638,11 @@ bool Plane::suppress_throttle(void) } } + if (quadplane.is_flying()) { + gcs_send_text_fmt(MAV_SEVERITY_INFO, "Throttle enabled VTOL"); + throttle_suppressed = false; + } + // throttle remains suppressed return true; } @@ -931,7 +936,7 @@ void Plane::set_servos(void) } else if (control_mode == QSTABILIZE || control_mode == QHOVER || control_mode == QLOITER || - (control_mode == AUTO && auto_state.vtol_mode)) { + quadplane.in_vtol_auto()) { // no forward throttle for now channel_throttle->servo_out = 0; channel_throttle->calc_pwm(); diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index e10717d327..525e9ce6cb 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -125,6 +125,10 @@ void Plane::update_is_flying_5Hz(void) isFlyingProbability = (0.85f * isFlyingProbability) + (0.15f * (float)is_flying_bool); } + if (quadplane.is_flying()) { + is_flying_bool = true; + } + /* update last_flying_ms so we always know how long we have not been flying for. This helps for crash detection and auto-disarm diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 37006755cd..0d5a7fd5a5 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -440,6 +440,19 @@ void QuadPlane::init_loiter(void) init_throttle_wait(); } + +// helper for is_flying() +bool QuadPlane::is_flying(void) +{ + if (!available()) { + return false; + } + if (motors->get_throttle() > 200 && !motors->limit.throttle_lower) { + return true; + } + return false; +} + // crude landing detector to prevent tipover bool QuadPlane::should_relax(void) { @@ -677,7 +690,7 @@ void QuadPlane::update(void) bool quad_mode = (plane.control_mode == QSTABILIZE || plane.control_mode == QHOVER || plane.control_mode == QLOITER || - (plane.control_mode == AUTO && plane.auto_state.vtol_mode)); + in_vtol_auto()); if (!quad_mode) { update_transition(); @@ -793,6 +806,26 @@ bool QuadPlane::handle_do_vtol_transition(const mavlink_command_long_t &packet) return MAV_RESULT_FAILED; } +/* + are we in a VTOL auto state? + */ +bool QuadPlane::in_vtol_auto(void) +{ + if (plane.control_mode != AUTO) { + return false; + } + if (plane.auto_state.vtol_mode) { + return true; + } + switch (plane.mission.get_current_nav_cmd().id) { + case MAV_CMD_NAV_VTOL_LAND: + case MAV_CMD_NAV_VTOL_TAKEOFF: + return true; + default: + return false; + } +} + /* handle auto-mode when auto_state.vtol_mode is true */ @@ -828,7 +861,13 @@ void QuadPlane::control_auto(const Location &loc) 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); + if (plane.auto_state.wp_distance > 2) { + pos_control->set_alt_target_from_climb_rate_ff(0, plane.G_Dt, true); + } else if (plane.current_loc.alt > plane.next_WP_loc.alt+5*100) { + pos_control->set_alt_target_with_slew(wp_nav->get_loiter_target().z, plane.ins.get_loop_delta_t()); + } else { + pos_control->set_alt_target_from_climb_rate_ff(-30, plane.G_Dt, false); + } 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()); @@ -852,7 +891,6 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd) 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 @@ -871,7 +909,6 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd) } 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; @@ -892,7 +929,6 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd) 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; } diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 6e3070c543..a5fdb2f074 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -46,6 +46,10 @@ public: 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); + bool in_vtol_auto(void); + + // vtol help for is_flying() + bool is_flying(void); private: AP_AHRS_NavEKF &ahrs;