From 691d4b6ca7969590de521b5ec918dc5f26a38d25 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 May 2016 10:40:42 +1000 Subject: [PATCH] Plane: added local reached_loiter_target() this distinguishes between VTOL and fixed wing loiter targets --- ArduPlane/ArduPlane.cpp | 2 +- ArduPlane/Plane.h | 1 + ArduPlane/altitude.cpp | 4 ++-- ArduPlane/commands_logic.cpp | 9 +++++++-- ArduPlane/navigation.cpp | 15 +++++++++++++-- 5 files changed, 24 insertions(+), 7 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 9076d9c481..f84bfa5b67 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -761,7 +761,7 @@ void Plane::update_navigation() break; } else if (g.rtl_autoland == 1 && !auto_state.checked_for_autoland && - nav_controller->reached_loiter_target() && + reached_loiter_target() && labs(altitude_error_cm) < 1000) { // we've reached the RTL point, see if we have a landing sequence jump_to_landing_sequence(); diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 6105464053..d51fd0f6be 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -892,6 +892,7 @@ private: void update_cruise(); void update_fbwb_speed_height(void); void setup_turn_angle(void); + bool reached_loiter_target(void); bool print_buffer(char *&buf, uint16_t &buf_size, const char *fmt, ...); uint16_t create_mixer(char *buf, uint16_t buf_size, const char *filename); bool setup_failsafe_mixing(void); diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 3924a83eab..e3f9becaa7 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -37,7 +37,7 @@ void Plane::adjust_altitude_target() } else if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE) { setup_landing_glide_slope(); - } else if (nav_controller->reached_loiter_target()) { + } else if (reached_loiter_target()) { // once we reach a loiter target then lock to the final // altitude target set_target_altitude_location(next_WP_loc); @@ -490,7 +490,7 @@ float Plane::lookahead_adjustment(void) // there is no target waypoint in FBWB, so use yaw as an approximation bearing_cd = ahrs.yaw_sensor; distance = g.terrain_lookahead; - } else if (!nav_controller->reached_loiter_target()) { + } else if (!reached_loiter_target()) { bearing_cd = nav_controller->target_bearing_cd(); distance = constrain_float(auto_state.wp_distance, 0, g.terrain_lookahead); } else { diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 12e65ddaaa..58e7085c84 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -618,7 +618,7 @@ bool Plane::verify_loiter_time() update_loiter(0); if (loiter.start_time_ms == 0) { - if (nav_controller->reached_loiter_target() && loiter.sum_cd > 1) { + if (reached_loiter_target() && loiter.sum_cd > 1) { // we've reached the target, start the timer loiter.start_time_ms = millis(); } @@ -702,7 +702,7 @@ bool Plane::verify_RTL() } update_loiter(abs(g.rtl_radius)); if (auto_state.wp_distance <= (uint32_t)MAX(g.waypoint_radius,0) || - nav_controller->reached_loiter_target()) { + reached_loiter_target()) { gcs_send_text(MAV_SEVERITY_INFO,"Reached HOME"); return true; } else { @@ -975,6 +975,11 @@ void Plane::exit_mission_callback() bool Plane::verify_loiter_heading(bool init) { + if (quadplane.in_vtol_auto()) { + // skip heading verify if in VTOL auto + return true; + } + //Get the lat/lon of next Nav waypoint after this one: AP_Mission::Mission_Command next_nav_cmd; if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1, diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 848c03581d..2f9087b6be 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -34,7 +34,7 @@ void Plane::loiter_angle_update(void) { int32_t target_bearing_cd = nav_controller->target_bearing_cd(); int32_t loiter_delta_cd; - if (loiter.sum_cd == 0 && !nav_controller->reached_loiter_target()) { + if (loiter.sum_cd == 0 && !reached_loiter_target()) { // we don't start summing until we are doing the real loiter loiter_delta_cd = 0; } else if (loiter.sum_cd == 0) { @@ -188,7 +188,7 @@ void Plane::update_loiter(uint16_t radius) } if (loiter.start_time_ms == 0) { - if (nav_controller->reached_loiter_target() || + if (reached_loiter_target() || auto_state.wp_proportion > 1) { // we've reached the target, start the timer loiter.start_time_ms = millis(); @@ -291,3 +291,14 @@ void Plane::setup_turn_angle(void) } } +/* + see if we have reached our loiter target + */ +bool Plane::reached_loiter_target(void) +{ + if (quadplane.in_vtol_auto()) { + return auto_state.wp_distance < 3; + } + return nav_controller->reached_loiter_target(); +} +