From 5e784ddb5c215ed5382e76e139f4e78e92ade734 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 1 Jan 2016 18:18:53 +1100
Subject: [PATCH] Plane: consider flying status for throttle_wait

---
 ArduPlane/ArduPlane.cpp | 11 +++++++----
 ArduPlane/Attitude.cpp  |  2 +-
 ArduPlane/quadplane.cpp | 20 +++++++++++++++-----
 ArduPlane/quadplane.h   |  3 +++
 4 files changed, 26 insertions(+), 10 deletions(-)

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index 046598c891..1673e00408 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -895,10 +895,6 @@ void Plane::update_flight_stage(void)
             } else {
                 set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
             }
-        } else if (control_mode == QSTABILIZE ||
-                   control_mode == QHOVER ||
-                   control_mode == QLOITER) {
-            set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL);            
         } else {
             set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
         }
@@ -913,6 +909,13 @@ void Plane::update_flight_stage(void)
         if (should_log(MASK_LOG_TECS)) {
             Log_Write_TECS_Tuning();
         }
+    } else if (control_mode == QSTABILIZE ||
+               control_mode == QHOVER ||
+               control_mode == QLOITER ||
+               quadplane.in_assisted_flight()) {
+        set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL);
+    } else {
+        set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
     }
 
     // tell AHRS the airspeed to true airspeed ratio
diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp
index 1fc6ae133b..8ebddcd595 100644
--- a/ArduPlane/Attitude.cpp
+++ b/ArduPlane/Attitude.cpp
@@ -1103,7 +1103,7 @@ void Plane::demo_servos(uint8_t i)
 void Plane::adjust_nav_pitch_throttle(void)
 {
     uint8_t throttle = throttle_percentage();
-    if (throttle < aparm.throttle_cruise) {
+    if (throttle < aparm.throttle_cruise && flight_stage != AP_SpdHgtControl::FLIGHT_VTOL) {
         float p = (aparm.throttle_cruise - throttle) / (float)aparm.throttle_cruise;
         nav_pitch_cd -= g.stab_pitch_down * 100.0f * p;
     }
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 268ca3ce91..69d0438f03 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -469,6 +469,11 @@ void QuadPlane::control_loiter()
     if (should_relax()) {
         wp_nav->loiter_soften_for_landing();
     }
+
+    if (millis() - last_loiter_ms > 500) {
+        wp_nav->init_loiter_target();
+    }
+    last_loiter_ms = millis();
     
     // initialize vertical speed and acceleration
     pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max);
@@ -601,7 +606,9 @@ void QuadPlane::update_transition(void)
       see if we should provide some assistance
      */
     if (have_airspeed && aspeed < assist_speed &&
-        (plane.auto_throttle_mode || plane.channel_throttle->control_in>0)) {
+        (plane.auto_throttle_mode ||
+         plane.channel_throttle->control_in>0 ||
+         plane.is_flying())) {
         // the quad should provide some assistance to the plane
         transition_state = TRANSITION_AIRSPEED_WAIT;
         transition_start_ms = millis();
@@ -682,7 +689,11 @@ void QuadPlane::update(void)
         // output to motors
         motors->output();
         transition_start_ms = 0;
-        transition_state = TRANSITION_AIRSPEED_WAIT;
+        if (throttle_wait && !plane.is_flying()) {
+            transition_state = TRANSITION_DONE;
+        } else {
+            transition_state = TRANSITION_AIRSPEED_WAIT;
+        }
         last_throttle = motors->get_throttle();
     }
 
@@ -794,12 +805,11 @@ void QuadPlane::control_auto(const Location &loc)
     target.y = diff2d.y * 100;
     target.z = loc.alt - origin.alt;
 
-    printf("target %.2f %.2f %.2f\n", target.x*0.01, target.y*0.01, target.z*0.01);
-    
-    if (!locations_are_same(loc, last_auto_target)) {
+    if (!locations_are_same(loc, last_auto_target) || millis() - last_loiter_ms > 500) {
         wp_nav->set_wp_destination(target);
         last_auto_target = loc;
     }
+    last_loiter_ms = millis();
     
     // initialize vertical speed and acceleration
     pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max);
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index ce071480b8..f2d9de8bdf 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -145,4 +145,7 @@ private:
 
     // time when motors reached lower limit
     uint32_t motors_lower_limit_start_ms;
+
+    // time we last set the loiter target
+    uint32_t last_loiter_ms;
 };