AC_PrecLand: LAG to float and use main loop rate

also includes other changes from peer review:

- check_inertia_buffer renamed to init_inertial_buffer and nullptr checks removed to make it more clear this should only be called once
- init_inertial_buffer made private
- add check that inertial_buffer_size is never less than 1
- fixup comments

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -1,4 +1,5 @@
 #include <AP_HAL/AP_HAL.h>
+#include <AP_Scheduler/AP_Scheduler.h>
 #include "AC_PrecLand.h"
 #include "AC_PrecLand_Backend.h"
 #include "AC_PrecLand_Companion.h"
@@ -93,12 +94,12 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
     // @Param: LAG
     // @DisplayName: Precision Landing sensor lag
     // @Description: Precision Landing sensor lag in ms, to cope with variable landing_target latency
-    // @Range: 1 250
+    // @Range: 0.02 0.250
     // @Increment: 1
-    // @Units: ms
+    // @Units: s
     // @User: Advanced
     // @RebootRequired: True
-    AP_GROUPINFO("LAG", 9, AC_PrecLand, _lag_ms, 20), // 20ms is the old default buffer size (8 frames @ 400hz/2.5ms)
+    AP_GROUPINFO("LAG", 9, AC_PrecLand, _lag, 0.02), // 20ms is the old default buffer size (8 frames @ 400hz/2.5ms)
 
     AP_GROUPEND
 };
@@ -125,11 +126,9 @@ void AC_PrecLand::init()
     _backend = nullptr;
     _backend_state.healthy = false;
 
-    // Create inertial history buffer
-    check_inertial_buffer();
-    /* NOTE: If this check below returns without creating buffer and backend then SITL crashes */
-    if (_inertial_history == nullptr) {
-        return; // allocation failed
+    // create inertial history buffer, return on failure so backends are not created
+    if (!init_inertial_buffer()) {
+        return;
     }
 
     // instantiate backend based on type parameter
@@ -162,24 +161,6 @@ void AC_PrecLand::init()
     }
 }
 
-// create inertial buffer
-void AC_PrecLand::check_inertial_buffer()
-{
-    // if lag parameter is not within bounds, return without doing anything
-    if (_lag_ms <= 0 || _lag_ms > 250) {
-        return;
-    }
-
-    // calculate inertial buffer size from delay length (loop runs at 400hz)
-    uint16_t inertial_buffer_size = (uint16_t)roundf(_lag_ms / (1000/400.0f));
-
-    // create buffer if it doesn't exist or if latency parameter has changed
-    if (_inertial_history == nullptr || inertial_buffer_size != _inertial_history->size()) {
-        // instantiate ring buffer to hold inertial history
-        _inertial_history = new ObjectArray<inertial_data_frame_s>(inertial_buffer_size);
-    }
-}
-
 // update - give chance to driver to get updates from sensor
 void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid)
 {
@@ -270,6 +251,22 @@ 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];

libraries/AC_PrecLand/AC_PrecLand.h

@@ -59,9 +59,6 @@ public:
     // returns time of last update
     uint32_t last_update_ms() const { return _last_update_ms; }
 
-    // create or update the inertial buffer
-    void check_inertial_buffer();
-
     // returns time of last time target was seen
     uint32_t last_backend_los_meas_ms() const { return _last_backend_los_meas_ms; }
 
@@ -104,6 +101,9 @@ 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);
 
@@ -122,7 +122,7 @@ private:
     AP_Int8                     _type;              // precision landing sensor type
     AP_Int8                     _bus;               // which sensor bus
     AP_Int8                     _estimator_type;    // precision landing estimator type
-    AP_Int16                    _lag_ms;            // sensor lag in ms
+    AP_Float                    _lag;               // sensor lag in seconds
     AP_Float                    _yaw_align;         // Yaw angle from body x-axis to sensor x-axis.
     AP_Float                    _land_ofs_cm_x;     // Desired landing position of the camera forward of the target in vehicle body frame
     AP_Float                    _land_ofs_cm_y;     // Desired landing position of the camera right of the target in vehicle body frame