AP_Param: use background parameter save

this moves both the storage scan and the save code out of the main
thread and into the IO thread. It means that if we have more than 30
parameters saves in very rapid succession while armed that we can lose
parameter changes, but that is extremely unlikely.

This fixes an issue where parameter saves in flight can cause
considerable scheduling problems, sometimes several milliseconds

libraries/AP_Param/AP_Param.cpp

@@ -68,6 +68,12 @@ const AP_Param::Info *AP_Param::_var_info;
 struct AP_Param::param_override *AP_Param::param_overrides = nullptr;
 uint16_t AP_Param::num_param_overrides = 0;
 
+ObjectBuffer<AP_Param::param_save> AP_Param::save_queue{30};
+bool AP_Param::registered_save_handler;
+
+// we need a dummy object for the parameter save callback
+static AP_Param save_dummy;
+
 /*
   this holds default parameters in the normal NAME=value form for a
   parameter file. It can be manipulated by apj_tool.py to change the
@@ -977,9 +983,10 @@ void AP_Param::notify() const {
 }
 
 
-// Save the variable to EEPROM, if supported
-//
-bool AP_Param::save(bool force_save)
+/*
+  Save the variable to HAL storage, synchronous version
+*/
+void AP_Param::save_sync(bool force_save)
 {
     uint32_t group_element = 0;
     const struct GroupInfo *ginfo;
@@ -990,7 +997,7 @@ bool AP_Param::save(bool force_save)
 
     if (info == nullptr) {
         // we don't have any info on how to store it
-        return false;
+        return;
     }
 
     struct Param_header phdr;
@@ -1007,7 +1014,7 @@ bool AP_Param::save(bool force_save)
     ap = this;
     if (phdr.type != AP_PARAM_VECTOR3F && idx != 0) {
         // only vector3f can have non-zero idx for now
-        return false;
+        return;
     }
     if (idx != 0) {
         ap = (const AP_Param *)((ptrdiff_t)ap) - (idx*sizeof(float));
@@ -1027,10 +1034,10 @@ bool AP_Param::save(bool force_save)
         // found an existing copy of the variable
         eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
         send_parameter(name, (enum ap_var_type)phdr.type, idx);
-        return true;
+        return;
     }
     if (ofs == (uint16_t) ~0) {
-        return false;
+        return;
     }
 
     // if the value is the default value then don't save
@@ -1044,7 +1051,7 @@ bool AP_Param::save(bool force_save)
         }
         if (is_equal(v1,v2) && !force_save) {
             gcs().send_parameter_value(name, (enum ap_var_type)info->type, v2);
-            return true;
+            return;
         }
         if (!force_save &&
             (phdr.type != AP_PARAM_INT32 &&
@@ -1052,14 +1059,14 @@ bool AP_Param::save(bool force_save)
             // for other than 32 bit integers, we accept values within
             // 0.01 percent of the current value as being the same
             gcs().send_parameter_value(name, (enum ap_var_type)info->type, v2);
-            return true;
+            return;
         }
     }
 
     if (ofs+type_size((enum ap_var_type)phdr.type)+2*sizeof(phdr) >= _storage.size()) {
         // we are out of room for saving variables
         hal.console->printf("EEPROM full\n");
-        return false;
+        return;
     }
 
     // write a new sentinal, then the data, then the header
@@ -1068,7 +1075,50 @@ bool AP_Param::save(bool force_save)
     eeprom_write_check(&phdr, ofs, sizeof(phdr));
 
     send_parameter(name, (enum ap_var_type)phdr.type, idx);
