Plane: added local reached_loiter_target()

this distinguishes between VTOL and fixed wing loiter targets

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();

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);

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 {

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,

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();
+}
+