AP_HAL: added checked registers interface

allow drivers to easily monitor and correct critical registers

libraries/AP_HAL/Device.cpp

@@ -0,0 +1,102 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "Device.h"
+#include <stdio.h>
+
+/*
+  using checked registers allows a device check that a set of critical
+  register values don't change at runtime. This is useful on key
+  sensors (such as IMUs) which may experience brownouts or other
+  issues in flight
+
+  To use register checking call setup_checked_registers() once to
+  allocate the space for the checked register values. The set the
+  checked flag on any write_register() calls that you want protected.
+
+  Periodically (say at 50Hz) you should then call
+  check_next_register(). If that returns false then the sensor has had
+  a corrupted register value. Marking the sensor as unhealthy is
+  approriate. The bad value will be corrected
+ */
+
+/*
+  setup nregs checked registers
+ */
+bool AP_HAL::Device::setup_checked_registers(uint8_t nregs)
+{
+    if (_checked.regs != nullptr) {
+        delete[] _checked.regs;
+        _checked.n_allocated = 0;
+        _checked.n_set = 0;
+        _checked.next = 0;
+    }
+    _checked.regs = new struct checkreg[nregs];
+    if (_checked.regs == nullptr) {
+        return false;
+    }
+    _checked.n_allocated = nregs;
+    return true;
+}
+
+/*
+  set value of one checked register
+ */
+void AP_HAL::Device::set_checked_register(uint8_t reg, uint8_t val)
+{
+    if (_checked.regs == nullptr) {
+        return;
+    }
+    struct checkreg *regs = _checked.regs;
+    for (uint8_t i=0; i<_checked.n_set; i++) {
+        if (regs[i].regnum == reg) {
+            regs[i].value = val;
+            return;
+        }
+    }
+    if (_checked.n_set == _checked.n_allocated) {
+        printf("Not enough checked registers for reg 0x%02x on device 0x%x\n",
+               (unsigned)reg, (unsigned)get_bus_id());
+        return;
+    }
+    regs[_checked.n_set].regnum = reg;
+    regs[_checked.n_set].value = val;
+    _checked.n_set++;
+}
+
+/*
+  check one register value
+ */
+bool AP_HAL::Device::check_next_register(void)
+{
+    if (_checked.n_set == 0) {
+        return true;
+    }
+    struct checkreg &reg = _checked.regs[_checked.next];
+    uint8_t v;
+    if (!read_registers(reg.regnum, &v, 1) || v != reg.value) {
+        // a register has changed value unexpectedly. Try changing it back
+        // and re-check it next time
+#if 0
+        printf("Device 0x%x fixing 0x%02x 0x%02x -> 0x%02x\n",
+               (unsigned)get_bus_id(),
+               (unsigned)reg.regnum, (unsigned)v, (unsigned)reg.value);
+#endif
+        write_register(reg.regnum, reg.value);
+        return false;
+    }
+    _checked.next = (_checked.next+1) % _checked.n_set;
+    return true;
+}

libraries/AP_HAL/Device.h

@@ -62,7 +62,11 @@ public:
     }
     
     
-    virtual ~Device() { }
+    virtual ~Device() {
+        if (_checked.regs != nullptr) {
+            delete[] _checked.regs;
+        }
+    }
 
     /*
      * Set the speed of future transfers. Depending on the bus the speed may
@@ -102,12 +106,31 @@ public:
      *
      * Return: true on a successful transfer, false on failure.
      */
-    bool write_register(uint8_t reg, uint8_t val)
+    bool write_register(uint8_t reg, uint8_t val, bool checked=false)
     {
         uint8_t buf[2] = { reg, val };
+        if (checked) {
+            set_checked_register(reg, val);
+        }
         return transfer(buf, sizeof(buf), nullptr, 0);
     }
 
+    /**
+     * set a value for a checked register
+     */
+    void set_checked_register(uint8_t reg, uint8_t val);
+
+    /**
+     * setup for register value checking
+     */
+    bool setup_checked_registers(uint8_t num_regs);
+
+    /**
+     * check next register value for correctness. Return false if value is incorrect
+     * or register checking has not been setup
+     */
+    bool check_next_register(void);
+    
     /**
      * Wrapper function over #transfer() to read a sequence of bytes from
      * device. No value is written, differently from the #read_registers()
@@ -232,4 +255,17 @@ protected:
     void set_device_bus(uint8_t bus) {
         _bus_id.devid_s.bus = bus;
     }
+
+private:
+    // checked registers
+    struct checkreg {
+        uint8_t regnum;
+        uint8_t value;
+    };
+    struct {
+        uint8_t n_allocated;
+        uint8_t n_set;
+        uint8_t next;
+        struct checkreg *regs;
+    } _checked;
 };