【最終回】家庭用サイクルトレーナーにmicro:bitをつなぐ（その５）- BLEのGATTサービス(FTMP/FTMS)で推定パワーをサポートする

# 家庭用サイクルトレーナーにmicro:bitをつなぐ（その５）
前回までの記事（[その１](https://qiita.com/jp-96/items/110b6364a3da89553b27)、[その２](https://qiita.com/jp-96/items/c494da04f04603ef258d)、[その３](https://qiita.com/jp-96/items/3863e31b1c92a9caa44b)、[その４](https://qiita.com/jp-96/items/abb988a286bfb72ea7c6)）では、家庭用サイクルトレーナーのワイヤレス運動測量計とmicro:bitをつなぎ、ケイデンスやスピードをUSBシリアル通信でモニターしていましたが、これをBLE通信でスマホ・アプリで表示できるようにGATTサービスであるFTMP/FTMSとして実装しました。

**リンク**

1. [**家庭用サイクルトレーナーにmicro:bitをつなぐ（その１）**  
ワイヤレス運動測量計から出力されるクランク回転センサーの信号をモニターする](https://qiita.com/jp-96/items/110b6364a3da89553b27)
2. [**家庭用サイクルトレーナーにmicro:bitをつなぐ（その２）**  
STEP信号からワイヤレス運動測量計に表示されているスピードをmicro:bitで推定する](https://qiita.com/jp-96/items/c494da04f04603ef258d)
3. [**家庭用サイクルトレーナーにmicro:bitをつなぐ（その３）**  
STEP信号からケイデンスとスピードを求めるモジュールをドライバー（アイドル・コンポーネント）として作成する](https://qiita.com/jp-96/items/3863e31b1c92a9caa44b)
4. [**家庭用サイクルトレーナーにmicro:bitをつなぐ（その４）**  
スマホ・アプリでケイデンスやスピードを表示できるようにBLEのGATTサービス(FTMP/FTMS)を実装する](https://qiita.com/jp-96/items/abb988a286bfb72ea7c6)

BLE通信でスマホ・アプリで、ケイデンスやスピードが表示できるようになりましたが、バーチャルライドアプリでのスピードはその値の補正が必要であることがわかりました。そこで、今回は、スピード（ケイデンス）からパワーを推定し、FTMSとしてパワーを利用できるように改良します。
![IMG_20210226_004141.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/252891/eb664145-d364-52ac-913a-5d7e353ffb0f.jpeg)

その前に、一定時間、バーチャルライドアプリと接続していると、Micro:BitがPanic状態もしくは無応答状態になってしまうので、そのあたりの改善を行います。

# Micro:Bitが停止してしまう - Fitness Machine Control Point

バーチャルライドアプリと接続すると、`Fitness Machine Control Point`への書き込みがあり、これを連続受信するとMicro:BitがPanic状態もしくは無応答状態になってしまいます。

## バーチャルライドアプリ - OneLap

今回もバーチャルライドアプリに、OneLapを利用しています。

[OneLap - https://www.onelap.com/](https://www.onelap.com/)
[OneLap Japan - https://www.onelapjapan.jp/](https://www.onelapjapan.jp/)


## 原因の特定

`MESSAGE_BUS_LISTENER_IMMEDIATE`からデフォルトの`MESSAGE_BUS_LISTENER_QUEUE_IF_BUSY`に変更しました。
チェックポイントとして、２つのメソッドにUSBシリアル通信のデバッグコードを追加し、`MicroBitIndoorBikeStepService::onFitnessMachineControlPoint`メソッドでは、即座に`ruturn`するようにしました。

それでも、バーチャルライドアプリと連動させると、途中で無応答状態になってしまいます。

```MicroBitIndoorBikeStepService.cpp
    :
    // Microbit Event listen
    if (EventModel::defaultEventBus)
    {
        EventModel::defaultEventBus->listen(this->indoorBike.getId(), MICROBIT_INDOOR_BIKE_STEP_SENSOR_EVT_DATA_UPDATE
            , this, &MicroBitIndoorBikeStepService::indoorBikeUpdate, MESSAGE_BUS_LISTENER_QUEUE_IF_BUSY);
        EventModel::defaultEventBus->listen(this->id, FTMP_EVENT_VAL_FITNESS_MACHINE_CONTROL_POINT
            , this, &MicroBitIndoorBikeStepService::onFitnessMachineControlPoint, MESSAGE_BUS_LISTENER_QUEUE_IF_BUSY);
    }

}

void MicroBitIndoorBikeStepService::onDataWritten(const GattWriteCallbackParams *params)
{
    uBit.serial.printf("CP:%" PRIu32 ", onDataWritten\r\n", (uint32_t)system_timer_current_time());
    :
}

void MicroBitIndoorBikeStepService::onFitnessMachineControlPoint(MicroBitEvent e)
{
    uBit.serial.printf("CP:%" PRIu32 ", onFitnessMachineControlPoint\r\n", (uint32_t)system_timer_current_time());
    return;
    :
}
:
```

## 改善案

`MicroBitIndoorBikeStepService::onDataWritten`メソッドから、`MicroBitEvent`を生成して、`MicroBitIndoorBikeStepService::onFitnessMachineControlPoint`を呼び出す（Callback）ことに原因があると考え、これらを`MicroBitEvent`による呼び出しではなく、関数化しました。

```MicroBitIndoorBikeStepService.cpp
:
void MicroBitIndoorBikeStepService::onDataWritten(const GattWriteCallbackParams *params)
{
    if (params->handle == fitnessMachineControlPointCharacteristicHandle && params->len >= 1)
    {
        this->doFitnessMachineControlPoint(params);
    }
}

void MicroBitIndoorBikeStepService::doFitnessMachineControlPoint(const GattWriteCallbackParams *params)
{
    uint8_t responseBuffer[3];
    responseBuffer[0] = FTMP_OP_CODE_CPPR_80_RESPONSE_CODE;
    uint8_t *opCode=&responseBuffer[1];
    opCode[0]=params->data[0];
    uint8_t *result=&responseBuffer[2];
    result[0] = FTMP_RESULT_CODE_CPPR_03_INVALID_PARAMETER;
    switch (opCode[0])
    {
    case FTMP_OP_CODE_CPPR_00_REQUEST_CONTROL:
        if (params->len == 1)
        {
            result[0] = FTMP_RESULT_CODE_CPPR_01_SUCCESS;
        }
        break;

    case FTMP_OP_CODE_CPPR_01_RESET:
        if (params->len == 1)
        {
            result[0] = FTMP_RESULT_CODE_CPPR_01_SUCCESS;
        }
        break;

    case FTMP_OP_CODE_CPPR_07_START_RESUME:
        if (params->len == 1)
        {
            result[0] = FTMP_RESULT_CODE_CPPR_01_SUCCESS;
        }
        break;

    case FTMP_OP_CODE_CPPR_08_STOP_PAUSE:
        if (params->len == 2)
        {
            this->stopOrPause = params->data[1];
            result[0] = FTMP_RESULT_CODE_CPPR_01_SUCCESS;
        }
        break;
        
    default:
        result[0] = FTMP_RESULT_CODE_CPPR_02_NOT_SUPORTED;
        break;

    }

    // Response - Fitness Machine Control Point
    uBit.ble->gattServer().write(fitnessMachineControlPointCharacteristicHandle
            , (const uint8_t *)&responseBuffer, sizeof(responseBuffer));
    
    // opCode procedure
    if (result[0]==FTMP_RESULT_CODE_CPPR_01_SUCCESS)
    {
        switch (opCode[0])
        {
        case FTMP_OP_CODE_CPPR_00_REQUEST_CONTROL:
            // # 0x00 M Request Control
            // #define FTMP_EVENT_VAL_OP_CODE_CPPR_00_REQUEST_CONTROL
            // (NOP)
            break;
        case FTMP_OP_CODE_CPPR_01_RESET:
            // # 0x01 M Reset
            // #define FTMP_EVENT_VAL_OP_CODE_CPPR_01_RESET
            this->sendTrainingStatusManualMode();
            break;
        case FTMP_OP_CODE_CPPR_07_START_RESUME:
            // # 0x07 M Start or Resume
            // #define FTMP_EVENT_VAL_OP_CODE_CPPR_07_START_RESUME
            this->sendTrainingStatusManualMode();
            break;
        case FTMP_OP_CODE_CPPR_08_STOP_PAUSE:
            // # 0x08 M Stop or Pause [UINT8, 0x01-STOP, 0x02-PAUSE]
            // #define FTMP_EVENT_VAL_OP_CODE_CPPR_08_STOP_PAUSE
            this->sendTrainingStatusIdle();
            break;
        default:
            break;
        }

    }
    
    // Debug - USB Serial
    if (true)
    {
        uBit.serial.printf("CP:%" PRIu32 ", opCode[0x%02X], result[0x%02X], data", (uint32_t)system_timer_current_time(), opCode[0], result[0]);
        for (int i=0; i<params->len; i++)
        {
            uBit.serial.printf(", 0x%02X", params->data[i]);
        }
        uBit.serial.printf("\r\n");
    }

}
:
```

## 改善結果
完全案で、コンパイルし、Micro:Bitで実行してみると、バーチャルライドアプリを連続稼働していても、Micro:Bitが安定して、動作することを確認しました。

> **【失敗談】**
> BLEからのCallbackメソッド内で、その処理を重くしたり、何度も`uBit.ble->gattServer().write()`を呼び出したりするのを避けようとしたために、`MicroBitEvent`による呼び出しを行いましたが、逆にそれが問題となってしまったようです。


```USB-Serial.log
CP:41583966, onDataWritten
CP:41583969, doFitnessMachineControlPoint
CP:41583973, opCode[0x00], result[0x01], data, 0x00
CP:41584209, onDataWritten
CP:41584212, doFitnessMachineControlPoint
CP:41584216, opCode[0x11], result[0x02], data, 0x11, 0x00, 0x00, 0x07, 0x02, 0x28, 0x2D
CP:41584936, onDataWritten
CP:41584938, doFitnessMachineControlPoint
CP:41584943, opCode[0x00], result[0x01], data, 0x00
CP:41586197, onDataWritten
CP:41586200, doFitnessMachineControlPoint
CP:41586204, opCode[0x11], result[0x02], data, 0x11, 0x00, 0x00, 0x07, 0x02, 0x28, 0x2D
```

# 推定パワーの算出とパワーのサポート

スピード（ケイデンス）や負荷（ダイアル1～8）から、推定パワーを算出し、そのパワーをFTMSでサポートします。

## ソースコード

ソースコードのビルドは、[Arm Mbed オンライン - https://ide.mbed.com/compiler/](https://ide.mbed.com/compiler/)で可能です。

Source URLに https://github.com/jp-96/microbit-digital-capture5.git と入力し、Programとしてインポートして、コンパイルします。


## 推定パワー

パワー(`power`)は、スピード(`speed100`)と負荷の値(`resistanceLevel10` - 負荷ダイアル1～8を10倍した値)から次の式で算出します。

> \***power** = (int32_t)((double)(*speed100) * (K_INCLINE_A * ((double)resistanceLevel10)/10 + K_INCLINE_B) * K_POWER);

**参考**：[リアルタイムでパワーを計測する簡易的な方法 - https://diary.cyclekikou.net/archives/15876](https://diary.cyclekikou.net/archives/15876)

## 推定パワー算出の実装

ケイデンスとスピードを算出している`MicroBitIndoorBikeStepSensor::calcIndoorBikeData`メソッドで実装します。

```MicroBitIndoorBikeStepSensor.cpp
:
void MicroBitIndoorBikeStepSensor::calcIndoorBikeData(uint32_t crankIntervalTime, uint8_t resistanceLevel10, uint32_t* cadence2, uint32_t* speed100, int16_t* power)
{
    if (crankIntervalTime==0)
    {
        *cadence2 = 0;
        *speed100 = 0;
        *power = 0;
    }
    else
    {
        *cadence2 = (uint32_t)( (uint64_t)K_STEP_CADENCE / crankIntervalTime );
        *speed100 = (uint32_t)( (uint64_t)K_STEP_SPEED   / crankIntervalTime );
        // https://diary.cyclekikou.net/archives/15876
        *power = (int32_t)((double)(*speed100) * (K_INCLINE_A * ((double)resistanceLevel10)/10 + K_INCLINE_B) * K_POWER);
    }
}
:
```

## Indoor Bike Dataでのパワーのサポート - Instantaneous Power Present

`Indoor Bike Data`には、`Instantaneous Power`項目がありますので、これを有効化し、書き込みます。

```MicroBitIndoorBikeStepService.h
:

/*
# Bit Definitions for the Indoor Bike Data Characteristic
#                                          000 (bits 13-15) Reserved for Future Use
#                                             0 (bit 12) Remaining Time Present
#                                              0 (bit 11) Elapsed Time Present
#                                               0 (bit 10) Metabolic Equivalent Present
#                                                0 (bit  9) Heart Rate Present
#                                                 0 (bit  8) Expended Energy Present
#                                                  0 (bit  7) Average Power Present
#                                                   1 (bit  6)*Instantaneous Power Present
#                                                    0 (bit  5) Resistance Level Present
#                                                     0 (bit  4) Total Distance Present
#                                                      0 (bit  3) Average Cadence present
#                                                       1 (bit  2)*Instantaneous Cadence (uint16, 1/minute with a resolution of 0.5)
#                                                        0 (bit  1) Average Speed present
#                                                         0 (bit  0) More Data
#                                          5432109876543210 */
#define FTMP_FLAGS_INDOOR_BIKE_DATA_CHAR 0b0000000001000100
:

/*
# Definition of the bits of the Fitness Machine Features field
#                                                  000000000000000 (bits 17-31) Reserved for Future Use
#                                                                 0 (bit 16) User Data Retention Supported
#                                                                  0 (bit 15) Force on Belt and Power Output Supported
#                                                                   1 (bit 14)*Power Measurement Supported
#                                                                    0 (bit 13) Remaining Time Supported
#                                                                     0 (bit 12) Elapsed Time Supported
#                                                                      0 (bit 11) Metabolic Equivalent Supported
#                                                                       0 (bit 10) Heart Rate Measurement Supported
#                                                                        0 (bit  9) Expended Energy Supported
#                                                                         0 (bit  8) Stride Count Supported
#                                                                          0 (bit  7) Resistance Level Supported
#                                                                           0 (bit  6) Step Count Supported
#                                                                            0 (bit  5) Pace Supported
#                                                                             0 (bit  4) Elevation Gain Supported
#                                                                              0 (bit  3) Inclination Supported
#                                                                               0 (bit  2) Total Distance Supported
#                                                                                1 (bit  1)*Cadence Supported
#                                                                                 0 (bit  0) Average Speed Supported
#                                                  10987654321098765432109876543210 */
#define FTMP_FLAGS_FITNESS_MACINE_FEATURES_FIELD 0b00000000000000000100000000000010
:

    // Characteristic buffer
    static const uint16_t indoorBikeDataCharacteristicBufferSize = 2+2+2+2; // "<HHHh", FTMS p.42, <Flags>, <Instantaneous Speed>, <Instantaneous Cadence>, <Instantaneous Power Present>
    :
```

```MicroBitIndoorBikeStepService.cpp
:
void MicroBitIndoorBikeStepService::indoorBikeUpdate(MicroBitEvent e)
{
    if (uBit.ble->getGapState().connected)
    {
        uint8_t buff[indoorBikeDataCharacteristicBufferSize];
        struct_pack(buff, "<HHHh",
            FTMP_FLAGS_INDOOR_BIKE_DATA_CHAR,
            this->indoorBike.getSpeed100(),
            this->indoorBike.getCadence2(),
            this->indoorBike.getPower()
        );
        uBit.ble->gattServer().notify(indoorBikeDataCharacteristicHandle
            , (uint8_t *)&buff, indoorBikeDataCharacteristicBufferSize);
    }
}
:
```

## 負荷ダイアル合わせ - 手動

推定パワーの計算式は、負荷ダイアルの値を考慮していますので、負荷ダイアルと負荷の値(`resistanceLevel10` - 負荷ダイアル1～8を10倍した値)とを手動で合わせられるように、`A`ボタンと`B`ボタンとで変更できるようにします。
使い方によっては、Bダッシュも可能です。

```main.cpp
:
void addResistanceLevel(int8_t addLevel)
{
    sensor->setResistanceLevel10(sensor->getResistanceLevel10() + (addLevel*10));
    uBit.display.print(sensor->getResistanceLevel10()/10);
}

void onButtonA(MicroBitEvent e)
{
    addResistanceLevel(-1);
}

void onButtonB(MicroBitEvent e)
{
    addResistanceLevel(1);
}

void setup()
{
    sensor = new MicroBitIndoorBikeStepSensor(uBit);
    addResistanceLevel(1);
    service = new MicroBitIndoorBikeStepService(uBit, *sensor);
    sensor->idleTick();

    uBit.messageBus.listen(MICROBIT_ID_BUTTON_A, MICROBIT_BUTTON_EVT_CLICK, onButtonA);
    uBit.messageBus.listen(MICROBIT_ID_BUTTON_B, MICROBIT_BUTTON_EVT_CLICK, onButtonB);

}
:
```

# おわりに

- `MicroBitEvent`の連続呼び出しの不具合を回避しました
- スピード（ケイデンス）から、推定パワーを算出しました
- 負荷ダイアルに合わせて、負荷値を手動で変更することにより、推定パワーも増減するようにしました

家庭用サイクルトレーナーのワイヤレス運動測量計にmicro:bitをつなぎ、BLEのFTMSを実装することにより、バーチャルライドアプリを楽しむことができるようになりました。

`Fitness Machine Control Point`では、`0x11 - Set Indoor Bike Simulation Parameters`が送られてきていますが、そこに含まれる傾斜（`Grade`）を読み取り、家庭用サイクルトレーナーの負荷をサーボモーターで連動させることにも成功しています。これは、またの機会にご紹介します。
**→【追記 2021/03/05】**[BLEのGATTサービス(FTMP/FTMS)を実装したmicro:bitと家庭用サイクルトレーナーでバーチャルライドを楽しもう](https://qiita.com/jp-96/items/07bde8fa4248a4beb258)

[![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/252891/28ffbf65-e969-0bcb-dce0-d7f694d298dd.png)](https://amzn.to/2O4TAm8)

これからも、在宅ワークがより一層浸透していくと思いますが、机の下に一台置いておくといつでも運動できて、血行促進、疲労回復、健康促進に一役買うのではないでしょうか。バーチャルライドを気軽に楽しみましょう。

<!--
リンクを別タブで開く
https://qiita.com/yukichi/items/df4f5126f193a1c8efdb
-->
<script>
jQuery(document).ready(function(){
    jQuery('a[href^=http]')
    .not('[href*="'+location.hostname+'"]')
    .attr({target:"_blank"})
    .addClass("external")
    ;})  
</script>