MAX30100_BeatDetector.cpp

00001 /*
00002 Arduino-MAX30100 oximetry / heart rate integrated sensor library
00003 Copyright (C) 2016  OXullo Intersecans <x@brainrapers.org>
00004 This program is free software: you can redistribute it and/or modify
00005 it under the terms of the GNU General Public License as published by
00006 the Free Software Foundation, either version 3 of the License, or
00007 (at your option) any later version.
00008 This program is distributed in the hope that it will be useful,
00009 but WITHOUT ANY WARRANTY; without even the implied warranty of
00010 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00011 GNU General Public License for more details.
00012 You should have received a copy of the GNU General Public License
00013 along with this program.  If not, see <http://www.gnu.org/licenses/>.
00014 */
00015 
00016 #include "MAX30100_BeatDetector.h"
00017 
00018 BeatDetector::BeatDetector() :
00019     state(BEATDETECTOR_STATE_INIT),
00020     threshold(BEATDETECTOR_MIN_THRESHOLD),
00021     beatPeriod(0),
00022     lastMaxValue(0),
00023     tsLastBeat(0)
00024 {
00025     t.start();
00026 }
00027 
00028 bool BeatDetector::addSample(float sample)
00029 {
00030     return checkForBeat(sample);
00031 }
00032 
00033 float BeatDetector::getRate()
00034 {
00035     if (beatPeriod != 0) {
00036         return 1 / beatPeriod * 1000 * 60;
00037     } else {
00038         return 0;
00039     }
00040 }
00041 
00042 float BeatDetector::getCurrentThreshold()
00043 {
00044     return threshold;
00045 }
00046 
00047 bool BeatDetector::checkForBeat(float sample)
00048 {
00049     bool beatDetected = false;
00050 
00051     switch (state) {
00052         case BEATDETECTOR_STATE_INIT:
00053             if (t.read_ms() > BEATDETECTOR_INIT_HOLDOFF) {
00054                 state = BEATDETECTOR_STATE_WAITING;
00055             }
00056             break;
00057 
00058         case BEATDETECTOR_STATE_WAITING:
00059             if (sample > threshold) {
00060                 threshold = std::min((uint32_t)sample, (uint32_t)BEATDETECTOR_MAX_THRESHOLD);
00061                 state = BEATDETECTOR_STATE_FOLLOWING_SLOPE;
00062             }
00063 
00064             // Tracking lost, resetting
00065             if (t.read_ms() > BEATDETECTOR_INVALID_READOUT_DELAY) {
00066                 beatPeriod = 0;
00067                 lastMaxValue = 0;
00068             }
00069 
00070             decreaseThreshold();
00071             break;
00072 
00073         case BEATDETECTOR_STATE_FOLLOWING_SLOPE:
00074             if (sample < threshold) {
00075                 state = BEATDETECTOR_STATE_MAYBE_DETECTED;
00076             } else {
00077                 threshold = std::min((uint32_t)sample, (uint32_t)BEATDETECTOR_MAX_THRESHOLD);
00078             }
00079             break;
00080 
00081         case BEATDETECTOR_STATE_MAYBE_DETECTED:
00082             if (sample + BEATDETECTOR_STEP_RESILIENCY < threshold) {
00083                 // Found a beat
00084                 beatDetected = true;
00085                 lastMaxValue = sample;
00086                 state = BEATDETECTOR_STATE_MASKING;
00087                 float delta = t.read_ms();
00088                 if (delta) {
00089                     beatPeriod = BEATDETECTOR_BPFILTER_ALPHA * delta +
00090                             (1 - BEATDETECTOR_BPFILTER_ALPHA) * beatPeriod;
00091                 }
00092 
00093                 t.reset();
00094             } else {
00095                 state = BEATDETECTOR_STATE_FOLLOWING_SLOPE;
00096             }
00097             break;
00098 
00099         case BEATDETECTOR_STATE_MASKING:
00100             if (t.read_ms() > BEATDETECTOR_MASKING_HOLDOFF) {
00101                 state = BEATDETECTOR_STATE_WAITING;
00102             }
00103             decreaseThreshold();
00104             break;
00105     }
00106 
00107     return beatDetected;
00108 }
00109 
00110 void BeatDetector::decreaseThreshold()
00111 {
00112     // When a valid beat rate readout is present, target the
00113     if (lastMaxValue > 0 && beatPeriod > 0) {
00114         threshold -= lastMaxValue * (1 - BEATDETECTOR_THRESHOLD_FALLOFF_TARGET) /
00115                 (beatPeriod / BEATDETECTOR_SAMPLES_PERIOD);
00116     } else {
00117         // Asymptotic decay
00118         threshold *= BEATDETECTOR_THRESHOLD_DECAY_FACTOR;
00119     }
00120 
00121     if (threshold < BEATDETECTOR_MIN_THRESHOLD) {
00122         threshold = BEATDETECTOR_MIN_THRESHOLD;
00123     }
00124 }