mbed HRM1017 で Heart Rate Profile をサポートしてみる（５）

mbed HRM1017 をいよいよ Heart Rate Monitor にして、スマホとBLEで接続できるようにしてみたいと思います。心拍センサーの出力は前回の実験から10kΩを挟んで計測しています。





Heart Rate 計測のアルゴリズムは PulseSensor.com の内容をmbedに移植しました。


PULSE SENSOR AMPED
http://pulsesensor.com/pages/pulse-sensor-amped-arduino-v1dot1


ときどき値が飛ぶのでまだバグがあるようですが、参考までにソースコードを公開しておきます。

#include "mbed.h"
#include &ltlimits.h>

#ifdef __cplusplus
extern "C" {
#endif // __cplusplus

#define NEED_DEBUG 
#ifdef NEED_DEBUG
#define DEBUG(...) { printf(__VA_ARGS__); }
#else
#define DEBUG(...) /* nothing */
#endif 

#define N 10
#define DEFAULT_THRESHOLD 500
#define DEFAULT_AMP 100

volatile int BPM = 60;
volatile int IBI = 600;

volatile bool Pulse = false;
volatile bool QS = false;

volatile int Rate[N];
volatile int CurrentBeatTime = 0;
volatile int LastBeatTime = 0;

volatile uint16_t Signal;
volatile uint16_t P = DEFAULT_THRESHOLD;
volatile uint16_t T = DEFAULT_THRESHOLD;
volatile uint16_t Threshold = DEFAULT_THRESHOLD;

volatile int Amplifier = DEFAULT_AMP;

Timer timer;
AnalogIn ain(p6);

void initPulseSensor(void) {
    for (int i = 0; i < N; ++i) {
        Rate[i] = 0;
    }
    timer.start();
    LastBeatTime = timer.read_ms();
}

int getBPM(void) {
    return BPM;
}

bool isQS(void) {
    bool qs = QS;
    QS = false;
    return qs;
}


void reset() {
    Threshold = DEFAULT_THRESHOLD;
    Amplifier = DEFAULT_AMP;
    P = T = DEFAULT_THRESHOLD;
}

void calcHeartRate(void) {
    
    Signal = ain.read_u16();
    CurrentBeatTime = timer.read_ms();
    // printf("%d\t%d\r\n", CurrentBeatTime, Signal);
    
    int interval = 0;
    if (CurrentBeatTime < LastBeatTime) {
        interval = INT_MAX - CurrentBeatTime + LastBeatTime;
    } else {
        interval = CurrentBeatTime - LastBeatTime;
    }
    
    if ((Signal < Threshold) && (interval > IBI * 3/5)) {
        if (Signal < T) { 
            T = Signal; // hold bottom
        }
    } else if ((Signal > Threshold) && (Signal > P)) {
        P = Signal; // hold peak 
    }
    
    if (interval > 250 && interval < 2500) { // msec
    
        if ((Signal > Threshold) && !Pulse && (interval > IBI * 3/5)) {
            Pulse = true;
            IBI = interval;
            
            if (Rate[0] < 0) {  // first time
                Rate[0] = 0;
                LastBeatTime = timer.read_ms();
                return;
            } else if (Rate[0] == 0) { // second time
                for (int i = 0; i < N; ++i) {
                    Rate[i] = IBI;
                }
            }
            
            int running_total = 0;
            for (int i = 0; i < N-1; ++i) {
                Rate[i] = Rate[i+1];
                running_total += Rate[i];
            }
            
            Rate[N-1] = IBI;
            running_total += IBI;
            running_total /= N;
            BPM = 60000 / running_total;
            QS = true;
            LastBeatTime = timer.read_ms();
            DEBUG("P:%d T:%d AMP:%d THR:%d BPM:%d\r\n"
                    ,P ,T ,Amplifier ,Threshold ,BPM);
        }
    }
    
    if (Pulse) {
        Pulse = false;
        if (P >= T) {
            Amplifier = P - T;
            Threshold = Amplifier/2 + T;  // update Threshold
            P = T = Threshold;
            DEBUG("Update Threshold:%d\r\n", Threshold);
        } else {
            DEBUG("Error: T(%d) over P(%d)\r\n", T, P);
            reset();
        }
    }
    
    // check if no Signal is over 2.5 sec
    if (interval >= 2500) {
        DEBUG("No Signal over 2.5sec\r\n");
        reset();
        LastBeatTime = timer.read_ms();
        for (int i = 0; i < N; ++i) {
            Rate[i] = -1;
        }
    }
}

#ifdef __cplusplus
}
#endif

メインのプログラムは下記のように変更を加えました。BLEのライブラリはこちらのサンプルで使われているライブラリをアップデートせずに使っています。

#include "mbed.h"
#include "BLE.h"
#include &ltmath.h>

#include "PulseSensor.h"

#define NEED_DEBUG
#ifdef NEED_DEBUG
#define DEBUG(...) { printf(__VA_ARGS__); }
#else
#define DEBUG(...) /* nothing */
#endif 

const static char  DEVICE_NAME[] = "mbed HRM1017";
static volatile bool  triggerSensorPolling = false;

BLEDevice  ble;

