From bc614de4b30fc5744a90a54f195415261af5e1b4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 10 Nov 2016 17:14:17 +1100 Subject: [PATCH] AP_HAL: added checked registers interface allow drivers to easily monitor and correct critical registers --- libraries/AP_HAL/Device.cpp | 102 ++++++++++++++++++++++++++++++++++++ libraries/AP_HAL/Device.h | 40 +++++++++++++- 2 files changed, 140 insertions(+), 2 deletions(-) create mode 100644 libraries/AP_HAL/Device.cpp diff --git a/libraries/AP_HAL/Device.cpp b/libraries/AP_HAL/Device.cpp new file mode 100644 index 0000000000..6b0533ed5d --- /dev/null +++ b/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 . + */ + +#include "Device.h" +#include + +/* + 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 ® = _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; +} diff --git a/libraries/AP_HAL/Device.h b/libraries/AP_HAL/Device.h index aa17cde208..40e7dd7f9a 100644 --- a/libraries/AP_HAL/Device.h +++ b/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; };