From 95b3a5bacdece66055d27306e7ab8f0caa0ee46f Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Tue, 10 Apr 2018 16:07:24 -0600
Subject: [PATCH] ArduPlane: implement hover throttle learning for quadplanes

disable by default
check quadplane.enable in update_hover_learn
---
 ArduPlane/ArduPlane.cpp |  1 +
 ArduPlane/Plane.h       |  1 +
 ArduPlane/quadplane.cpp | 30 ++++++++++++++++++++++++++++++
 ArduPlane/quadplane.h   |  1 +
 ArduPlane/servos.cpp    |  5 +++++
 5 files changed, 38 insertions(+)

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index 4aaa6b8b64..78e7ef20ed 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -39,6 +39,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
     SCHED_TASK(update_flight_mode,    400,    100),
     SCHED_TASK(stabilize,             400,    100),
     SCHED_TASK(set_servos,            400,    100),
+    SCHED_TASK(update_throttle_hover, 100,     90),
     SCHED_TASK(read_control_switch,     7,    100),
     SCHED_TASK(update_GPS_50Hz,        50,    300),
     SCHED_TASK(update_GPS_10Hz,        10,    400),
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 49f5967ccb..bcd7df02f1 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -1004,6 +1004,7 @@ private:
     void calc_nav_yaw_ground(void);
     void throttle_slew_limit(SRV_Channel::Aux_servo_function_t func);
     bool suppress_throttle(void);
+    void update_throttle_hover();
     void channel_function_mixer(SRV_Channel::Aux_servo_function_t func1_in, SRV_Channel::Aux_servo_function_t func2_in,
                                 SRV_Channel::Aux_servo_function_t func1_out, SRV_Channel::Aux_servo_function_t func2_out);
     void flaperon_update(int8_t flap_percent);
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 2a3f0c8204..e79aadea5e 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -416,6 +416,7 @@ static const struct AP_Param::defaults_table_struct defaults_table[] = {
     { "Q_A_RAT_PIT_I",    0.25 },
     { "Q_A_RAT_PIT_FILT", 10.0 },
     { "Q_M_SPOOL_TIME",   0.25 },
+    { "Q_M_HOVER_LEARN",     0 },
     { "Q_LOIT_ANG_MAX",   15.0 },
     { "Q_LOIT_ACC_MAX",   250.0 },
     { "Q_LOIT_BRK_ACCEL", 50.0 },
@@ -781,6 +782,7 @@ void QuadPlane::run_z_controller(void)
         // controller. We need to assume the integrator may be way off
 
         // the base throttle we start at is the current throttle we are using
+        // note that AC_PosControl::run_z_controller() adds the Z pid (_pid_accel_z) output to _motors.get_throttle_hover()
         float base_throttle = constrain_float(motors->get_throttle() - motors->get_throttle_hover(), -1, 1) * 1000;
         pos_control->get_accel_z_pid().set_integrator(base_throttle);
 
@@ -1608,6 +1610,34 @@ void QuadPlane::check_throttle_suppression(void)
     last_motors_active_ms = 0;
 }
 
+// update estimated throttle required to hover (if necessary)
+//  called at 100hz
+void QuadPlane::update_throttle_hover()
+{
+    if (!enable) {
+        return;
+    }
+    
+    // if not armed or landed exit
+    if (!motors->armed() || !is_flying_vtol()) {
+        return;
+    }
+
+    // do not update while climbing or descending
+    if (!is_zero(pos_control->get_desired_velocity().z)) {
+        return;
+    }
+
+    // get throttle output
+    float throttle = motors->get_throttle();
+
+    // calc average throttle if we are in a level hover
+    if (throttle > 0.0f && fabsf(inertial_nav.get_velocity_z()) < 60 &&
+            labs(ahrs_view->roll_sensor) < 500 && labs(ahrs_view->pitch_sensor) < 500) {
+        // Can we set the time constant automatically
+        motors->update_throttle_hover(0.01f);
+    }
+}
 /*
   output motors and do any copter needed
  */
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 91aa036b51..ddff04f93f 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -76,6 +76,7 @@ public:
     bool verify_vtol_land(void);
     bool in_vtol_auto(void) const;
     bool in_vtol_mode(void) const;
+    void update_throttle_hover();
 
     // vtol help for is_flying()
     bool is_flying(void);
diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp
index b12ae9d4ba..e06cdd5f5b 100644
--- a/ArduPlane/servos.cpp
+++ b/ArduPlane/servos.cpp
@@ -781,6 +781,11 @@ void Plane::servos_output(void)
     }
 }
 
+void Plane::update_throttle_hover() {
+    // update hover throttle at 100Hz
+    quadplane.update_throttle_hover();
+}
+
 /*
   implement automatic persistent trim of control surfaces with
   AUTO_TRIM=2, only available when SERVO_RNG_ENABLE=1 as otherwise it