/* Heart Rate Service */ 
static uint8_t hrmCounter = 100;
static uint8_t bpm[2] = {0x00, hrmCounter};
GattCharacteristic  hrmChar(
     GattCharacteristic::UUID_HEART_RATE_MEASUREMENT_CHAR
    ,bpm, sizeof(bpm) ,sizeof(bpm)
    ,GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
static uint8_t location = 0x05; /* Ear Lobe */
GattCharacteristic hrmLocation(
     GattCharacteristic::UUID_BODY_SENSOR_LOCATION_CHAR
    ,(uint8_t *)&location ,sizeof(location) ,sizeof(location)
    ,GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);
GattCharacteristic *hrmChars[] = {&hrmChar, &hrmLocation,};
GattService hrmService(GattService::UUID_HEART_RATE_SERVICE
    ,hrmChars ,sizeof(hrmChars)/sizeof(GattCharacteristic *));

/* Battery Level Service */
static uint8_t batt = 100;
GattCharacteristic battLevel(
     GattCharacteristic::UUID_BATTERY_LEVEL_CHAR
    ,(uint8_t *)&batt ,sizeof(batt) ,sizeof(batt)
    ,GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);             
GattCharacteristic *battChars[] = {&battLevel,};
GattService battService(
     GattService::UUID_BATTERY_SERVICE
    ,battChars ,sizeof(battChars)/sizeof(GattCharacteristic *));

/* Device Information service */
static uint8_t deviceName[] = {'H', 'R', 'M', '1', '0', '1', '7'};
GattCharacteristic deviceManufacturer(
     GattCharacteristic::UUID_MANUFACTURER_NAME_STRING_CHAR
    ,(uint8_t *)deviceName ,sizeof(deviceName) ,sizeof(deviceName)
    ,GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);
GattCharacteristic *devInfoChars[] = {&deviceManufacturer,};
GattService deviceInformationService(
     GattService::UUID_DEVICE_INFORMATION_SERVICE
    ,devInfoChars ,sizeof(devInfoChars)/sizeof(GattCharacteristic *));

static uint16_t uuid16_list[] = {
     GattService::UUID_HEART_RATE_SERVICE
    ,GattService::UUID_BATTERY_SERVICE
    ,GattService::UUID_DEVICE_INFORMATION_SERVICE
};

void updateServiceValues(void);
static Gap::ConnectionParams_t connectionParams;

void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
    DEBUG("Disconnected handle %u, reason %u\r\n", params->handle, params->reason);
    DEBUG("Restarting the advertising process\r\n");
    ble.gap().startAdvertising();
}

void onConnectionCallback(const Gap::ConnectionCallbackParams_t *params) 
{
    DEBUG("connected. Got handle %u\r\n", params->handle);

    connectionParams.slaveLatency = 1;
    if (ble.gap().updateConnectionParams(params->handle, &connectionParams)  
        != BLE_ERROR_NONE) {
        DEBUG("failed to update connection paramter\r\n");
    }
}

void periodicCallback(void)
{
    triggerSensorPolling = true;
}

int main(void)
{   
    Ticker ticker;
    ticker.attach(periodicCallback, 1);
        
    DEBUG("Initialising the nRF51822\r\n");
    ble.init();
    DEBUG("Init done\r\n");
    ble.gap().onDisconnection(disconnectionCallback);
    ble.gap().onConnection(onConnectionCallback);
    ble.gap().getPreferredConnectionParams(&connectionParams);

    /* setup advertising */
    ble.gap().accumulateAdvertisingPayload(
          GapAdvertisingData::BREDR_NOT_SUPPORTED 
        | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    ble.gap().accumulateAdvertisingPayload(
         GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS
        ,(uint8_t*)uuid16_list, sizeof(uuid16_list));
    ble.gap().accumulateAdvertisingPayload(
         GapAdvertisingData::GENERIC_HEART_RATE_SENSOR);
    ble.gap().accumulateAdvertisingPayload(
         GapAdvertisingData::COMPLETE_LOCAL_NAME
        ,(uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
    ble.gap().setAdvertisingType(
         GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().setAdvertisingInterval(160);  /* 100ms; in multiples of 0.625ms. */
    ble.gap().startAdvertising();
    DEBUG("Start Advertising\r\n");

    ble.gattServer().addService(hrmService);
    ble.gattServer().addService(battService);
    ble.gattServer().addService(deviceInformationService);
    DEBUG("Add Service\r\n");
    
    initPulseSensor();

    Ticker hrm_ticker;
    hrm_ticker.attach(calcHeartRate, 0.02);

    while (true) {
        if (triggerSensorPolling) {
            triggerSensorPolling = false;
            updateServiceValues();
        } else {
            ble.waitForEvent();
        }
    }
}

void updateServiceValues(void)
{
    /* Decrement the battery level. */
    batt <= 50 ? batt = 100 : batt--;
    ble.gattServer().write(
        battLevel.getValueAttribute().getHandle() ,(uint8_t*)&batt, sizeof(batt));

    /* Randomize the heart rate. */
    if (isQS()) {
        hrmCounter = getBPM();
        DEBUG("BPM: %d\r\n", hrmCounter);
    }
    bpm[1] = hrmCounter;
    ble.gattServer().write(hrmChar.getValueAttribute().getHandle() ,bpm, sizeof(bpm));
}

Nordic のスマホ向けテストアプリ nRFToolBox で動きを確かめてみました。心拍数が時々あがっているところは軽くスクワットをしているところです。





ちょっと値がおかしいところもありますが、ほぼ想定通りに動いているようです。実験しながらアルゴリズムを調整していきたいと思います。
（＾_＾）／～

---

心拍センサ

• 出版社/メーカー: スイッチサイエンス
• メディア: エレクトロニクス

mbed HRM1017

• 出版社/メーカー: スイッチサイエンス
• メディア: Personal Computers

mbed電子工作レシピ

• 作者: 勝 純一
• 出版社/メーカー: 翔泳社
• 発売日: 2016/01/23
• メディア: 大型本

---