-    return true;
+}
+
+/*
+  put variable into queue to be saved
+*/
+void AP_Param::save(bool force_save)
+{
+    struct param_save p;
+    p.param = this;
+    p.force_save = force_save;
+    while (!save_queue.push(p)) {
+        // if we can't save to the queue
+        if (hal.util->get_soft_armed()) {
+            // if we are armed then don't sleep, instead we lose the
+            // parameter save
+            return;
+        }
+        // when we are disarmed then loop waiting for a slot to become
+        // available. This guarantees completion for large parameter
+        // set loads
+        hal.scheduler->delay_microseconds(500);
+    }
+}
+
+/*
+  background function for saving parameters. This runs on the IO thread
+ */
+void AP_Param::save_io_handler(void)
+{
+    struct param_save p;
+    while (save_queue.pop(p)) {
+        p.param->save_sync(p.force_save);
+    }
+}
+
+/*
+  wait for all parameters to save
+*/
+void AP_Param::flush(void)
+{
+    uint16_t counter = 200; // 2 seconds max
+    while (counter-- && save_queue.available()) {
+        hal.scheduler->delay(10);
+    }
 }
 
 // Load the variable from EEPROM, if supported
@@ -1276,6 +1326,11 @@ bool AP_Param::load_all()
 
     reload_defaults_file(false);
 
+    if (!registered_save_handler) {
+        registered_save_handler = true;
+        hal.scheduler->register_io_process(FUNCTOR_BIND((&save_dummy), &AP_Param::save_io_handler, void));
+    }
+    
     while (ofs < _storage.size()) {
         _storage.read_block(&phdr, ofs, sizeof(phdr));
         // note that this is an || not an && for robustness

libraries/AP_Param/AP_Param.h

@@ -25,6 +25,7 @@
 #include <cmath>
 
 #include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/utility/RingBuffer.h>
 #include <StorageManager/StorageManager.h>
 
 #include "float.h"
@@ -301,14 +302,24 @@ public:
     ///
     void notify() const;
     
-    /// Save the current value of the variable to EEPROM.
+    /// Save the current value of the variable to storage, synchronous API
     ///
     /// @param  force_save     If true then force save even if default
     ///
     /// @return                True if the variable was saved successfully.
     ///
-    bool save(bool force_save=false);
+    void save_sync(bool force_save=false);
 
+    /// flush all pending parameter saves
+    /// used on reboot
+    static void flush(void);
+    
+    /// Save the current value of the variable to storage, async interface
+    ///
+    /// @param  force_save     If true then force save even if default
+    ///
+    void save(bool force_save=false);
+    
     /// Load the variable from EEPROM.
     ///
     /// @return                True if the variable was loaded successfully.
@@ -608,6 +619,18 @@ private:
     static const uint8_t        k_EEPROM_revision    = 6; ///< current format revision
 
     static bool _hide_disabled_groups;
+
+    // support for background saving of parameters. We pack it to reduce memory for the
+    // queue
+    struct PACKED param_save {
+        AP_Param *param;
+        bool force_save;
+    };
+    static ObjectBuffer<struct param_save> save_queue;
+    static bool registered_save_handler;
+
+    // background function for saving parameters
+    void save_io_handler(void);
 };
 
 /// Template class for scalar variables.
@@ -660,10 +683,10 @@ public:
 
     /// Combined set and save
     ///
-    bool set_and_save(const T &v) {
+    void set_and_save(const T &v) {
         bool force = fabsf((float)(_value - v)) < FLT_EPSILON;
         set(v);
-        return save(force);
+        save(force);
     }
 
     /// Combined set and save, but only does the save if the value if
@@ -671,12 +694,12 @@ public:
     /// scan(). This should only be used where we have not set() the
     /// value separately, as otherwise the value in EEPROM won't be
     /// updated correctly.
-    bool set_and_save_ifchanged(const T &v) {
+    void set_and_save_ifchanged(const T &v) {
         if (v == _value) {
-            return true;
+            return;
         }
         set(v);
-        return save(true);
+        save(true);
     }
 
     /// Conversion to T returns a reference to the value.
@@ -765,10 +788,10 @@ public:
 
     /// Combined set and save
     ///
-    bool set_and_save(const T &v) {
+    void set_and_save(const T &v) {
         bool force = (_value != v);
         set(v);
-        return save(force);
+        save(force);
     }
 
     /// Conversion to T returns a reference to the value.