From 9c8fca02d955a0c8e2a893a86e9d8b5a8ad8103b Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Tue, 7 Feb 2017 14:43:01 -0500 Subject: [PATCH] AP_LeakDetector: New library and analog/digital sensor drivers --- libraries/AP_LeakDetector/AP_LeakDetector.cpp | 106 ++++++++++++++++++ libraries/AP_LeakDetector/AP_LeakDetector.h | 54 +++++++++ .../AP_LeakDetector_Analog.cpp | 21 ++++ .../AP_LeakDetector/AP_LeakDetector_Analog.h | 13 +++ .../AP_LeakDetector_Backend.cpp | 6 + .../AP_LeakDetector/AP_LeakDetector_Backend.h | 15 +++ .../AP_LeakDetector_Digital.cpp | 18 +++ .../AP_LeakDetector/AP_LeakDetector_Digital.h | 9 ++ 8 files changed, 242 insertions(+) create mode 100644 libraries/AP_LeakDetector/AP_LeakDetector.cpp create mode 100644 libraries/AP_LeakDetector/AP_LeakDetector.h create mode 100644 libraries/AP_LeakDetector/AP_LeakDetector_Analog.cpp create mode 100644 libraries/AP_LeakDetector/AP_LeakDetector_Analog.h create mode 100644 libraries/AP_LeakDetector/AP_LeakDetector_Backend.cpp create mode 100644 libraries/AP_LeakDetector/AP_LeakDetector_Backend.h create mode 100644 libraries/AP_LeakDetector/AP_LeakDetector_Digital.cpp create mode 100644 libraries/AP_LeakDetector/AP_LeakDetector_Digital.h diff --git a/libraries/AP_LeakDetector/AP_LeakDetector.cpp b/libraries/AP_LeakDetector/AP_LeakDetector.cpp new file mode 100644 index 0000000000..55fa8a12db --- /dev/null +++ b/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(); +} diff --git a/libraries/AP_LeakDetector/AP_LeakDetector.h b/libraries/AP_LeakDetector/AP_LeakDetector.h new file mode 100644 index 0000000000..23da92f812 --- /dev/null +++ b/libraries/AP_LeakDetector/AP_LeakDetector.h @@ -0,0 +1,54 @@ +#pragma once + +#include + +#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 +}; diff --git a/libraries/AP_LeakDetector/AP_LeakDetector_Analog.cpp b/libraries/AP_LeakDetector/AP_LeakDetector_Analog.cpp new file mode 100644 index 0000000000..36891119bc --- /dev/null +++ b/libraries/AP_LeakDetector/AP_LeakDetector_Analog.cpp @@ -0,0 +1,21 @@ +#include "AP_LeakDetector_Analog.h" +#include + +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; + } +} diff --git a/libraries/AP_LeakDetector/AP_LeakDetector_Analog.h b/libraries/AP_LeakDetector/AP_LeakDetector_Analog.h new file mode 100644 index 0000000000..cd51639e4a --- /dev/null +++ b/libraries/AP_LeakDetector/AP_LeakDetector_Analog.h @@ -0,0 +1,13 @@ +#pragma once + +#include "AP_LeakDetector_Backend.h" +#include + +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; +}; diff --git a/libraries/AP_LeakDetector/AP_LeakDetector_Backend.cpp b/libraries/AP_LeakDetector/AP_LeakDetector_Backend.cpp new file mode 100644 index 0000000000..c720406cee --- /dev/null +++ b/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) +{} diff --git a/libraries/AP_LeakDetector/AP_LeakDetector_Backend.h b/libraries/AP_LeakDetector/AP_LeakDetector_Backend.h new file mode 100644 index 0000000000..0a9bb88921 --- /dev/null +++ b/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; +}; diff --git a/libraries/AP_LeakDetector/AP_LeakDetector_Digital.cpp b/libraries/AP_LeakDetector/AP_LeakDetector_Digital.cpp new file mode 100644 index 0000000000..e32da3fde7 --- /dev/null +++ b/libraries/AP_LeakDetector/AP_LeakDetector_Digital.cpp @@ -0,0 +1,18 @@ +#include "AP_LeakDetector_Digital.h" +#include + +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; + } +} diff --git a/libraries/AP_LeakDetector/AP_LeakDetector_Digital.h b/libraries/AP_LeakDetector/AP_LeakDetector_Digital.h new file mode 100644 index 0000000000..d9721be921 --- /dev/null +++ b/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); +};