HAND
Diff: MAX30100_BeatDetector.cpp
- Revision:
- 0:010b908e2187
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MAX30100_BeatDetector.cpp Fri Nov 25 00:52:54 2016 +0000 @@ -0,0 +1,124 @@ +/* +Arduino-MAX30100 oximetry / heart rate integrated sensor library +Copyright (C) 2016 OXullo Intersecans <x@brainrapers.org> +This program 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 program 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 "MAX30100_BeatDetector.h" + +BeatDetector::BeatDetector() : + state(BEATDETECTOR_STATE_INIT), + threshold(BEATDETECTOR_MIN_THRESHOLD), + beatPeriod(0), + lastMaxValue(0), + tsLastBeat(0) +{ + t.start(); +} + +bool BeatDetector::addSample(float sample) +{ + return checkForBeat(sample); +} + +float BeatDetector::getRate() +{ + if (beatPeriod != 0) { + return 1 / beatPeriod * 1000 * 60; + } else { + return 0; + } +} + +float BeatDetector::getCurrentThreshold() +{ + return threshold; +} + +bool BeatDetector::checkForBeat(float sample) +{ + bool beatDetected = false; + + switch (state) { + case BEATDETECTOR_STATE_INIT: + if (t.read_ms() > BEATDETECTOR_INIT_HOLDOFF) { + state = BEATDETECTOR_STATE_WAITING; + } + break; + + case BEATDETECTOR_STATE_WAITING: + if (sample > threshold) { + threshold = std::min((uint32_t)sample, (uint32_t)BEATDETECTOR_MAX_THRESHOLD); + state = BEATDETECTOR_STATE_FOLLOWING_SLOPE; + } + + // Tracking lost, resetting + if (t.read_ms() > BEATDETECTOR_INVALID_READOUT_DELAY) { + beatPeriod = 0; + lastMaxValue = 0; + } + + decreaseThreshold(); + break; + + case BEATDETECTOR_STATE_FOLLOWING_SLOPE: + if (sample < threshold) { + state = BEATDETECTOR_STATE_MAYBE_DETECTED; + } else { + threshold = std::min((uint32_t)sample, (uint32_t)BEATDETECTOR_MAX_THRESHOLD); + } + break; + + case BEATDETECTOR_STATE_MAYBE_DETECTED: + if (sample + BEATDETECTOR_STEP_RESILIENCY < threshold) { + // Found a beat + beatDetected = true; + lastMaxValue = sample; + state = BEATDETECTOR_STATE_MASKING; + float delta = t.read_ms(); + if (delta) { + beatPeriod = BEATDETECTOR_BPFILTER_ALPHA * delta + + (1 - BEATDETECTOR_BPFILTER_ALPHA) * beatPeriod; + } + + t.reset(); + } else { + state = BEATDETECTOR_STATE_FOLLOWING_SLOPE; + } + break; + + case BEATDETECTOR_STATE_MASKING: + if (t.read_ms() > BEATDETECTOR_MASKING_HOLDOFF) { + state = BEATDETECTOR_STATE_WAITING; + } + decreaseThreshold(); + break; + } + + return beatDetected; +} + +void BeatDetector::decreaseThreshold() +{ + // When a valid beat rate readout is present, target the + if (lastMaxValue > 0 && beatPeriod > 0) { + threshold -= lastMaxValue * (1 - BEATDETECTOR_THRESHOLD_FALLOFF_TARGET) / + (beatPeriod / BEATDETECTOR_SAMPLES_PERIOD); + } else { + // Asymptotic decay + threshold *= BEATDETECTOR_THRESHOLD_DECAY_FACTOR; + } + + if (threshold < BEATDETECTOR_MIN_THRESHOLD) { + threshold = BEATDETECTOR_MIN_THRESHOLD; + } +} \ No newline at end of file