From d271dda8c58d986f3e7f3fccd1b8f9d8c68d3563 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 21 Sep 2018 16:01:30 +0900
Subject: [PATCH] AC_PrecLand: init accepts update rate

---
 libraries/AC_PrecLand/AC_PrecLand.cpp | 33 +++++++++++----------------
 libraries/AC_PrecLand/AC_PrecLand.h   |  6 ++---
 2 files changed, 15 insertions(+), 24 deletions(-)

diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp
index d4ef3d1753..a31bfcf6de 100644
--- a/libraries/AC_PrecLand/AC_PrecLand.cpp
+++ b/libraries/AC_PrecLand/AC_PrecLand.cpp
@@ -114,8 +114,9 @@ AC_PrecLand::AC_PrecLand()
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// init - perform any required initialisation of backends
-void AC_PrecLand::init()
+// perform any required initialisation of landing controllers
+// update_rate_hz should be the rate at which the update method will be called in hz
+void AC_PrecLand::init(uint16_t update_rate_hz)
 {
     // exit immediately if init has already been run
     if (_backend != nullptr) {
@@ -126,8 +127,16 @@ void AC_PrecLand::init()
     _backend = nullptr;
     _backend_state.healthy = false;
 
-    // create inertial history buffer, return on failure so backends are not created
-    if (!init_inertial_buffer()) {
+    // create inertial history buffer
+    // constrain lag parameter to be within bounds
+    _lag = constrain_float(_lag, 0.02f, 0.25f);
+
+    // calculate inertial buffer size from lag and minimum of main loop rate and update_rate_hz argument
+    const uint16_t inertial_buffer_size = MAX((uint16_t)roundf(_lag * MIN(update_rate_hz, AP::scheduler().get_loop_rate_hz())), 1);
+
+    // instantiate ring buffer to hold inertial history, return on failure so no backends are created
+    _inertial_history = new ObjectArray<inertial_data_frame_s>(inertial_buffer_size);
+    if (_inertial_history == nullptr) {
         return;
     }
 
@@ -251,22 +260,6 @@ void AC_PrecLand::handle_msg(mavlink_message_t* msg)
 // Private methods
 //
 
-// initialise inertial buffer
-bool AC_PrecLand::init_inertial_buffer()
-{
-    // constrain lag parameter to be within bounds
-    _lag = constrain_float(_lag, 0.02f, 0.25f);
-
-    // calculate inertial buffer size from delay length (loop runs at main loop rate)
-    uint16_t inertial_buffer_size = MAX((uint16_t)roundf(_lag * AP::scheduler().get_loop_rate_hz()), 1);
-
-    // instantiate ring buffer to hold inertial history
-    _inertial_history = new ObjectArray<inertial_data_frame_s>(inertial_buffer_size);
-
-    // return success/failure
-    return (_inertial_history != nullptr);
-}
-
 void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid)
 {
     const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0];
diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h
index 27f578c1d9..66c450ca4c 100644
--- a/libraries/AC_PrecLand/AC_PrecLand.h
+++ b/libraries/AC_PrecLand/AC_PrecLand.h
@@ -48,7 +48,8 @@ public:
     };
 
     // perform any required initialisation of landing controllers
-    void init();
+    // update_rate_hz should be the rate at which the update method will be called in hz
+    void init(uint16_t update_rate_hz);
 
     // returns true if precision landing is healthy
     bool healthy() const { return _backend_state.healthy; }
@@ -101,9 +102,6 @@ private:
     // returns enabled parameter as an behaviour
     enum PrecLandBehaviour get_behaviour() const { return (enum PrecLandBehaviour)(_enabled.get()); }
 
-    // initialise inertial buffer, return true on success
-    bool init_inertial_buffer();
-
     // run target position estimator
     void run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid);