AP_LeakDetector: New library and analog/digital sensor drivers

libraries/AP_LeakDetector/AP_LeakDetector.cpp

@@ -0,0 +1,106 @@
+#include "AP_LeakDetector.h"
+#include "AP_LeakDetector_Analog.h"
+#include "AP_LeakDetector_Digital.h"
+
+const AP_Param::GroupInfo AP_LeakDetector::var_info[] = {
+
+    // @Param: 1_PIN
+    // @DisplayName: Pin that leak detector is connected to
+    // @Description: Pin that the leak detector is connected to
+    // @Values: -1:Disabled,50:Pixhawk Aux1,51:Pixhawk Aux2,52:Pixhawk Aux3,53:Pixhawk Aux4,54:Pixhawk Aux5,55:Pixhawk Aux6,13:Pixhawk 3.3ADC1,14:Pixhawk 3.3ADC2,15:Pixhawk 6.6ADC
+    // @User: Standard
+    AP_GROUPINFO("1_PIN", 1, AP_LeakDetector, _pin[0], -1),
+
+    // @Param: 1_LOGIC
+    // @DisplayName: Default reading of leak detector when dry
+    // @Description: Default reading of leak detector when dry
+    // @Values: 0:Low,1:High
+    // @User: Standard
+    AP_GROUPINFO("1_LOGIC", 2, AP_LeakDetector, _default_reading[0], 0),
+
+#if LEAKDETECTOR_MAX_INSTANCES > 1
+    // @Param: 2_PIN
+    // @DisplayName: Pin that leak detector is connected to
+    // @Description: Pin that the leak detector is connected to
+    // @Values: -1:Disabled,50:Pixhawk Aux1,51:Pixhawk Aux2,52:Pixhawk Aux3,53:Pixhawk Aux4,54:Pixhawk Aux5,55:Pixhawk Aux6,13:Pixhawk 3.3ADC1,14:Pixhawk 3.3ADC2,15:Pixhawk 6.6ADC
+    // @User: Standard
+    AP_GROUPINFO("2_PIN", 3, AP_LeakDetector, _pin[1], -1),
+
+    // @Param: 2_LOGIC
+    // @DisplayName: Default reading of leak detector when dry
+    // @Description: Default reading of leak detector when dry
+    // @Values: 0:Low,1:High
+    // @User: Standard
+    AP_GROUPINFO("2_LOGIC", 4, AP_LeakDetector, _default_reading[1], 0),
+#endif
+
+#if LEAKDETECTOR_MAX_INSTANCES > 2
+    // @Param: 3_PIN
+    // @DisplayName: Pin that leak detector is connected to
+    // @Description: Pin that the leak detector is connected to
+    // @Values: -1:Disabled,50:Pixhawk Aux1,51:Pixhawk Aux2,52:Pixhawk Aux3,53:Pixhawk Aux4,54:Pixhawk Aux5,55:Pixhawk Aux6,13:Pixhawk 3.3ADC1,14:Pixhawk 3.3ADC2,15:Pixhawk 6.6ADC
+    // @User: Standard
+    AP_GROUPINFO("3_PIN", 5, AP_LeakDetector, _pin[2], -1),
+
+    // @Param: 3_LOGIC
+    // @DisplayName: Default reading of leak detector when dry
+    // @Description: Default reading of leak detector when dry
+    // @Values: 0:Low,1:High
+    // @User: Standard
+    AP_GROUPINFO("3_LOGIC", 6, AP_LeakDetector, _default_reading[2], 0),
+#endif
+
+    AP_GROUPEND
+};
+
+AP_LeakDetector::AP_LeakDetector() :
+    _status(false),
+    _last_detect_ms(0)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+
+    memset(_state,0,sizeof(_state));
+    memset(_drivers,0,sizeof(_drivers));
+};
+
+void AP_LeakDetector::init()
+{
+    for (int i = 0; i < LEAKDETECTOR_MAX_INSTANCES; i++) {
+        switch (_pin[i]) {
+        case 50 ... 55:
+            _state[i].instance = i;
+            _drivers[i] = new AP_LeakDetector_Digital(*this, _state[i]);
+            break;
+        case 13 ... 15:
+            _state[i].instance = i;
+            _drivers[i] = new AP_LeakDetector_Analog(*this, _state[i]);
+            break;
+        default:
+            _drivers[i] = NULL;
+            break;
+        }
+    }
+}
+
+bool AP_LeakDetector::update()
+{
+    uint32_t tnow = AP_HAL::millis();
+
+    for (int i = 0; i < LEAKDETECTOR_MAX_INSTANCES; i++) {
+        if (_drivers[i] != NULL) {
+            _drivers[i]->read();
+            if (_state[i].status) {
+                _last_detect_ms = tnow;
+            }
+        }
+    }
+
+    _status = tnow < _last_detect_ms + LEAKDETECTOR_COOLDOWN_MS;
+
+    return _status;
+}
+
+void AP_LeakDetector::set_detect()
+{
+    _last_detect_ms = AP_HAL::millis();
+}

libraries/AP_LeakDetector/AP_LeakDetector.h

@@ -0,0 +1,54 @@
+#pragma once
+
+#include <AP_Param/AP_Param.h>
+
+#define LEAKDETECTOR_MAX_INSTANCES 3
+
+#define LEAKDETECTOR_COOLDOWN_MS 3000 // Status will return true for this long after last time leak was detected
+
+class AP_LeakDetector_Backend;
+
+class AP_LeakDetector {
+
+    friend class AP_LeakDetector_Analog;
+    friend class AP_LeakDetector_Digital;
+
+public:
+    AP_LeakDetector();
+
+    enum LeakDetector_Type {
+        LEAKDETECTOR_TYPE_NONE = 0,
+        LEAKDETECTOR_TYPE_DIGITAL = 1,
+        LEAKDETECTOR_TYPE_ANALOG = 2
+    };
+
+    struct LeakDetector_State {
+        uint8_t instance;
+        bool status; // leaking?
+    };
+
+    // Return current status
+    bool get_status(void) const { return _status; }
+
+    // Set status externally, ie via mavlink message from subsystems
+    void set_detect(void);
+
+    // Initialize all drivers
+    void init(void);
+
+    // Update all drivers
+    bool update(void);
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+    AP_LeakDetector_Backend *_drivers[LEAKDETECTOR_MAX_INSTANCES];
+    LeakDetector_State _state[LEAKDETECTOR_MAX_INSTANCES];
+
+    bool _status; // Current status, true if leak detected, false if all sensors dry
+    uint32_t _last_detect_ms;
+
+    AP_Int8 _type[LEAKDETECTOR_MAX_INSTANCES]; // Analog, Digital, Mavlink
+    AP_Int8 _pin[LEAKDETECTOR_MAX_INSTANCES]; // Pin that detector is connected to
+    AP_Int8 _default_reading[LEAKDETECTOR_MAX_INSTANCES]; // Default reading when leak detector is dry
+};

libraries/AP_LeakDetector/AP_LeakDetector_Analog.cpp

@@ -0,0 +1,21 @@
+#include "AP_LeakDetector_Analog.h"
+#include <AP_HAL/AP_HAL.h>
+
+extern const AP_HAL::HAL& hal;
+
+AP_LeakDetector_Analog::AP_LeakDetector_Analog(AP_LeakDetector &_leak_detector, AP_LeakDetector::LeakDetector_State &_state) :
+    AP_LeakDetector_Backend(_leak_detector, _state)
+{
+    source = hal.analogin->channel(leak_detector._pin[state.instance]);
+}
+
+void AP_LeakDetector_Analog::read()
+{
+    if (source != NULL && leak_detector._pin[state.instance] >= 0) {
+        source->set_pin(leak_detector._pin[state.instance]);
+        state.status = source->voltage_average() > 2.0f;
+        state.status = state.status==leak_detector._default_reading[state.instance]?false:true;
+    } else {
+        state.status = false;
+    }
+}

libraries/AP_LeakDetector/AP_LeakDetector_Analog.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "AP_LeakDetector_Backend.h"
+#include <AP_HAL/AP_HAL.h>
+
+class AP_LeakDetector_Analog : public AP_LeakDetector_Backend {
+public:
+    AP_LeakDetector_Analog(AP_LeakDetector &_leak_detector, AP_LeakDetector::LeakDetector_State &_state);
+    void read(void);
+
+private:
+    AP_HAL::AnalogSource *source;
+};

libraries/AP_LeakDetector/AP_LeakDetector_Backend.cpp

@@ -0,0 +1,6 @@
+#include "AP_LeakDetector_Backend.h"
+
+AP_LeakDetector_Backend::AP_LeakDetector_Backend(AP_LeakDetector &_leak_detector, AP_LeakDetector::LeakDetector_State &_state) :
+    leak_detector(_leak_detector),
+    state(_state)
+{}

libraries/AP_LeakDetector/AP_LeakDetector_Backend.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "AP_LeakDetector.h"
+
+class AP_LeakDetector_Backend {
+public:
+    AP_LeakDetector_Backend(AP_LeakDetector &_leak_detector, AP_LeakDetector::LeakDetector_State &_state);
+
+    // Each backend type must provide an implementation to read the sensor
+    virtual void read(void) = 0;
+
+protected:
+    AP_LeakDetector &leak_detector;
+    AP_LeakDetector::LeakDetector_State &state;
+};

libraries/AP_LeakDetector/AP_LeakDetector_Digital.cpp

@@ -0,0 +1,18 @@
+#include "AP_LeakDetector_Digital.h"
+#include <AP_HAL/AP_HAL.h>
+
+extern const AP_HAL::HAL& hal;
+
+AP_LeakDetector_Digital::AP_LeakDetector_Digital(AP_LeakDetector &_leak_detector, AP_LeakDetector::LeakDetector_State &_state) :
+    AP_LeakDetector_Backend(_leak_detector, _state)
+{}
+
+void AP_LeakDetector_Digital::read()
+{
+    if (leak_detector._pin[state.instance] >= 0) {
+        hal.gpio->pinMode(leak_detector._pin[state.instance], HAL_GPIO_INPUT);
+        state.status = hal.gpio->read(leak_detector._pin[state.instance])==leak_detector._default_reading[state.instance]?false:true;
+    } else {
+        state.status = false;
+    }
+}

libraries/AP_LeakDetector/AP_LeakDetector_Digital.h

@@ -0,0 +1,9 @@
+#pragma once
+
+#include "AP_LeakDetector_Backend.h"
+
+class AP_LeakDetector_Digital : public AP_LeakDetector_Backend {
+public:
+    AP_LeakDetector_Digital(AP_LeakDetector &_leak_detector, AP_LeakDetector::LeakDetector_State &_state);
+    void read(void);
+};