Hi, I hope you are doing well, and I am really sorry for the delay in posting this part, I was a little lost and busy with other work. But I will give my best finishing this project and making it reliable.
A Quick Recap
I was working on project that uses data from multiple resources like breathing data from the radar module, heart beat rate , blood oxygen level to detect the stage of sleep you are in to help you wake up at the right time so you wouldn`t feel tired after you wake up, because wake you up at the right time.
These are the past posts:
Palno & Slemon #1 Introduction
Palno & Slemon #2.1 Configuring and Validating
Palno & Slemon #2.2 Configuring and Validating
Algorithm
- In the radar module
- In the heart rate and blood oxygen level sensor
First, radar module. I have thought of using a model to detect breath but I wasn't successful as it gave me terrible results, as I will show you soon.
We already have the algorithm from the resources provided by the analog devices but I would like to explain how it works.
Firstly I had got accuracy pretty well 50%, not that pretty well but better than 0%. I know, 50% was not that usable though. I thought after doing some more improvements I will really improve it but that wasn't the case. I even distorted the 50% and fell into 0%. Absolutely terrible.
This is how I lean towards the algorithm, with the stress that I had to it finish in 2 days.
3 30 100 30 0 100 100 34 10 9 10 2 11 8 100 100 80 41 41 28 1 1 3 53 100 30 0 100 100 100 11 9 2 5 8 5 100 100 80 42 42 29 1 1 3 66 100 53 0 100 100 100 19 10 8 11 9 13 100 100 82 44 45 31 1 1 3 72 100 66 0 100 100 100 33 8 10 11 8 13 100 100 85 48 48 34 1 1 3 73 100 72 31 99 100 53 19 13 9 8 9 7 100 100 89 52 51 38 1 1 3 75 100 73 34 36 100 31 2 10 12 8 9 8 100 100 92 55 54 41 1 1 3 67 79 75 36 35 79 14 3 5 8 10 9 7 100 100 95 58 56 44 1 1 3 71 55 67 38 19 55 11 6 3 6 9 8 8 100 100 98 61 58 47 1 1 3 41 94 71 37 90 94 9 10 1 3 8 5 7 100 100 99 64 61 49 1 1 3 72 100 41 40 42 100 18 9 3 8 6 5 9 100 100 100 67 63 52 1 1 3 62 100 72 40 100 100 18 7 3 10 4 3 10 100 100 100 70 65 54 1 1 3 66 100 62 41 57 100 16 9 2 9 4 2 9 100 100 100 73 68 57 1 1 3 62 62 66 41 37 62 12 9 3 6 4 2 8 100 100 100 75 70 59 1 1 3 63 100 62 41 79 100 21 4 4 4 5 3 4 100 100 100 78 72 61 1 1
#define LD2410_BAUD 256000
#define RX_PIN 16
#define TX_PIN 17
#define MAX_GATES 9
// Frame parser state: header -> length -> payload -> footer
enum { ST_HDR, ST_LEN, ST_DATA, ST_FOOT } rxSt = ST_HDR;
void setup() {
Serial.begin(115200);
Serial2.begin(LD2410_BAUD, SERIAL_8N1, RX_PIN, TX_PIN);
// Engineering mode is enabled later by sending 'n'
}
// Non-blocking: consumes whatever bytes are waiting on UART2
static void readRadar() {
while (Serial2.available()) {
uint8_t b = Serial2.read();
switch (rxSt) {
case ST_HDR: /* slide a 4-byte window until F4 F3 F2 F1 (data)
or FD FC FB FA (command ACK) matches */ break;
case ST_LEN: /* read 2-byte little-endian payload length */ break;
case ST_DATA: /* collect 'length' payload bytes */ break;
case ST_FOOT: /* collect F8 F7 F6 F5, then parseData() */ break;
}
}
}
// Decode one engineering-mode frame (d points at the payload)
static void parseData() {
const uint8_t* d = fb + 6;
if (d[1] != 0xAA) return; // data marker
g_state = d[2];
g_movDist = d[3] | (d[4] << 8); // cm
g_movEn = d[5];
g_staDist = d[6] | (d[7] << 8); // cm
g_detDist = d[9] | (d[10] << 8); // cm
if (d[0] == 0x01) { // engineering frame
uint8_t idx = 13;
for (uint8_t i = 0; i <= g_maxMov; i++) g_movG[i] = d[idx++]; // MG0..MG8
for (uint8_t i = 0; i <= g_maxSta; i++) g_staG[i] = d[idx++]; // SG0..SG8
g_light = d[idx++];
g_out = d[idx];
}
newData = true;
}
// One row per frame, 20 Hz
static void outputLine() {
Serial.print(g_state); Serial.print('\t');
Serial.print(g_movDist); Serial.print('\t');
Serial.print(g_movEn); Serial.print('\t');
Serial.print(g_staDist); Serial.print('\t');
Serial.print(g_detDist); Serial.print('\t');
for (uint8_t i = 0; i < MAX_GATES; i++) { Serial.print(g_movG[i]); Serial.print('\t'); }
for (uint8_t i = 3; i < MAX_GATES; i++) { Serial.print(g_staG[i]); Serial.print('\t'); }
Serial.print(g_light); Serial.print('\t');
Serial.println(g_out);
}
"""
LD2410 Real-Time mmWave Monitor & Breath Counter (Algorithm 1)
=============================================================
Connects to ESP32 over serial, prompts the user to send 'n' to
start streaming, verifies incoming radar data, opens a fast real-time
graph window, and runs the breath counting algorithm (Algorithm 1):
Algorithm 1 Logic:
1. Trigger / Arming: Waits until moving energy is >= 100 continuously
for the first 3 seconds (TRIGGER_DURATION_SEC = 3.0s).
2. Breath Counting: Once armed, counts breath pulses when non-zero peaks
are separated by >= 25 consecutive zero values (energy <= baseline).
3. Loop & Stop: Loops and increments breath count until moving energy
reaches constant 100 for 3 seconds (STOP_DURATION_SEC = 3.0s).
When constant 100 for 3.0s is met, the session concludes.
If moving energy >= 100 for 3.0s occurs again, it re-arms and loops.
Requirements:
pip install pyserial matplotlib numpy
Usage:
python ld2410_plotter.py # auto-detect port
python ld2410_plotter.py COM3 # specific port
python ld2410_plotter.py COM3 --auto-start # send 'n' without prompt
Controls in Plot Window:
[+] / [=] / [UP] : Increase Breath Baseline noise floor live (+1)
[-] / [_] / [DOWN]: Decrease Breath Baseline noise floor live (-1)
[Mouse Drag] : Drag the 'Baseline (Noise Floor)' slider at the bottom
[b] : Reset / Re-arm Breath Counter
[n] : Send 'n' (Start / Resume streaming)
[x] : Send 'x' (Pause streaming)
[1] - [5] : Set sensitivity presets (1=5/5 ... 5=75/75)
[v] : Read firmware version
[p] : Read radar parameters
[q] or [ESC] : Quit
"""
import sys
import os
import time
import argparse
import threading
import collections
import serial
import serial.tools.list_ports
import numpy as np
import matplotlib
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from matplotlib.gridspec import GridSpec
from matplotlib.widgets import Slider
# ─────────────────────── USER CONFIGURATION (ALTER VARIABLES HERE) ────────────────────────
# You can freely alter any of these variables directly to adjust radar and breath detection behavior:
# ── 1. Breath Detection & Baseline Tuning ──
SAMPLE_BASELINE = 15.0 # Breath detection noise floor / baseline (Moving Energy 0-100).
# Values <= SAMPLE_BASELINE are treated as silence/pause (zeros);
# Values > SAMPLE_BASELINE are detected as breathing motion peaks.
# NOTE: You can also adjust this LIVE in the plot window via
# the on-screen slider or [+] / [-] / [UP] / [DOWN] keys!
MIN_ZERO_GAP = 15.0
MIN_ZERO_GAP = SAMPLE_BASELINE
# Minimum consecutive zero values required between peaks (default: 25).
# Difference between two peaks is > 25 and all 25 or more values
# in between are zero (<= SAMPLE_BASELINE).
# ── 2. Trigger & Stop Durations (3s at 100 Trigger & Exit Loop) ──
TRIGGER_ENERGY_REQ = 100.0 # Energy required to arm (moving energy not less than 100).
TRIGGER_DURATION_SEC = 3.0 # Moving energy must be >= 100 for first 3 seconds to start counting.
STOP_DURATION_SEC = 3.0 # Loops until moving energy reaches constant 100 for 3 seconds to stop.
# ── 3. Radar & Display Window Settings ──
BAUD_RATE = 115200 # Serial communication baud rate (ESP32 defaults to 115200).
HISTORY = 200 # Rolling waveform buffer size (200 samples @ 20Hz = 10.0 seconds).
UPDATE_MS = 40 # Matplotlib GUI refresh interval in milliseconds (~25 FPS).
DARK_BG = True # True: Dark mode (#111111); False: Light mode.
GATE_DIST_M = 0.75 # Radar gate resolution in metres (0.75m per gate).
MAX_GATES = 9 # Total number of distance gates (0 to 8).
# Column indices (0-based) for the 22 columns received from ESP32
COL_STATE = 0
COL_MOVDIST = 1
COL_MOVEN = 2
COL_STADIST = 3
COL_DETDIST = 4
COL_MG_START = 5 # MG0..MG8 → indices 5..13
COL_SG_START = 14 # SG3..SG8 → indices 14..19
COL_LIGHT = 20
COL_OUTPIN = 21
TOTAL_COLS = 22
# ─────────────────────── Serial auto-detect ───────────────────
def find_esp32_port():
"""Return the most likely ESP32 COM port, or None."""
ESP_VIDS = {0x10C4, 0x1A86, 0x0403} # CP2102, CH340, FTDI
ports = serial.tools.list_ports.comports()
for p in ports:
if p.vid in ESP_VIDS:
return p.device
if ports:
return ports[0].device
return None
# ─────────────────────── Breath Counter (Algorithm 1) ───────────────────────
class BreathCounter:
"""
Real-Time Breath Counter Algorithm (Algorithm 1):
1. Arming / Trigger:
- Requires Moving Energy >= 100 for the first 3.0 seconds to start counting.
2. Peak & Zero-Crossing Detection:
- Once armed, increments breath count when two non-zero peaks are separated
by >= 25 consecutive zero values (moving energy <= SAMPLE_BASELINE).
3. Loop & Session Stop:
- Loops and continues counting until moving energy reaches constant 100
for 3.0 continuous seconds to stop.
- Re-arms if moving energy >= 100 for 3.0s occurs again.
"""
STATE_WAITING = "WAITING_TRIGGER" # Waiting for >= 100 for 3s
STATE_ACTIVE = "COUNTING" # Active breath counting session
STATE_STOPPED = "STOPPED" # Stopped after constant 100 for 3s
def __init__(self,
trigger_duration=TRIGGER_DURATION_SEC,
stop_duration=STOP_DURATION_SEC,
min_zero_gap=MIN_ZERO_GAP,
min_trigger_energy=TRIGGER_ENERGY_REQ,
peak_threshold=SAMPLE_BASELINE):
self.trigger_duration = float(trigger_duration)
self.stop_duration = float(stop_duration)
self.min_zero_gap = int(min_zero_gap)
self.min_trigger_energy = float(min_trigger_energy)
self.peak_threshold = float(peak_threshold)
self.state = self.STATE_WAITING
self.breath_count = 0
self.zero_count = 0
self.in_breath = False
self.has_peak = False
# Timers & arming flags
self.start_100_time = None
self.stop_100_time = None
self.waiting_to_clear_trigger = False
# Progress tracking
self.trigger_progress = 0.0
self.stop_progress = 0.0
# Diagnostics & BPM
self.last_breath_time = None
self.last_peak_val = 0.0
self.last_zero_interval = 0
self.bpm = 0.0
def reset(self):
"""Reset counter back to waiting for trigger."""
self.state = self.STATE_WAITING
self.breath_count = 0
self.zero_count = 0
self.in_breath = False
self.has_peak = False
self.start_100_time = None
self.stop_100_time = None
self.waiting_to_clear_trigger = False
self.trigger_progress = 0.0
self.stop_progress = 0.0
self.last_breath_time = None
self.last_peak_val = 0.0
self.last_zero_interval = 0
self.bpm = 0.0
print(f"\n[BREATH] Counter reset. Waiting for Moving Energy >= {self.min_trigger_energy:.0f} for {self.trigger_duration:.1f}s...\n")
def process_sample(self, moving_energy: float, timestamp: float = None) -> bool:
"""
Process incoming radar moving energy sample.
Returns True if a new breath peak was detected on this sample, False otherwise.
"""
if timestamp is None:
timestamp = time.time()
is_breath = False
# ── State 1: WAITING_TRIGGER ──
if self.state == self.STATE_WAITING:
if moving_energy >= self.min_trigger_energy:
if self.start_100_time is None:
self.start_100_time = timestamp
elapsed = timestamp - self.start_100_time
self.trigger_progress = min(1.0, elapsed / self.trigger_duration)
if elapsed >= (self.trigger_duration - 1e-4):
self.state = self.STATE_ACTIVE
self.breath_count = 0
self.zero_count = 0
self.has_peak = True
self.waiting_to_clear_trigger = True
self.start_100_time = None
self.trigger_progress = 1.0
print(f"\n[BREATH] >>> TRIGGER MET! Moving Energy was >= {self.min_trigger_energy:.0f} for {self.trigger_duration:.1f}s. Breath counting ACTIVE! <<<\n")
else:
self.start_100_time = None
self.trigger_progress = 0.0
# ── State 2: COUNTING ──
elif self.state == self.STATE_ACTIVE:
# Check stop condition: constant 100 for stop_duration (3.0s)
if moving_energy >= self.min_trigger_energy:
if not self.waiting_to_clear_trigger:
if self.stop_100_time is None:
self.stop_100_time = timestamp
elapsed = timestamp - self.stop_100_time
self.stop_progress = min(1.0, elapsed / self.stop_duration)
if elapsed >= (self.stop_duration - 1e-4):
self.state = self.STATE_STOPPED
self.stop_progress = 1.0
print(f"\n[BREATH] >>> SESSION COMPLETED! Constant {self.min_trigger_energy:.0f} for {self.stop_duration:.1f}s reached. Final Breath Count: {self.breath_count} <<<\n")
return False
# While energy is saturated at 100, do not count as a breath peak
self.zero_count = 0
return False
else:
self.waiting_to_clear_trigger = False
self.stop_100_time = None
self.stop_progress = 0.0
# Peak & Zero-crossing detection based on baseline
if moving_energy > self.peak_threshold:
if not self.in_breath:
# Check if preceded by >= 25 consecutive zero values
if self.zero_count >= self.min_zero_gap:
self.breath_count += 1
self.in_breath = True
self.last_zero_interval = self.zero_count
self.last_peak_val = moving_energy
if self.last_breath_time is not None:
interval = timestamp - self.last_breath_time
if 1.0 <= interval <= 15.0:
self.bpm = 60.0 / interval
self.last_breath_time = timestamp
self.zero_count = 0
is_breath = True
bpm_str = f" ({self.bpm:.0f} BPM)" if self.bpm > 0 else ""
print(f"[BREATH] +1 Breath #{self.breath_count} detected!{bpm_str} (Zeros in between: {self.last_zero_interval}, Energy: {moving_energy:.0f})")
else:
# Continuing current breath or sub-pulse
self.zero_count = 0
self.last_peak_val = max(self.last_peak_val, moving_energy)
else:
self.zero_count = 0
self.last_peak_val = max(self.last_peak_val, moving_energy)
else:
# Value is in the baseline noise floor (<= peak_threshold)
self.zero_count += 1
if self.zero_count >= self.min_zero_gap:
self.in_breath = False
# ── State 3: STOPPED ──
elif self.state == self.STATE_STOPPED:
# Re-arm loop if moving energy >= 100 for 3s again
if moving_energy < self.min_trigger_energy:
self.start_100_time = None
self.trigger_progress = 0.0
else:
if self.start_100_time is None:
self.start_100_time = timestamp
elapsed = timestamp - self.start_100_time
if elapsed >= (self.trigger_duration - 1e-4):
print(f"\n[BREATH] >>> RE-TRIGGER MET! Starting new breath counting session... <<<\n")
self.state = self.STATE_ACTIVE
self.breath_count = 0
self.zero_count = 0
self.has_peak = True
self.waiting_to_clear_trigger = True
self.start_100_time = None
self.stop_100_time = None
return is_breath
def get_status_summary(self):
return {
"state": self.state,
"count": self.breath_count,
"zero_count": self.zero_count,
"last_interval": self.last_zero_interval,
"trigger_progress": self.trigger_progress,
"stop_progress": self.stop_progress,
"bpm": self.bpm,
"peak_threshold": self.peak_threshold
}
# ─────────────────────── Data thread ──────────────────────────
class RadarReader(threading.Thread):
"""Reads serial lines in background and feeds data to thread-safe buffer."""
def __init__(self, ser, breath_counter=None):
super().__init__(daemon=True)
self.ser = ser
self.buf = collections.deque(maxlen=HISTORY)
self.breath_flags = collections.deque(maxlen=HISTORY)
self.lock = threading.Lock()
self.status = "Streaming"
self.connected = True
self._stop = False
self.latest = [0.0] * TOTAL_COLS
self.packet_cnt = 0
self.breath_counter = breath_counter or BreathCounter()
def stop(self):
self._stop = True
def send(self, cmd: str):
"""Send a single character command to ESP32."""
if self.ser and self.ser.is_open:
try:
self.ser.write((cmd + '\n').encode('ascii'))
self.ser.flush()
except Exception as e:
print(f"[SERIAL] Write error: {e}")
def run(self):
while not self._stop and self.ser and self.ser.is_open:
try:
raw = self.ser.readline()
if not raw:
continue
line = raw.decode('ascii', errors='ignore').strip()
if not line or line.startswith('#'):
if line.startswith('#'):
# Display firmware / parameters feedback
if any(k in line for k in ("FW", "Params", "Sens", "MaxDist", "STREAMING")):
print(f"[ESP32] {line}")
continue
# Data line starts with numeric State (0, 1, 2, 3)
if not line[0].isdigit():
continue
sep = ',' if ',' in line else '\t'
parts = line.split(sep)
if len(parts) != TOTAL_COLS:
continue
row = [float(p) for p in parts]
is_breath = self.breath_counter.process_sample(row[COL_MOVEN])
with self.lock:
self.buf.append(row)
self.breath_flags.append(is_breath)
self.latest = row
self.packet_cnt += 1
except serial.SerialException as e:
self.status = f"Serial error: {e}"
self.connected = False
break
except Exception as e:
self.status = f"Error: {e}"
# ─────────────────────── Plot Setup ───────────────────────────
STATE_LABELS = {0: "No Target", 1: "Moving", 2: "Static", 3: "Both"}
STATE_COLORS = {0: '#555555', 1: '#00ccff', 2: '#ffaa00', 3: '#ff4444'}
if DARK_BG:
plt.style.use('dark_background')
fig = plt.figure(figsize=(15, 9.5), facecolor='#111111' if DARK_BG else '#f5f5f5')
fig.canvas.manager.set_window_title('HLK-LD2410 Real-Time mmWave Monitor & Breath Counter')
gs = GridSpec(4, 2, figure=fig,
left=0.06, right=0.97,
top=0.92, bottom=0.075,
hspace=0.52, wspace=0.30)
ax_dist = fig.add_subplot(gs[0, :]) # distances — full width
ax_moven = fig.add_subplot(gs[1, :]) # moving energy + target state + breath
ax_movg = fig.add_subplot(gs[2, :]) # per-gate moving energies MG0–MG8
ax_stag = fig.add_subplot(gs[3, 0]) # static gate energies SG3–SG8
ax_misc = fig.add_subplot(gs[3, 1]) # Light + Output pin
def style_ax(ax, title, ylabel='', ylim=None):
ax.set_title(title, fontsize=9.5, pad=3, fontweight='bold',
color='#dddddd' if DARK_BG else '#222222')
ax.set_ylabel(ylabel, fontsize=8)
ax.tick_params(labelsize=7.5)
ax.grid(True, alpha=0.25, linestyle=':')
if ylim: ax.set_ylim(ylim)
style_ax(ax_dist, 'Distance Tracking (cm)', 'cm', (0, 700))
style_ax(ax_moven, 'Motion Energy (0-100) & Breath Detection (Algorithm 1)', 'Level', (0, 105))
style_ax(ax_movg, 'Moving Gate Energies: Gate 0 to Gate 8 (0.75m steps)', 'Energy', (0, 105))
style_ax(ax_stag, 'Static Gate Energies: Gate 3 to Gate 8', 'Energy', (0, 105))
style_ax(ax_misc, 'Photosensitive Light & OUT Pin Status', 'Value')
x_data = list(range(HISTORY))
# ── Distance plot lines ──
line_movdist, = ax_dist.plot(x_data, [0]*HISTORY, color='#00d4ff', lw=1.5, label='Moving Target Dist')
line_stadist, = ax_dist.plot(x_data, [0]*HISTORY, color='#ffaa00', lw=1.5, label='Stationary Target Dist')
line_detdist, = ax_dist.plot(x_data, [0]*HISTORY, color='#44ff88', lw=1.2, ls='--', label='Detection Range')
ax_dist.legend(fontsize=7.5, loc='upper left', framealpha=0.35)
# ── Motion energy + state lines + breath detections + baseline ──
line_moven, = ax_moven.plot(x_data, [0]*HISTORY, color='#ff3388', lw=1.6, label='Moving Energy')
bar_state = ax_moven.fill_between(x_data, 0, [0]*HISTORY, alpha=0.18, color='#00aaff', label='State Area')
line_baseline = ax_moven.axhline(
y=SAMPLE_BASELINE, color='#ffee00', linestyle='--',
linewidth=1.3, alpha=0.85, label=f'Baseline ({SAMPLE_BASELINE:.0f})', zorder=4
)
line_breaths, = ax_moven.plot([], [], 'o', color='#00ffff', markersize=7.5,
markeredgecolor='#ffffff', markeredgewidth=1.2,
label='Breath Peak Detected', zorder=5)
ax_moven.legend(fontsize=7.5, loc='upper left', framealpha=0.35)
# Breath detection HUD badge on ax_moven
breath_hud = ax_moven.text(
0.985, 0.88, '[Breath] Initializing...',
transform=ax_moven.transAxes, ha='right', va='top',
fontsize=9.0, fontweight='bold',
color='#ffaa00',
bbox=dict(boxstyle='round,pad=0.38', facecolor='#151c28' if DARK_BG else '#ffffff',
edgecolor='#ffaa00', alpha=0.9, lw=1.2)
)
# ── Moving gate energies MG0–MG8 ──
MOVG_COLORS = plt.cm.turbo(np.linspace(0.08, 0.92, MAX_GATES))
movg_lines = []
for i in range(MAX_GATES):
ln, = ax_movg.plot(x_data, [0]*HISTORY,
color=MOVG_COLORS[i], lw=1.2,
label=f'G{i} ({i*GATE_DIST_M:.2f}m)')
movg_lines.append(ln)
ax_movg.legend(fontsize=6.5, loc='upper left', framealpha=0.35, ncol=5)
# ── Static gate energies SG3–SG8 ──
STAG_COLORS = plt.cm.cool(np.linspace(0.15, 0.95, 6))
stag_lines = []
for j, i in enumerate(range(3, 9)):
ln, = ax_stag.plot(x_data, [0]*HISTORY,
color=STAG_COLORS[j], lw=1.2,
label=f'SG{i} ({i*GATE_DIST_M:.2f}m)')
stag_lines.append(ln)
ax_stag.legend(fontsize=6.5, loc='upper left', framealpha=0.35, ncol=3)
# ── Light & OutPin ──
line_light, = ax_misc.plot(x_data, [0]*HISTORY, color='#ffee33', lw=1.3, label='Light Level')
line_outpin, = ax_misc.plot(x_data, [0]*HISTORY, color='#33ffaa', lw=1.6, ls='--', label='OUT Pin (Occupied=100)')
ax_misc.set_ylim(0, 260)
ax_misc.legend(fontsize=7.5, loc='upper left', framealpha=0.35)
# Inset horizontal snapshot bar chart for moving gates
bar_ax = ax_movg.inset_axes([0.91, 0.05, 0.08, 0.90])
bar_ax.set_xlim(0, 105)
bar_ax.set_ylim(-0.5, MAX_GATES - 0.5)
bar_ax.set_yticks(range(MAX_GATES))
bar_ax.set_yticklabels([f'G{i}' for i in range(MAX_GATES)], fontsize=6)
bar_ax.set_xticks([])
bar_ax.set_title('Live G0-8', fontsize=6.5, pad=2, color='#cccccc' if DARK_BG else '#333333')
bar_ax.invert_yaxis()
if DARK_BG: bar_ax.set_facecolor('#1c1c1c')
bars = bar_ax.barh(range(MAX_GATES), [0]*MAX_GATES, color=MOVG_COLORS, height=0.65)
# Top status header text
status_txt = fig.text(0.5, 0.965, 'Connecting…', ha='center', va='top',
fontsize=10, fontweight='bold',
color='#aaffaa' if DARK_BG else '#008800')
# Global background reader
reader = None
# Bottom interactive Slider for live Breath Baseline Noise Floor adjustment
slider_ax = fig.add_axes([0.22, 0.015, 0.56, 0.024], facecolor='#202020' if DARK_BG else '#e8e8e8')
baseline_slider = Slider(
slider_ax, 'Breath Baseline (Noise Floor):',
0.0, 70.0, valinit=SAMPLE_BASELINE, valstep=1.0,
color='#00ffcc',
initcolor='none',
track_color='#141414' if DARK_BG else '#cccccc'
)
baseline_slider.label.set_color('#dddddd' if DARK_BG else '#222222')
baseline_slider.label.set_fontsize(8.5)
baseline_slider.label.set_fontweight('bold')
baseline_slider.valtext.set_color('#00ffcc')
baseline_slider.valtext.set_fontsize(8.5)
baseline_slider.valtext.set_fontweight('bold')
def on_baseline_slider(val):
if reader and reader.breath_counter:
reader.breath_counter.peak_threshold = float(val)
line_baseline.set_ydata([val, val])
line_baseline.set_label(f'Baseline ({val:.0f})')
ax_moven.legend(fontsize=7.5, loc='upper left', framealpha=0.35)
fig.canvas.draw_idle()
baseline_slider.on_changed(on_baseline_slider)
# ─────────────────────── Animation Update ─────────────────────
def update(_frame):
global bar_state
with reader.lock:
data = list(reader.buf)
b_flags = list(reader.breath_flags)
latest = list(reader.latest)
b_status = reader.breath_counter.get_status_summary()
state = int(latest[COL_STATE]) if latest else 0
slabel = STATE_LABELS.get(state, 'Unknown')
b_cnt = b_status['count']
b_st = b_status['state']
# Update top status text
status_txt.set_text(
f"State: {slabel} │ "
f"[Breath] Count: {b_cnt} [{b_st}] │ "
f"Moving: {latest[COL_MOVDIST]:.0f}cm (E:{latest[COL_MOVEN]:.0f}) │ "
f"Static: {latest[COL_STADIST]:.0f}cm │ "
f"Detect: {latest[COL_DETDIST]:.0f}cm │ "
f"OUT: {'■ OCCUPIED' if latest[COL_OUTPIN] else '□ Empty'}"
)
status_txt.set_color(STATE_COLORS.get(state, '#aaaaaa') if reader.connected else '#ff4444')
# Update breath HUD badge on ax_moven
if b_st == BreathCounter.STATE_WAITING:
pct = int(b_status['trigger_progress'] * 100)
breath_hud.set_text(f"[Breath] WAITING TRIGGER (Moving Energy >= 100 for 3s): {pct}%\nBreaths: {b_cnt}")
breath_hud.set_color('#ffaa00')
breath_hud.get_bbox_patch().set_edgecolor('#ffaa00')
elif b_st == BreathCounter.STATE_ACTIVE:
stop_pct = int(b_status['stop_progress'] * 100)
stop_hint = f" │ Stop E=100: {stop_pct}%" if stop_pct > 0 else ""
bpm_str = f" │ Rate: {b_status['bpm']:.0f} BPM" if b_status.get('bpm', 0) > 0 else ""
cur_base = b_status.get('peak_threshold', SAMPLE_BASELINE)
breath_hud.set_text(
f"[Breath] COUNT: {b_cnt}{bpm_str} [ACTIVE]\n"
f"Silence: {b_status['zero_count']}z (req >= 25z) │ Last Gap: {b_status['last_interval']}z │ Base: {cur_base:.0f}{stop_hint}"
)
breath_hud.set_color('#00ffcc')
breath_hud.get_bbox_patch().set_edgecolor('#00ffcc')
elif b_st == BreathCounter.STATE_STOPPED:
breath_hud.set_text(
f"[Breath] SESSION STOPPED (Constant 100 for 3s)\n"
f"FINAL BREATH COUNT: {b_cnt} │ Press [B] to Reset / Re-arm"
)
breath_hud.set_color('#ff5555')
breath_hud.get_bbox_patch().set_edgecolor('#ff5555')
if not data:
return
n = len(data)
arr = np.array(data)
pad = HISTORY - n
def padded(col_idx):
return [0.0]*pad + arr[:, col_idx].tolist()
# 1. Distances with adaptive Y-axis
d_mov = padded(COL_MOVDIST)
d_sta = padded(COL_STADIST)
d_det = padded(COL_DETDIST)
line_movdist.set_ydata(d_mov)
line_stadist.set_ydata(d_sta)
line_detdist.set_ydata(d_det)
max_dist = max(arr[:, [COL_MOVDIST, COL_STADIST, COL_DETDIST]].max(), 50)
ax_dist.set_ylim(0, max(250, max_dist * 1.15))
# 2. Motion Energy & State fill & Breath markers
mov_energies = padded(COL_MOVEN)
line_moven.set_ydata(mov_energies)
bar_state.remove()
state_scaled = [v * 25 for v in padded(COL_STATE)]
bar_state = ax_moven.fill_between(x_data, 0, state_scaled, alpha=0.18, color='#00aaff')
padded_flags = [False] * pad + b_flags
b_indices = [idx for idx, flg in enumerate(padded_flags) if flg]
b_vals = [mov_energies[idx] for idx in b_indices]
line_breaths.set_data(b_indices, b_vals)
# 3. Moving Gate Energies MG0–MG8
for i, ln in enumerate(movg_lines):
ln.set_ydata(padded(COL_MG_START + i))
if n > 0:
snap = arr[-1, COL_MG_START:COL_MG_START+MAX_GATES]
for bar, val in zip(bars, snap):
bar.set_width(val)
# 4. Static Gate Energies SG3–SG8
for j, ln in enumerate(stag_lines):
ln.set_ydata(padded(COL_SG_START + j))
# 5. Light & OUT Pin
line_light.set_ydata(padded(COL_LIGHT))
outpin_scaled = [v * 100 for v in padded(COL_OUTPIN)]
line_outpin.set_ydata(outpin_scaled)
# ─────────────────────── Keyboard Handler ─────────────────────
def on_key(event):
global reader
if not event.key:
return
k = event.key
# Live baseline threshold adjustment
if k in ('+', '=', 'up'):
new_val = min(70.0, baseline_slider.val + 1.0)
baseline_slider.set_val(new_val)
print(f"[BASELINE] Increased to {new_val:.0f}")
return
elif k in ('-', '_', 'down'):
new_val = max(0.0, baseline_slider.val - 1.0)
baseline_slider.set_val(new_val)
print(f"[BASELINE] Decreased to {new_val:.0f}")
return
# Reset / Re-arm breath counter
if k in ('b', 'B'):
reader.breath_counter.reset()
with reader.lock:
reader.breath_flags.clear()
reader.breath_flags.extend([False] * len(reader.buf))
return
# Relay commands directly to ESP32
if k in ('n', 'x', '1', '2', '3', '4', '5', 'v', 'p', 'd', 'H'):
reader.send(k)
print(f"[CMD] Sent '{k}' to ESP32")
elif k in ('q', 'escape'):
print("[EXIT] Closing window...")
plt.close(fig)
fig.canvas.mpl_connect('key_press_event', on_key)
# ─────────────────────── Main Entry ───────────────────────────
def main():
global reader
parser = argparse.ArgumentParser(description='HLK-LD2410 Live Plotter & Breath Counter (Algorithm 1)')
parser.add_argument('port', nargs='?', default=None,
help='Serial port (e.g. COM3 or /dev/ttyUSB0)')
parser.add_argument('--baud', type=int, default=BAUD_RATE,
help=f'Baud rate (default {BAUD_RATE})')
parser.add_argument('--auto-start', action='store_true',
help='Send "n" without waiting for manual console Enter')
parser.add_argument('--trigger-sec', type=float, default=TRIGGER_DURATION_SEC,
help=f'Seconds of moving energy >= 100 to start breath counting (default: {TRIGGER_DURATION_SEC})')
parser.add_argument('--stop-sec', type=float, default=STOP_DURATION_SEC,
help=f'Seconds of constant 100 moving energy to stop breath counting (default: {STOP_DURATION_SEC})')
parser.add_argument('--min-zeros', type=int, default=MIN_ZERO_GAP,
help=f'Minimum consecutive zero values between peaks (default: {MIN_ZERO_GAP})')
parser.add_argument('--peak-thresh', type=float, default=SAMPLE_BASELINE,
help=f'Minimum moving energy noise floor / baseline (default: {SAMPLE_BASELINE})')
args = parser.parse_args()
port = args.port or find_esp32_port()
if not port:
print("ERROR: No serial port detected. Connect the ESP32 or specify port (e.g. COM3).")
print("Available ports:", [p.device for p in serial.tools.list_ports.comports()])
sys.exit(1)
print("\n" + "=" * 65)
print(" HLK-LD2410 mmWave Radar Plotter & Breath Counter (Alg 1)")
print("=" * 65)
print(f"Connecting to {port} @ {args.baud} baud...")
try:
ser = serial.Serial(port, args.baud, timeout=0.5)
except Exception as e:
print(f"Failed to open port {port}: {e}")
sys.exit(1)
# Discard any stale bootloader garbage in serial buffer
time.sleep(1.0)
ser.reset_input_buffer()
# Read and print any initial startup messages from ESP32
deadline = time.time() + 1.5
print("\n[ESP32 Startup Messages]:")
while time.time() < deadline:
if ser.in_waiting:
line = ser.readline().decode('ascii', errors='ignore').strip()
if line:
print(f" {line}")
# Prompt user in console to send 'n'
if not args.auto_start:
print("\n" + "-" * 65)
print(" [READY] The ESP32 is waiting for the start command 'n'.")
print(" Press [ENTER] or type 'n' to begin streaming radar data:")
print("-" * 65)
try:
input(">>> ")
except (KeyboardInterrupt, EOFError):
ser.close()
sys.exit(0)
# Send 'n' to start streaming
print("\nSending 'n' command to ESP32...")
ser.write(b'n\n')
ser.flush()
# Verify that data has actually started flowing
print("Verifying data stream from ESP32...")
data_confirmed = False
verify_deadline = time.time() + 5.0
while time.time() < verify_deadline:
if ser.in_waiting:
line = ser.readline().decode('ascii', errors='ignore').strip()
if line:
if line.startswith('#'):
print(f" [ESP32] {line}")
elif line[0].isdigit():
parts = line.split('\t') if '\t' in line else line.split(',')
if len(parts) == TOTAL_COLS:
data_confirmed = True
print(f" [OK] Valid data frame received ({TOTAL_COLS} columns)!")
break
if not data_confirmed:
print("\n[WARNING] Did not receive valid data frame yet, but launching plot window anyway.")
print("If graph is empty, press 'n' inside the plot window to resend start command.")
else:
print("\n[SUCCESS] Radar streaming active! Launching live graph window...\n")
print("Keyboard shortcuts in Plot Window:")
print(" [+] / [UP] : Increase Breath Baseline Noise Floor (+1)")
print(" [-] / [DOWN] : Decrease Breath Baseline Noise Floor (-1)")
print(" [Slider] : Drag 'Baseline' slider at bottom of window")
print(" [b] : Reset / re-arm Breath Counter")
print(" [n] : Start / Resume streaming")
print(" [x] : Pause streaming")
print(" [1] - [5] : Set sensitivity presets (1=5/5 ... 5=75/75)")
print(" [v] : Read firmware version")
print(" [p] : Read radar parameters")
print(" [q] or [ESC] : Quit\n")
# Sync baseline slider with user args
if baseline_slider.val != args.peak_thresh:
baseline_slider.set_val(args.peak_thresh)
# Initialize BreathCounter (Algorithm 1) and background reader
breath_counter = BreathCounter(
trigger_duration=args.trigger_sec,
stop_duration=args.stop_sec,
min_zero_gap=args.min_zeros,
peak_threshold=args.peak_thresh
)
reader = RadarReader(ser, breath_counter=breath_counter)
reader.start()
# Start matplotlib animation
ani = animation.FuncAnimation(
fig, update,
interval=UPDATE_MS,
blit=False,
cache_frame_data=False
)
try:
plt.show()
finally:
reader.stop()
if ser.is_open:
ser.close()
if __name__ == '__main__':
main()
The clock
The ESP32 prints one row every 50 ms, so the sample rate is 20 Hz and each sample is Δt = 0.05 s. The Python side works in samples, so converting between samples and seconds is the maths you’ll use most:
time = samples × 0.05 s samples = time × 20Step 1: the start signal (arming)
The counter sits in WAITING until E ≥ 100 continuously for 3 s. That’s 3 × 20 = 60 consecutive samples at 100. In practice you wave your hand in front of the sensor, which saturates the energy. If E drops below 100 even once, the timer resets. When the 3 s is met, it enters COUNTING with count = 0. The idea is that you tell the system “I’m now lying still, start counting” without touching the PC.
One detail: right after arming, waiting_to_clear_trigger stays true until E drops below 100. Without it, your starting wave would immediately look like a stop signal.
Step 2: turning energy into silence and motion
This is the core of it. Pick a baseline B (default 15, adjustable with the slider). Every sample is classified as:
E[k] > B → motion
E[k] ≤ B → silenceA running counter z tracks how many silent samples in a row you’ve seen. Any motion sample resets it to 0, and any silent sample adds 1.
Step 3: counting a breath
A new breath is counted when two things are true at once:
E[k] > B (motion just appeared)
z ≥ G (it was preceded by at least G silent samples)G is MIN_ZERO_GAP. After a count, an in_breath flag stays on until a long enough silence passes, so one breath that wobbles up and down doesn’t count twice. In words: a breath is a burst of motion that comes after a proper pause.
Step 4: the rate
When a breath is counted, the code measures the time since the previous one, Δt_b, and ignores it unless 1 s ≤ Δt_b ≤ 15 s:
BPM = 60 / Δt_bFor example, a breath every 4 s gives 60/4 = 15 BPM, which is a normal resting rate. A period of 4 s is 80 samples, so a normal breath is roughly 80 samples long and the silent gap is a fraction of that.
Step 5: the stop signal
Wave again until E ≥ 100 for 3 s and the state flips to STOPPED, freezing the final count. If you hold 100 for 3 s again, it re-arms and starts a fresh session. While E ≥ 100 inside a session, samples are not counted as breaths, so the stop wave isn’t counted as breathing.
The BPM and SPO2 Sensor ( MAXREFDES117)
It is fairly tiny compared to how it looks here.
The process of using this was fairly easy as they provide all the resources and the design files. I just needed to alter some specifications, like the pins for the I2C communication, according to my need and I am also testing this on the ESP32. For simplicity I had removed the other part of the original code so it includes some kind of adafruit, things that I don't need, so I had removed. Also the way it works is quite simple. Firstly there are two LEDs: one is IR and one is red. They emit light and there is also a sensor to detect them. The light that gets reflected back from the object, or your finger, or the skin, by that reflection, we measure the SpO2 and the BPM.
This is the sketch I used with the algorithm. I had just modified it a little bit from the original code I got from the design files.
#include <Arduino.h>
#include "algorithm.h"
#include "max30102.h"
#include <Wire.h>
#define INT_PIN 4
#define LED_PIN 2
uint32_t aun_ir_buffer[100]; //infrared LED sensor data
uint32_t aun_red_buffer[100]; //red LED sensor data
int32_t n_ir_buffer_length; //data length
int32_t n_spo2; //SPO2 value
int8_t ch_spo2_valid; //indicator to show if the SPO2 calculation is valid
int32_t n_heart_rate; //heart rate value
int8_t ch_hr_valid; //indicator to show if the heart rate calculation is valid
uint8_t uch_dummy;
// the setup routine runs once when you press reset:
void setup() {
Wire.begin();
maxim_max30102_reset(); //resets the MAX30102
// initialize serial communication at 115200 bits per second:
Serial.begin(115200);
pinMode(INT_PIN, INPUT); //pin connects to the interrupt output pin of the MAX30102
pinMode(LED_PIN, OUTPUT);
delay(1000);
maxim_max30102_read_reg(REG_INTR_STATUS_1,&uch_dummy); //Reads/clears the interrupt status register
while(Serial.available()==0) //wait until user presses a key
{
Serial.write(27); // ESC command
Serial.print(F("[2J")); // clear screen command
Serial.println(F("ESP32"));
Serial.println(F("Press any key to start conversion"));
delay(1000);
}
uch_dummy=Serial.read();
maxim_max30102_init(); //initialize the MAX30102
}
// the loop routine runs over and over again forever:
void loop() {
int32_t i;
n_ir_buffer_length=100; //buffer length of 100 stores 4 seconds of samples running at 25sps
//read the first 100 samples, and determine the signal range
for(i=0;i<n_ir_buffer_length;i++)
{
while(digitalRead(INT_PIN)==1); //wait until the interrupt pin asserts
maxim_max30102_read_fifo((aun_red_buffer+i), (aun_ir_buffer+i)); //read from MAX30102 FIFO
Serial.print(F("red="));
Serial.print(aun_red_buffer[i], DEC);
Serial.print(F(", ir="));
Serial.println(aun_ir_buffer[i], DEC);
}
//calculate heart rate and SpO2 after first 100 samples (first 4 seconds of samples)
maxim_heart_rate_and_oxygen_saturation(aun_ir_buffer, n_ir_buffer_length, aun_red_buffer, &n_spo2, &ch_spo2_valid, &n_heart_rate, &ch_hr_valid);
//Continuously taking samples from MAX30102. Heart rate and SpO2 are calculated every 1 second
while(1)
{
i=0;
//dumping the first 25 sets of samples in the memory and shift the last 75 sets of samples to the top
for(i=25;i<100;i++)
{
aun_red_buffer[i-25]=aun_red_buffer[i];
aun_ir_buffer[i-25]=aun_ir_buffer[i];
}
//take 25 sets of samples before calculating the heart rate.
for(i=75;i<100;i++)
{
while(digitalRead(INT_PIN)==1);
digitalWrite(LED_PIN, !digitalRead(LED_PIN));
maxim_max30102_read_fifo((aun_red_buffer+i), (aun_ir_buffer+i));
//send samples and calculation result to terminal program through UART
Serial.print(F("red="));
Serial.print(aun_red_buffer[i], DEC);
Serial.print(F(", ir="));
Serial.print(aun_ir_buffer[i], DEC);
Serial.print(F(", HR="));
Serial.print(n_heart_rate, DEC);
Serial.print(F(", HRvalid="));
Serial.print(ch_hr_valid, DEC);
Serial.print(F(", SPO2="));
Serial.print(n_spo2, DEC);
Serial.print(F(", SPO2Valid="));
Serial.println(ch_spo2_valid, DEC);
}
maxim_heart_rate_and_oxygen_saturation(aun_ir_buffer, n_ir_buffer_length, aun_red_buffer, &n_spo2, &ch_spo2_valid, &n_heart_rate, &ch_hr_valid);
}
}
Think of this sketch as a sliding 4-second window that the Maxim algorithm re-analyses every second. The sensor shines red and infrared light into your finger, and the sketch looks at how much light comes back. Each heartbeat pushes a little blood into the finger, which absorbs a little more light, so the signal pulses with your pulse. The code collects that wiggle, and the library function maxim_heart_rate_and_oxygen_saturation() turns it into HR and SpO2. (You called it DS117; the board is the MAXREFDES117#, which uses the MAX30102.)
The clock
The sensor is set up in maxim_max30102_init() to run at 100 samples/s with 4-sample averaging, which gives an effective rate of 25 samples/s. So:
Δt = 1/25 = 0.04 s per sample
buffer = 100 samples = 4 s
slide = 25 samples = 1 sThe while(digitalRead(INT_PIN)==1); line waits for the sensor’s interrupt pin (active low) to say “a new sample is ready”, then maxim_max30102_read_fifo() reads one red and one IR value.
The sliding window
First, 100 samples are collected, which is the 4 s warm-up with no valid reading. After that the loop repeats forever:
- Shift the last 75 samples to the front, dropping the oldest 25.
- Read 25 fresh samples (1 s) into the end.
- Recompute HR and SpO2 on all 100.
Consecutive windows overlap by 75%, so the answer updates every second but is always based on the last 4 s. That is why the numbers feel stable but lag a little.
How the library finds heart rate
Inside the function, the IR signal goes through four steps.
- Remove the DC part and flip the sign, so the signal is centred on zero and the pulses become peaks:
x[k] = -(IR[k] - mean(IR))- Smooth it with a 4-point moving average, so sensor noise doesn’t create fake peaks.
- Find peaks that are above a threshold (the signal mean, clamped to a range of about 30–60) and at least 4 samples apart.
- Average the gaps between peaks:
T = (sum of peak gaps) / (number of peaks − 1) [in samples]
HR = 25 × 60 / T = 1500 / TFor example, if peaks are on average 20 samples apart, that is 0.8 s per beat, so HR = 1500/20 = 75 bpm. With fewer than two peaks it can’t compute a gap and sets HRvalid = 0.
How it finds SpO2
Oxygenated and deoxygenated blood absorb red and IR light differently. For each beat, the function measures the pulsing part (AC) and the steady part (DC) of both channels and forms the “ratio of ratios”:
R = (AC_red / DC_red) / (AC_ir / DC_ir)It takes the median R across the beats in the window and maps it to a percentage with Maxim’s empirical calibration curve, roughly:
SpO2 ≈ −45.06·R² + 30.354·R + 94.845So R ≈ 0.5 gives about 99%, and R ≈ 1 gives about 80%. If R falls outside the plausible range, SpO2Valid = 0. That flag matters for your project: never feed a reading into the sleep-stage logic unless its valid flag is 1.
What gets printed
Every sample prints red, ir, HR, HRvalid, SPO2 and SPO2Valid. Because HR and SpO2 are only recomputed once per second, the same values repeat on 25 consecutive lines, and then they jump. The LED on pin 2 toggles per sample as a “loop is alive” blink.
The Algorithm
It was going to be the most difficult part but it was fairly easy for me, as the developers of this have already given that in the design files. It was going to be the most difficult because we are just getting the IR and red LED readings for the detection of the emitted and reflected-back light. We have just those and we have detected the SpO2 and the BPM from this. It was going to be difficult but thankfully it doesn't.
#include "algorithm.h"
#include "arduino.h"
#if defined(ARDUINO_AVR_UNO)
//Arduino Uno doesn't have enough SRAM to store 100 samples of IR led data and red led data in 32-bit format
//To solve this problem, 16-bit MSB of the sampled data will be truncated. Samples become 16-bit data.
void maxim_heart_rate_and_oxygen_saturation(uint16_t *pun_ir_buffer, int32_t n_ir_buffer_length, uint16_t *pun_red_buffer, int32_t *pn_spo2, int8_t *pch_spo2_valid,
int32_t *pn_heart_rate, int8_t *pch_hr_valid)
#else
void maxim_heart_rate_and_oxygen_saturation(uint32_t *pun_ir_buffer, int32_t n_ir_buffer_length, uint32_t *pun_red_buffer, int32_t *pn_spo2, int8_t *pch_spo2_valid,
int32_t *pn_heart_rate, int8_t *pch_hr_valid)
#endif
/**
* \brief Calculate the heart rate and SpO2 level
* \par Details
* By detecting peaks of PPG cycle and corresponding AC/DC of red/infra-red signal, the an_ratio for the SPO2 is computed.
* Since this algorithm is aiming for Arm M0/M3. formaula for SPO2 did not achieve the accuracy due to register overflow.
* Thus, accurate SPO2 is precalculated and save longo uch_spo2_table[] per each an_ratio.
*
* \param[in] *pun_ir_buffer - IR sensor data buffer
* \param[in] n_ir_buffer_length - IR sensor data buffer length
* \param[in] *pun_red_buffer - Red sensor data buffer
* \param[out] *pn_spo2 - Calculated SpO2 value
* \param[out] *pch_spo2_valid - 1 if the calculated SpO2 value is valid
* \param[out] *pn_heart_rate - Calculated heart rate value
* \param[out] *pch_hr_valid - 1 if the calculated heart rate value is valid
*
* \retval None
*/
{
uint32_t un_ir_mean,un_only_once ;
int32_t k, n_i_ratio_count;
int32_t i, s, m, n_exact_ir_valley_locs_count, n_middle_idx;
int32_t n_th1, n_npks, n_c_min;
int32_t an_ir_valley_locs[15] ;
int32_t n_peak_interval_sum;
int32_t n_y_ac, n_x_ac;
int32_t n_spo2_calc;
int32_t n_y_dc_max, n_x_dc_max;
int32_t n_y_dc_max_idx, n_x_dc_max_idx;
int32_t an_ratio[5], n_ratio_average;
int32_t n_nume, n_denom ;
// calculates DC mean and subtract DC from ir
un_ir_mean =0;
for (k=0 ; k<n_ir_buffer_length ; k++ ) un_ir_mean += pun_ir_buffer[k] ;
un_ir_mean =un_ir_mean/n_ir_buffer_length ;
// remove DC and invert signal so that we can use peak detector as valley detector
for (k=0 ; k<n_ir_buffer_length ; k++ )
an_x[k] = -1*(pun_ir_buffer[k] - un_ir_mean) ;
// 4 pt Moving Average
for(k=0; k< BUFFER_SIZE-MA4_SIZE; k++){
an_x[k]=( an_x[k]+an_x[k+1]+ an_x[k+2]+ an_x[k+3])/(int)4;
}
// calculate threshold
n_th1=0;
for ( k=0 ; k<BUFFER_SIZE ;k++){
n_th1 += an_x[k];
}
n_th1= n_th1/ ( BUFFER_SIZE);
if( n_th1<30) n_th1=30; // min allowed
if( n_th1>60) n_th1=60; // max allowed
for ( k=0 ; k<15;k++) an_ir_valley_locs[k]=0;
// since we flipped signal, we use peak detector as valley detector
maxim_find_peaks( an_ir_valley_locs, &n_npks, an_x, BUFFER_SIZE, n_th1, 4, 15 );//peak_height, peak_distance, max_num_peaks
n_peak_interval_sum =0;
if (n_npks>=2){
for (k=1; k<n_npks; k++) n_peak_interval_sum += (an_ir_valley_locs[k] -an_ir_valley_locs[k -1] ) ;
n_peak_interval_sum =n_peak_interval_sum/(n_npks-1);
*pn_heart_rate =(int32_t)( (FS*60)/ n_peak_interval_sum );
*pch_hr_valid = 1;
}
else {
*pn_heart_rate = -999; // unable to calculate because # of peaks are too small
*pch_hr_valid = 0;
}
// load raw value again for SPO2 calculation : RED(=y) and IR(=X)
for (k=0 ; k<n_ir_buffer_length ; k++ ) {
an_x[k] = pun_ir_buffer[k] ;
an_y[k] = pun_red_buffer[k] ;
}
// find precise min near an_ir_valley_locs
n_exact_ir_valley_locs_count =n_npks;
//using exact_ir_valley_locs , find ir-red DC andir-red AC for SPO2 calibration an_ratio
//finding AC/DC maximum of raw
n_ratio_average =0;
n_i_ratio_count = 0;
for(k=0; k< 5; k++) an_ratio[k]=0;
for (k=0; k< n_exact_ir_valley_locs_count; k++){
if (an_ir_valley_locs[k] > BUFFER_SIZE ){
*pn_spo2 = -999 ; // do not use SPO2 since valley loc is out of range
*pch_spo2_valid = 0;
return;
}
}
// find max between two valley locations
// and use an_ratio betwen AC compoent of Ir & Red and DC compoent of Ir & Red for SPO2
for (k=0; k< n_exact_ir_valley_locs_count-1; k++){
n_y_dc_max= -16777216 ;
n_x_dc_max= -16777216;
if (an_ir_valley_locs[k+1]-an_ir_valley_locs[k] >3){
for (i=an_ir_valley_locs[k]; i< an_ir_valley_locs[k+1]; i++){
if (an_x[i]> n_x_dc_max) {n_x_dc_max =an_x[i]; n_x_dc_max_idx=i;}
if (an_y[i]> n_y_dc_max) {n_y_dc_max =an_y[i]; n_y_dc_max_idx=i;}
}
n_y_ac= (an_y[an_ir_valley_locs[k+1]] - an_y[an_ir_valley_locs[k] ] )*(n_y_dc_max_idx -an_ir_valley_locs[k]); //red
n_y_ac= an_y[an_ir_valley_locs[k]] + n_y_ac/ (an_ir_valley_locs[k+1] - an_ir_valley_locs[k]) ;
n_y_ac= an_y[n_y_dc_max_idx] - n_y_ac; // subracting linear DC compoenents from raw
n_x_ac= (an_x[an_ir_valley_locs[k+1]] - an_x[an_ir_valley_locs[k] ] )*(n_x_dc_max_idx -an_ir_valley_locs[k]); // ir
n_x_ac= an_x[an_ir_valley_locs[k]] + n_x_ac/ (an_ir_valley_locs[k+1] - an_ir_valley_locs[k]);
n_x_ac= an_x[n_y_dc_max_idx] - n_x_ac; // subracting linear DC compoenents from raw
n_nume=( n_y_ac *n_x_dc_max)>>7 ; //prepare X100 to preserve floating value
n_denom= ( n_x_ac *n_y_dc_max)>>7;
if (n_denom>0 && n_i_ratio_count <5 && n_nume != 0)
{
an_ratio[n_i_ratio_count]= (n_nume*100)/n_denom ; //formular is ( n_y_ac *n_x_dc_max) / ( n_x_ac *n_y_dc_max) ;
n_i_ratio_count++;
}
}
}
// choose median value since PPG signal may varies from beat to beat
maxim_sort_ascend(an_ratio, n_i_ratio_count);
n_middle_idx= n_i_ratio_count/2;
if (n_middle_idx >1)
n_ratio_average =( an_ratio[n_middle_idx-1] +an_ratio[n_middle_idx])/2; // use median
else
n_ratio_average = an_ratio[n_middle_idx ];
if( n_ratio_average>2 && n_ratio_average <184){
n_spo2_calc= uch_spo2_table[n_ratio_average] ;
*pn_spo2 = n_spo2_calc ;
*pch_spo2_valid = 1;// float_SPO2 = -45.060*n_ratio_average* n_ratio_average/10000 + 30.354 *n_ratio_average/100 + 94.845 ; // for comparison with table
}
else{
*pn_spo2 = -999 ; // do not use SPO2 since signal an_ratio is out of range
*pch_spo2_valid = 0;
}
}
void maxim_find_peaks( int32_t *pn_locs, int32_t *n_npks, int32_t *pn_x, int32_t n_size, int32_t n_min_height, int32_t n_min_distance, int32_t n_max_num )
/**
* \brief Find peaks
* \par Details
* Find at most MAX_NUM peaks above MIN_HEIGHT separated by at least MIN_DISTANCE
*
* \retval None
*/
{
maxim_peaks_above_min_height( pn_locs, n_npks, pn_x, n_size, n_min_height );
maxim_remove_close_peaks( pn_locs, n_npks, pn_x, n_min_distance );
*n_npks = min( *n_npks, n_max_num );
}
void maxim_peaks_above_min_height( int32_t *pn_locs, int32_t *n_npks, int32_t *pn_x, int32_t n_size, int32_t n_min_height )
/**
* \brief Find peaks above n_min_height
* \par Details
* Find all peaks above MIN_HEIGHT
*
* \retval None
*/
{
int32_t i = 1, n_width;
*n_npks = 0;
while (i < n_size-1){
if (pn_x[i] > n_min_height && pn_x[i] > pn_x[i-1]){ // find left edge of potential peaks
n_width = 1;
while (i+n_width < n_size && pn_x[i] == pn_x[i+n_width]) // find flat peaks
n_width++;
if (pn_x[i] > pn_x[i+n_width] && (*n_npks) < 15 ){ // find right edge of peaks
pn_locs[(*n_npks)++] = i;
// for flat peaks, peak location is left edge
i += n_width+1;
}
else
i += n_width;
}
else
i++;
}
}
void maxim_remove_close_peaks(int32_t *pn_locs, int32_t *pn_npks, int32_t *pn_x, int32_t n_min_distance)
/**
* \brief Remove peaks
* \par Details
* Remove peaks separated by less than MIN_DISTANCE
*
* \retval None
*/
{
int32_t i, j, n_old_npks, n_dist;
/* Order peaks from large to small */
maxim_sort_indices_descend( pn_x, pn_locs, *pn_npks );
for ( i = -1; i < *pn_npks; i++ ){
n_old_npks = *pn_npks;
*pn_npks = i+1;
for ( j = i+1; j < n_old_npks; j++ ){
n_dist = pn_locs[j] - ( i == -1 ? -1 : pn_locs[i] ); // lag-zero peak of autocorr is at index -1
if ( n_dist > n_min_distance || n_dist < -n_min_distance )
pn_locs[(*pn_npks)++] = pn_locs[j];
}
}
// Resort indices int32_to ascending order
maxim_sort_ascend( pn_locs, *pn_npks );
}
void maxim_sort_ascend(int32_t *pn_x, int32_t n_size)
/**
* \brief Sort array
* \par Details
* Sort array in ascending order (insertion sort algorithm)
*
* \retval None
*/
{
int32_t i, j, n_temp;
for (i = 1; i < n_size; i++) {
n_temp = pn_x[i];
for (j = i; j > 0 && n_temp < pn_x[j-1]; j--)
pn_x[j] = pn_x[j-1];
pn_x[j] = n_temp;
}
}
void maxim_sort_indices_descend( int32_t *pn_x, int32_t *pn_indx, int32_t n_size)
/**
* \brief Sort indices
* \par Details
* Sort indices according to descending order (insertion sort algorithm)
*
* \retval None
*/
{
int32_t i, j, n_temp;
for (i = 1; i < n_size; i++) {
n_temp = pn_indx[i];
for (j = i; j > 0 && pn_x[n_temp] > pn_x[pn_indx[j-1]]; j--)
pn_indx[j] = pn_indx[j-1];
pn_indx[j] = n_temp;
}
}
Think of it as detective work on a wobbly line. Each heartbeat makes the finger absorb a little more light, so the IR signal dips once per beat. The code finds those dips and measures the spacing for heart rate. At each beat it also compares how strongly the red and IR light pulse for SpO2.
Step 1: Make the signal easy to read
First it subtracts the average (the DC part) and flips the sign:
x[k] = -(IR[k] - mean(IR))The flip is there because the beat shows up as a dip, and flipping turns the dip into a peak, so one peak finder can do the job. Then a 4-point moving average smooths out sensor fuzz:
x[k] = (x[k] + x[k+1] + x[k+2] + x[k+3]) / 4Step 2: The threshold
th = mean(x), clamped to the range 30 to 60Here is the first correction. I said before that the threshold was “the signal mean, clamped”. That’s true, but we just subtracted the mean, so mean(x) is about 0. The clamp therefore pushes it up to 30 almost every time. In practice the threshold is a fixed 30 ADC counts, and any beat whose swing is smaller is invisible. If your finger pressure is light or the LED current is low, the signal shrinks and beats disappear. That is a common reason for HRvalid = 0.
Step 3: Find the peaks
A sample counts as a peak if it is above th and higher than both neighbours (for a flat top, the left edge counts). Then the code cleans up the list:
- Sort the peaks from tallest to shortest.
- Keep the tallest, and throw away any peak within 4 samples of one already kept.
- Re-sort by position and keep at most 15.
This is the second correction. The spacing rule is “more than 4”, so peaks must be at least 5 samples apart (0.2 s), not 4. That caps the detectable rate at 1500/5 = 300 bpm, which is far above anything real.
Step 4: Heart rate
The average gap between neighbouring peaks is the sum of the gaps divided by N−1. The sum telescopes, so it is just (last peak − first peak) / (N−1), and it is done in integer division. With FS = 25 samples/s:
HR = (25 × 60) / T = 1500 / TFor example, with peaks at samples 5, 25, 46, 66 and 86, the gaps add up to 81, and 81/4 = 20 after rounding down. Then HR = 1500/20 = 75 bpm, although the true average gap is 20.25 samples (74 bpm). That rounding is why the output moves in steps: T = 19, 20, 21 gives 78, 75, 71 bpm. A 4 s window can’t resolve finer than about 3 to 4 bpm.
If fewer than 2 peaks are found, you get HR = -999 and HRvalid = 0. Otherwise the flag is 1. It only means “at least two peaks were found”, and nothing checks that the number is plausible. A noisy window could produce 200 bpm with HRvalid = 1.
Step 5: SpO2
For SpO2 it reloads the raw red and IR values, because the smoothed, flipped signal has lost the amplitude information. Then, for each pair of neighbouring valleys v1 and v2 (more than 3 samples apart):
- DC is the highest raw value between the two valleys, one for red and one for IR.
- AC is how far above the straight line joining the two valleys the top of the pulse sits. At the peak index p, that line is:
base = raw[v1] + (raw[v2] − raw[v1]) × (p − v1) / (v2 − v1)
AC = raw[p] − baseEach beat then gives the “ratio of ratios”:
R = (AC_red / DC_red) / (AC_ir / DC_ir) = (AC_red × DC_ir) / (AC_ir × DC_red)The idea is that you compare how strongly each colour pulses relative to its own brightness. Oxygenated blood absorbs less red light, so more oxygen means a smaller red pulse and a smaller R. The code stores 100·R as an integer and uses >>7 shifts to avoid overflow on small ARM chips.
It keeps up to 5 beat ratios, sorts them, and takes the middle one (or the average of the two around the middle). This acts like a median, so one bad beat can’t wreck the answer.
The final conversion isn’t computed live. It is a lookup, uch_spo2_table[ratio], precomputed from Maxim’s empirical curve:
SpO2 ≈ −45.06·R² + 30.354·R + 94.845R = 0.5 gives about 99%, and R = 1.0 gives about 80%. The result is marked valid when 2 < ratio < 184, which means R between 0.03 and 1.83. That range is very wide, so SPO2Valid = 1 doesn’t mean the number is believable either.
