From 66e43bf6a2a82071c4af01f40a55d2e313bac378 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 25 Jun 2016 06:27:56 +1000 Subject: [PATCH] Plane: start with low integrator on back quadplane transition When changing to alt_hold controller in quadplane with significant airspeed set the initial integrator to minus the hover throttle to allow the accel controller to climb slowly --- ArduPlane/quadplane.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index f4f04a86c6..8b3432c0e6 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1215,6 +1215,15 @@ void QuadPlane::vtol_position_controller(void) // max_speed will control how fast we will fly. It will always decrease poscontrol.max_speed = MAX(speed_towards_target, wp_nav->get_speed_xy() * 0.01); poscontrol.speed_scale = poscontrol.max_speed / MAX(distance, 1); + + // start with low integrator. The alt_hold controller will + // add hover throttle to initial integrator. By starting + // without it we end up with a smoother startup when + // transitioning from fixed wing flight + float aspeed; + if (ahrs.airspeed_estimate(&aspeed) && aspeed > 6) { + pid_accel_z.set_integrator((-motors->get_throttle_hover())*1000.0f); + } } // run fixed wing navigation