# EyeROV Problem Statement Solution- Embedded System Engineer [Amrith Harijayanthan] ## Problem statement At present, there is a chance of false detection of leaks due to electrical interference in the system. The firmware sends a leak detection alert even when a short pulse (< 1s) appears on the leak sensor pins. In the case of true leaks, the leak sensor signal will remain active for a long period of time (> 5 seconds). Task: - Identify the appropriate functions in the ArduSub firmware that perform the leak detection - Make suitable modifications to the source code so as to ignore short pulses on the leak detection pins and to only trigger a leak signal when the leak pins are active for > 5 seconds. ## Methodology Following are the files that needs to be considered inorder to achieve desired solution * ardupilot/libraries/AP_LeakDetector/AP_LeakDetector_Digital.cpp [digital GPIO read for SOS Leak sensor] * ardupilot/ArduSub/failsafe.cpp [contains function to monitor water leak and trigger action] * ardupilot/libraries/AP_LeakDetector/AP_LeakDetector.cpp ArduSub uses ChibiOS as RTOS and the function *leak_detector.update();* is scheduled to run every 3.3hz. Once this function is called the status is returned by checking Leak sensor and the cooloff time (time that the status is true after a leak detected) is 3s (default as per the source code) so every 10 readings of update is true no matter what the actual reading from the sensor is. In order to solve the **1s short pulse trigger** issue, inside the *AP_LeakDetector::update()* function a conditional block can be implemented to check the readings for 1s and set the status to true only if the leak is detected for 1s or higher else discard. Alternatively at the driver level implement an Input Capture method depending on Clock configuration the pulse width can be measured. **Since 5s pulse is the minimum required pulse duration for a leak to be detected as true , flag should be set only if the pulse duration is 5s or more, any other case the leak wont be detected.Simpler method to implement is to keep track of the time the leak was detected and if the time has elapsed 5s then set the flag and start cooldown** ## Proposed solution ### unmodified function ``` 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; } ``` ### modified to detect leak only if the signal persists for 5s or more adding an extra class member to keep track of initial signal detection and cooloff time. uint32_t _last_zero_detect_ms and uint32_t tcool ``` //ardupilot/libraries/AP_LeakDetector/AP_LeakDetector.h #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(); /* Do not allow copies */ AP_LeakDetector(const AP_LeakDetector &other) = delete; AP_LeakDetector &operator=(const AP_LeakDetector&) = delete; 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; uint32_t _last_zero_detect_ms; // Added by Amrith uint32_t tcool; //cooloff time , Added by Amrith 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 }; ``` the below function is modified to keep track of the time when the pulse turns on and the last time when it was on based on which the _status is set to true or false. For cooldown ``` bool AP_LeakDetector::update() { uint32_t tnow = AP_HAL::millis(); //current time when update is called _status = false; for (int i = 0; i < LEAKDETECTOR_MAX_INSTANCES; i++) { if (_drivers[i] != NULL) { _drivers[i]->read(); if (_state[i].status) { _last_detect_ms = tnow; //set tnow when leak is detected } } } if(!(_last_detect_ms==tnow)) //set zero detect when no leak _last_zero_detect=tnow; if((_last_zero_detect_ms - _last_detect_ms) > 5000)//set _status to true , leak detected { _status = true; t_cool=tnow; } if(tnow - t_cool <= LEAKDETECTOR_COOLDOWN_MS) _status = true; else _status = flase; return _status; } ``` ## Reference * https://github.com/bluerobotics/SOS-Leak-Sensor/blob/master/SOS-Leak-Sensor.pdf * https://bluerobotics.com/store/sensors-sonars-cameras/leak-sensor/sos-leak-sensor/