家庭用サイクルトレーナーにmicro:bitをつなぐ（その５）

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

リンク

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

BLE通信でスマホ・アプリで、ケイデンスやスピードが表示できるようになりましたが、バーチャルライドアプリでのスピードはその値の補正が必要であることがわかりました。そこで、今回は、スピード（ケイデンス）からパワーを推定し、FTMSとしてパワーを利用できるように改良します。

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

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

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

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

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

OneLap - https://www.onelap.com/
OneLap Japan - 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/で可能です。

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

推定パワー算出の実装

ケイデンスとスピードを算出している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と家庭用サイクルトレーナーでバーチャルライドを楽しもう

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