「扉が閉まってからでは遅い！」を無くす忘れ物防止端末

BLE
ESP32
MPU6050
obniz
加速度センサー
忘れ物
obniz IoT コンテスト 2021

<目次>
　・概要
　・動作原理
　・部品リスト
　・接続図
　・設計仕様
　・動作仕様
　・デモンストレーション動画
　・所感
　・ソースコード

<概要>
　電車やバス乗降車あるあるの、「扉が閉まってからでは遅い！」を解決するためのアプリケーションです。
　社員証や重要機器などのセキュリティ事故になるような物品から石焼き芋のような嗜好品まで、大切な持ち物の置き忘れを教えてくれるアラート端末です。
　すでに販売されている忘れ物防止端末は、BLE接続を常時確認する方式で、接続が切れたときに通知してくれるような仕様で、様々なシーンに有用ですが、公共交通機関の乗降時ではBLE接続が切れるタイミングは概ね扉が閉まった後になってしまいます。
　そこで、扉が閉まる前に置き忘れを即時通知し、「しまった、、、！」を防止してくれるような端末の作成を目指します。

<動作原理>
　加速度ジャイロセンサを2セット用いた「活動レベル同期方式(仮称)」で置き忘れを検出します。
　前提として、公共交通機関での置き忘れが発生した直後の状況は、本人は「歩行もしくは走行」し、持ち物は「静止」している状況が殆どです。
　そこで、本人(ホスト)と持ち物(デバイス)にそれぞれ端末を装着しておき、デバイスはBLE経由でホストに活動量を送信し、ホストは受信した値とホスト側の活動量を比較し、それぞれの活動レベルを認識します。※活動量：加速度・ジャイロ値から計算した値です。
　その結果、持ち物(デバイス)の活動レベルが「静止」中に本人(ホスト)の活動レベルが「歩行・走行」状態であれば置き忘れと判断し、ビープ音で通知する仕組みを作成します。

<部品リスト>
◇ホスト側
　obniz Board 1Y
　MPU-6050 使用３軸ジャイロスコープ・３軸加速度センサーモジュール　【価格：350円】　
　ブブザーモジュールタイマーArdinuo電子玩具警報装置用【価格：829円】
　モバイルバッテリー　５０００ｍＡｈ品番 4984279860151 【価格：550円（税込）】

◇デバイス側
　LILYGO_T-Higrow ESP32 WiFi + bluetooth + DHT11土壌温度および湿度センサーモジュール　【価格：3,282円】　
　MPU-6050 使用３軸ジャイロスコープ・３軸加速度センサーモジュール　【価格：350円】　
　モバイルバッテリー　５０００ｍＡｈ品番 4984279860151 【価格：550円（税込）】

<接続図>

<設計仕様>
◇ホスト側・デバイス側共通
　センサ分解能：加速度 4g　ジャイロ 500dps
　加速度AD0：Lo(address=0x68)
　活動量更新周期：500msec（50msec x 10sampleの平均値を使用）
　活動量の計算：abs(Δx)+abs(Δy)+abs(Δz)　※Acc,Gyroで共通の計算式。Δは1sample毎の前回との差分のことです。
　　※センサの向きを考慮しない計算方法を採用しています。

◇ホスト
　活動レベルと活動量の関係：
　　活動レベル：活動量
　　　0(静止)　　：0.0g
　　　1(交通機関移動)：0.2g
　　　2(歩行) ：0.4g
　　　3(走行) ：1.5g

ビープ音：440Hz, duty=50%

<動作仕様>
ホスト、デバイスそれぞれ非同期で設計仕様の通りデータサンプリングを継続的に実施します。
デバイスは活動量を継続的にBLEアドバタイズし、ホストは活動量を受信したら活動レベルの判定を行います。
・ホスト活動レベルが2(歩行)以上で、デバイス活動レベルが0(静止)であれば、置き忘れと判断しビープ音で通知します。
・デバイスの活動レベルが1(交通機関移動)以上になると、ホストの活動レベルによらずビープ音をストップします。
・ホストの活動レベルが2(歩行)未満であれば、デバイスの活動レベルによらずビープ音をストップします。
・デバイスの活動レベルに更新がない場合は「Out of Range」と表示し、ビープ音をストップします。

<デモンストレーション動画>
◇持ち歩き試験の様子
※環境ノイズが大きめですので、適宜音量を調整してご覧ください。

ここに動画が表示されます

◇動作原理の説明
※ビープ音は小さめに設定しておりますので、適宜音量を調整してご覧ください。

ここに動画が表示されます

制御フロー

<所感>
　シンプルな構成ですが、徒歩、走行、信号待ち、電車、バスのシーンで概ね期待通りの活動レベルが出ており、動作仕様通りに通知することができました。
　今回実装した活動レベル同期方式(仮称)に加え、従来製品のようなBLE接続状態での判定、またGPSを併用することでより有用な忘れ物防止端末ができるのではないかと思いました。

Obnizの使用感としては、今回1週間程度で構想～フィールドテストまで行い、忘れ物防止の感触をつかむことができました。
　その要因として、
　　・Wifi接続が容易
　　・Web上でコーディング可能(PCにインストール不要)
　　・PC上のGUIでリアルタイムに動作確認ができる
　　・ObnizのBLE受信の部分がシンプルかつサンプルが準備されている
　　・加速度センサの部品ライブラリが用意されており、選定～動作までサポートされている
　　・多少雑にソースコードを組んでも壊れない
　　・多少雑にハードを扱っても壊れない
　　・ハードのI/Fが汎用的
　というあたりが挙げられ、自分のやりたい→やり方がわかる→大胆にやれると連鎖し、結果としてスピード開発につながったと思いました。

逆に、今後の改善の期待として
　　・ブロックプログラミングのライブラリ強化（BLE、MPU6050が対応しておらず即断念）
　　・部品ライブラリのアップデート（売切れの部品が目立つ。。。MPU6050はリファレンスマニュアルがリンク切れ、、、）
　　・スレッドの周期が不安定な時があり、リアルタイム性の仕様をある程度公開してほしい
　　　（今回はBLE受信、センサ取得＆判定の2スレッドで実施）
　　・サンプルソースの充実化
　　・チャット等で気軽にサポートしてくれるとありがたい
　　・消費電力の概算をシミュレーションしたい　
　あたりを手厚くしてくれると、より製品としての設計を視野に入れたスピード開発ができると感じました。

<ソースコード>

ホスト
<html>
  <head>
    <meta charset="utf-8" />
    <meta name="viewport" content="width=device-width, initial-scale=1" />
    <link
      rel="stylesheet"
      href="https://stackpath.bootstrapcdn.com/bootstrap/4.3.1/css/bootstrap.min.css"
    />
    <script src="https://code.jquery.com/jquery-3.2.1.min.js"></script>
    <script
      src="https://unpkg.com/obniz@3.14.0/obniz.js"
      crossorigin="anonymous"
    ></script>
  </head>
  <body>
    <div id="obniz-debug"></div>
    <h3>Connect From Your Browser</h3>
    <button class="btn btn-primary" id="on">LED ON</button>
    <button class="btn btn-primary" id="off">LED OFF</button>
    <div id="print"></div>

    <div>
      <input type="text" id="text" value="Hello World" />
      <button class="btn btn-primary" id="showtime">Print on obniz</button>
    </div>

    <div>Demo Pin Assign</div>
    <ul>
      <li>io0: LED anode</li>
      <li>io1: LED cathode</li>
      <li>Switch State will be printed on browser</li>
    </ul>

    <script>
      var obniz = new Obniz("YOUR_OBNIZ_ID");

      obniz.onconnect = async function() {

        //////////////////////////////////////////////////////////
        //デバイス側のAccGyroセンサの設定・データ取得
        //////////////////////////////////////////////////////////
        //MPU6050 ACC 単位換算[digit/g]
        const MPU6050_ACC_2G = 32768;
        const MPU6050_ACC_4G = 16384;
        const MPU6050_ACC_8G =  8192;
        const MPU6050_ACC_16G=  4096;
        //MPU6050 GYRO 単位換算[digit/(deg/sec)]
        const MPU6050_GYRO_250 = 262;
        const MPU6050_GYRO_500 = 131;
        const MPU6050_GYRO_1000=  65;
        const MPU6050_GYRO_2000=  32;

        var device_acc_value=0;
        var device_gyro_value=0;

        await obniz.ble.initWait();
        
        //デバイス側の加速度・ジャイロ値を取得。（活動量として計算された後の値で取得。）
        obniz.ble.scan.onfind = async function(ret){
          //上位バイトと下位バイトを連結し、単位換算
          device_acc_value = ((ret.adv_data[17]*256) + ret.adv_data[18])/MPU6050_ACC_4G;
          device_gyro_value =((ret.adv_data[19]*256) + ret.adv_data[20])/MPU6050_GYRO_500;
        };

        obniz.ble.scan.onfinish = async function(ret, error){
          console.log("scan timeout!");
        };

        var target = {
          localName: "ESP32"
        };
        var setting = {
          duration: null, // 60 sec
          duplicate: true, // allow duplicate
          filterOnDevice:true
        }

        await obniz.ble.scan.startWait(target, setting);

        //////////////////////////////////////////////////////////
        //ホスト側のAccGyroセンサの設定・データ取得
        //////////////////////////////////////////////////////////
        //ACCで判断した活動スレッシュ[g]
        const ACC_ACTIVE_THRESHOLD0 = 0.0; //静止
        const ACC_ACTIVE_THRESHOLD1 = 0.2; //交通手段移動中
        const ACC_ACTIVE_THRESHOLD2 = 0.4; //徒歩(実測:0.5-1.2g)
        const ACC_ACTIVE_THRESHOLD3 = 1.5; //走り(実測:1.5-2.0g)
        //GYROで判断した活動スレッシュ[deg/sec]
        const GYRO_ACTIVE_THRESHOLD0 = 0;  //静止
        const GYRO_ACTIVE_THRESHOLD1 = 15; //交通手段移動中
        const GYRO_ACTIVE_THRESHOLD2 = 30; //徒歩(40-80deg/sec)
        const GYRO_ACTIVE_THRESHOLD3 = 50; //走り(80-100deg/sec)※走りと徒歩で差が出ない場合もある

        //活動レベル
        const ACTIVE_LEVEL0 = 0; //静止
        const ACTIVE_LEVEL1 = 1; //交通手段移動中
        const ACTIVE_LEVEL2 = 2; //徒歩
        const ACTIVE_LEVEL3 = 3; //走り
        
        var data;
        var acc_value;
        var gyro_value;
        var device_acc_value_before=0;
        var device_gyro_value_before=0;

        var ret_acc;
        var ret_gyro;

        var acc_x = 0;
        var acc_y = 0;
        var acc_z = 0;
        var gyro_x = 0;
        var gyro_y = 0;
        var gyro_z = 0;

        var acc_x_before=0;
        var acc_y_before=0;
        var acc_z_before=0;

        var gyro_x_before;
        var gyro_y_before;
        var gyro_z_before;

        //判定結果（活動レベル 0～4段階）
        var acc_level=0;
        var device_acc_level=0;
        var gyro_level=0;
        var device_gyro_level=0;

        const SAMPLE_NUM = 10;
        const SAMPLE_PERIOD = 50;

        obniz.io4.output(false); //AD0=Lo 
        var sensor = obniz.wired("MPU6050", {scl: 0, sda: 1 , ad0:0x68});
        sensor.setConfig(4, 500);

        obniz.io7.output(false); //vcc 
        obniz.io9.output(false); //gnd 
        var pwm = obniz.getFreePwm();

//        await obniz.wait(5000);

        //ホストデータ取得・計算、活動レベル判定//
        // acc = gyro = abs(Δx)+abs(Δy)+abs(Δz)
        while(obniz.connectionState === 'connected') {
        try {
          await obniz.pingWait(); // 通信を確認

          ret_acc=0;
          ret_gyro=0;
          acc_x = 0;
          acc_y = 0;
          acc_z = 0;
          gyro_x = 0;
          gyro_y = 0;
          gyro_z = 0;

          for (let i = 0; i < SAMPLE_NUM; i++) {
            data = await sensor.getWait();
            //Acc 絶対値変換
            if(data.accelerometer.x>0){
              acc_x =  data.accelerometer.x;
            }else{
              acc_x = -data.accelerometer.x;
            }
            if(data.accelerometer.y>0){
              acc_y =  data.accelerometer.y;
            }else{
              acc_y = -data.accelerometer.y;
            }
            if(data.accelerometer.z>0){
              acc_z =  data.accelerometer.z;
            }else{
              acc_z = -data.accelerometer.z;
            }
            //Gyro 絶対値変換
            if(data.gyroscope.x>0){
              gyro_x =  data.gyroscope.x;
            }else{
              gyro_x = -data.gyroscope.x;
            }
            if(data.gyroscope.y>0){
              gyro_y =  data.gyroscope.y;
            }else{
              gyro_y = -data.gyroscope.y;
            }
            if(data.gyroscope.z>0){
              gyro_z =  data.gyroscope.z;
            }else{
              gyro_z = -data.gyroscope.z;
            }

            //Acc計算
            if(acc_x > acc_x_before){
              ret_acc  += acc_x - acc_x_before;  
            }else{
              ret_acc  += acc_x_before - acc_x;
            }
            if(acc_y > acc_y_before){
              ret_acc  += acc_y - acc_y_before;  
            }else{
              ret_acc  += acc_y_before - acc_y;
            }
            if(acc_z > acc_z_before){
              ret_acc  += acc_z - acc_z_before;  
            }else{
              ret_acc  += acc_z_before - acc_z;
            }

            //Gyro計算
            if(gyro_x > gyro_x_before){
              ret_gyro  += gyro_x - gyro_x_before;  
            }else{
              ret_gyro  += gyro_x_before - gyro_x;
            }
            if(gyro_y > gyro_y_before){
              ret_gyro  += gyro_y - gyro_y_before;  
            }else{
              ret_gyro  += gyro_y_before - gyro_y;
            }
            if(gyro_z > gyro_z_before){
              ret_gyro  += gyro_z - gyro_z_before;  
            }else{
              ret_gyro  += gyro_z_before - gyro_z;
            }

            acc_x_before = acc_x;
            acc_y_before = acc_y;
            acc_z_before = acc_z;

            gyro_x_before = gyro_x;
            gyro_y_before = gyro_y;
            gyro_z_before = gyro_z;

            await obniz.wait(SAMPLE_PERIOD);
          }
          acc_value  = ret_acc  / SAMPLE_NUM;
          gyro_value = ret_gyro / SAMPLE_NUM;

//          console.log('acc_value: ',acc_value);
//          console.log('gyro_value: ', gyro_value);
//          console.log(data);

          //判定処理本体
          //デバイス側のデータが更新された場合に実施する。
          //デバイス側とホスト側で活動レベルが異なった場合はブザー等で通知する
          if((device_acc_value!=device_acc_value_before)||(device_gyro_value!=device_gyro_value_before)){
            //Accの判定
            if(acc_value < ACC_ACTIVE_THRESHOLD1){
              acc_level = ACTIVE_LEVEL0; //静止
            }else if(acc_value < ACC_ACTIVE_THRESHOLD2){
              acc_level = ACTIVE_LEVEL1; //交通手段移動中
            }else if(acc_value < ACC_ACTIVE_THRESHOLD3){
              acc_level = ACTIVE_LEVEL2; //徒歩 
            }else if(acc_value >= ACC_ACTIVE_THRESHOLD3){
              acc_level = ACTIVE_LEVEL3; //走り
            }

            if(device_acc_value < ACC_ACTIVE_THRESHOLD1){
              device_acc_level = ACTIVE_LEVEL0; //静止
            }else if(device_acc_value < ACC_ACTIVE_THRESHOLD2){
              device_acc_level = ACTIVE_LEVEL1; //交通手段移動中
            }else if(device_acc_value < ACC_ACTIVE_THRESHOLD3){
              device_acc_level = ACTIVE_LEVEL2; //徒歩 
            }else if(device_acc_value >= ACC_ACTIVE_THRESHOLD3){
              device_acc_level = ACTIVE_LEVEL3; //走り
            }

            //Gyroの判定
            if(gyro_value < GYRO_ACTIVE_THRESHOLD1){
              gyro_level = ACTIVE_LEVEL0; //静止
            }else if(gyro_value < GYRO_ACTIVE_THRESHOLD2){
              gyro_level = ACTIVE_LEVEL1; //交通手段移動中
            }else if(gyro_value < GYRO_ACTIVE_THRESHOLD3){
              gyro_level = ACTIVE_LEVEL2; //徒歩 
            }else if(gyro_value >= GYRO_ACTIVE_THRESHOLD3){
              gyro_level = ACTIVE_LEVEL3; //走り
            }

            if(device_gyro_value < GYRO_ACTIVE_THRESHOLD1){
              device_gyro_level = ACTIVE_LEVEL0; //静止
            }else if(device_gyro_value < GYRO_ACTIVE_THRESHOLD2){
              device_gyro_level = ACTIVE_LEVEL1; //交通手段移動中
            }else if(device_gyro_value < GYRO_ACTIVE_THRESHOLD3){
              device_gyro_level = ACTIVE_LEVEL2; //徒歩 
            }else if(device_gyro_value >= GYRO_ACTIVE_THRESHOLD3){
              device_gyro_level = ACTIVE_LEVEL3; //走り
            }

            obniz.display.clear();

//            obniz.display.print(acc_value);
//            obniz.display.print(device_acc_value);
//            obniz.display.print(gyro_value);
//            obniz.display.print(device_gyro_value);
            obniz.display.print(acc_level);
            obniz.display.print(device_acc_level);
            obniz.display.print(gyro_level);
            obniz.display.print(device_gyro_level);

            //ホスト側が徒歩以上の活動レベルである場合
            if((acc_level  >= ACTIVE_LEVEL2)&&(gyro_level >= ACTIVE_LEVEL2)){
              if((device_acc_level  < ACTIVE_LEVEL1)&&(device_gyro_level < ACTIVE_LEVEL1)){
                //デバイス側が交通手段移動中未満の活動レベルである場合
                //置き忘れ
                obniz.io7.output(true); //vcc
                obniz.io9.output(false); //gnd
                pwm.start({io:8});
                pwm.freq(440); // 440Hz
                pwm.duty(50) // 50%

              }else{
                //ホストとデバイスの活動レベルが同期している状態
                //徒歩
                //走行
                obniz.io8.output(false); //signal 
                obniz.io7.output(false); //vcc 
                obniz.io9.output(false); //gnd 
              }
              console.log('acc_level: ',acc_level);
              console.log('device_acc_level: ',device_acc_level);

            }else{
              //ホスト側が徒歩以下の活動レベルである場合
              //デバイス・ホスト両方放置
              //信号待ち
              //交通手段移動中
              obniz.io8.output(false); //signal 
              obniz.io7.output(false); //vcc 
              obniz.io9.output(false); //gnd 
            }

            device_acc_value_before = device_acc_value;
            device_gyro_value_before= device_gyro_value;

          } else {
            //デバイス側のBLEが受信失敗状態
            obniz.display.clear();
            obniz.display.print("Out of Range");
            obniz.io8.output(false); //signal 
            obniz.io7.output(false); //vcc 
            obniz.io9.output(false); //gnd 
          }
        } catch(e) {
          // break or throw a error
        }
        //////////////////////////////////////////////////////////
        //////////////////////////////////////////////////////////
      }
        var led = obniz.wired("LED", { anode: 0, cathode: 1 });
      };
    </script>
  </body>
</html>

デバイス
#include <Arduino.h>
#include <Wire.h>

#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>

// *******************************************************************************************************************************
// START userdefined data
// *******************************************************************************************************************************
#define DEVICE_NAME "ESP32"         // デバイス名
#define DEVICE_NUMBER 1             // デバイス識別番号（1～8）
RTC_DATA_ATTR static uint8_t seq_number;    // RTCメモリー上のシーケンス番号

// MPU-6050 Accelerometer + Gyro
// レジスタアドレス
#define MPU6050_I2C_SDA             22
#define MPU6050_I2C_SCL             21
#define MPU6050_ACCEL_XOUT_H 0x3B  // R  
#define MPU6050_WHO_AM_I     0x75  // R
#define MPU6050_PWR_MGMT_1   0x6B  // R/W
#define MPU6050_I2C_ADDRESS  0x68
// 構造体定義
typedef union accel_t_gyro_union {
  struct {
    uint8_t x_accel_h;
    uint8_t x_accel_l;
    uint8_t y_accel_h;
    uint8_t y_accel_l;
    uint8_t z_accel_h;
    uint8_t z_accel_l;
    uint8_t t_h;
    uint8_t t_l;
    uint8_t x_gyro_h;
    uint8_t x_gyro_l;
    uint8_t y_gyro_h;
    uint8_t y_gyro_l;
    uint8_t z_gyro_h;
    uint8_t z_gyro_l;
  }
  reg;
  struct {
    int16_t x_accel;
    int16_t y_accel;
    int16_t z_accel;
    int16_t temperature;
    int16_t x_gyro;
    int16_t y_gyro;
    int16_t z_gyro;
  }
  value;
};
// MPU-6050 Accelerometer + Gyro
///////////////////////////////////////

// *******************************************************************************************************************************
// END userdefined data
// *******************************************************************************************************************************
void setup() {
  Serial.begin(115200);
  Serial.println("Void Setup");

  // MPU6050動作開始
  Wire.begin(MPU6050_I2C_SDA, MPU6050_I2C_SCL);
  // obnizサンプルコードの通りシーケンスを実行  
  MPU6050_write_reg(0x6B, 0x00);
  delay(2);
  MPU6050_write_reg(0x1A, 0x01);
  delay(2);
  MPU6050_write_reg(0x19, 0x05);
  delay(2);
  MPU6050_write_reg(0x38, 0x00);
  delay(2);
  MPU6050_write_reg(0x6A, 0x00);
  delay(2);
  MPU6050_write_reg(0x23, 0x00);
  delay(4);
  MPU6050_write_reg(0x37, 0x22);
  delay(2);
  MPU6050_write_reg(0x38, 0x01);
  delay(2);
  MPU6050_write_reg(0x1C, 0x00);
  MPU6050_write_reg(0x1B, 0x00);
  MPU6050_write_reg(0x1C, 0x00);
  MPU6050_write_reg(0x1B, 0x00);
  delay(100);

  BLEDevice::init(DEVICE_NAME);             // BLEデバイスを初期化する

  // BLEサーバーを作成してアドバタイズオブジェクトを取得する
  BLEServer *pServer = BLEDevice::createServer();
  BLEAdvertising *pAdvertising = pServer->getAdvertising();

  // アドバタイズ開始
  pAdvertising->start();
  Serial.println("Advertising started!");
  
  while(1){
    // 送信情報を設定してシーケンス番号をインクリメントする
    setAdvertisementData(pAdvertising); //センサ値取得・活動レベル計算、データ送信
    seq_number++;
  }
}

void loop() {
}

void setAdvertisementData(BLEAdvertising* pAdvertising) {
    float acc_tmp=0;
    float gyro_tmp=0;
    float acc_value=0;
    float gyro_value=0;
    uint16_t acc_value_16 = 0;
    uint16_t gyro_value_16 = 0;
    
    const uint8_t sample_num = 10;

    //加速度ジャイロの単純移動平均を取得する
    acc_tmp = 0;
    gyro_tmp = 0;
    acc_value = 0;
    gyro_value = 0;
    for(uint8_t i=0; i<sample_num; i++){    
      acc_gyro_process(&acc_tmp,&gyro_tmp);
      acc_value += acc_tmp;
      gyro_value += gyro_tmp;
      delay(50);
    }
    acc_value = acc_value/sample_num;
    gyro_value = gyro_value/sample_num;
    acc_value_16 = (uint16_t)acc_value;
    gyro_value_16 = (uint16_t)gyro_value;

    Serial.print("acc_value:");
    Serial.print(acc_value/16384, 2);
    Serial.print(", gyro_value:");
    Serial.print(gyro_value/131, 2);
    Serial.println("");
    Serial.print("acc_value_16:");
    Serial.print(acc_value_16, 2);
    Serial.print(", gyro_value_16:");
    Serial.print(gyro_value_16, 2);
    Serial.println("");

    // string領域に送信情報を連結する
    std::string strData = "";
    strData += (char)0xff;                  // Manufacturer specific data
    strData += (char)0xff;                  // manufacturer ID low byte
    strData += (char)0xff;                  // manufacturer ID high byte
    strData += (char)DEVICE_NUMBER;         // サーバー識別番号
    strData += (char)0xAA;                  // 異常状態判定
    strData += (char)seq_number;            // シーケンス番号
    strData += (char)((acc_value_16 >> 8) & 0xff);   // acc活動量の上位バイト
    strData += (char)(acc_value_16 & 0xff);          // acc活動量の下位バイト
    strData += (char)((gyro_value_16 >> 8) & 0xff);  // gyro活動量の上位バイト
    strData += (char)(gyro_value_16 & 0xff);         // gyro活動量の下位バイト
    strData = (char)strData.length() + strData; // 先頭にLengthを設定
 
    // デバイス名とフラグをセットし、送信情報を組み込んでアドバタイズオブジェクトに設定する
    BLEAdvertisementData oAdvertisementData = BLEAdvertisementData();
    oAdvertisementData.setName(DEVICE_NAME);
    oAdvertisementData.setFlags(0x06);      // LE General Discoverable Mode | BR_EDR_NOT_SUPPORTED
    oAdvertisementData.addData(strData);
    pAdvertising->setAdvertisementData(oAdvertisementData);
}

/*
 * 加速度センサ値を取得し、活動レベルを返す
 * 活動レベル= ( abs(Δx)+abs(Δy)+abs(Δz)+GyrX+GyrY+GyrZ )のTBD秒間の移動平均
 */
void acc_gyro_process(float *acc_value, float *gyro_value){
  int error;

  float ret_acc;
  float ret_gyro;

  static float acc_x_before=0;
  static float acc_y_before=0;
  static float acc_z_before=0;

  static float gyro_x_before=0;
  static float gyro_y_before=0;
  static float gyro_z_before=0;
  
  accel_t_gyro_union accel_t_gyro;

  Wire.begin(MPU6050_I2C_SDA, MPU6050_I2C_SCL);

  // 加速度、角速度の読み出し
  // accel_t_gyroは読み出した値を保存する構造体、その後ろの引数は取り出すバイト数
  error = MPU6050_read(MPU6050_ACCEL_XOUT_H, (uint8_t *)&accel_t_gyro, sizeof(accel_t_gyro));

  // 取得できるデータはビッグエンディアンなので上位バイトと下位バイトの入れ替え（AVRはリトルエンディアン）
  uint8_t swap;
  #define SWAP(x,y) swap = x; x = y; y = swap
  SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l);
  SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l);
  SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l);
  SWAP (accel_t_gyro.reg.t_h, accel_t_gyro.reg.t_l);
  SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l);
  SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l);
  SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l);

  // （未実施。ホスト側で実施する。）取得した加速度値を分解能で割って加速度(G)に変換する
  float acc_x = accel_t_gyro.value.x_accel;// / 16384.0; //FS_SEL_0 16,384 LSB / g
  float acc_y = accel_t_gyro.value.y_accel;// / 16384.0;
  float acc_z = accel_t_gyro.value.z_accel;// / 16384.0;

  // （未実施。ホスト側で実施する。）取得した角速度値を分解能で割って角速度(degrees per sec)に変換する
  float gyro_x = accel_t_gyro.value.x_gyro;// / 131.0;//FS_SEL_0 131 LSB / (°/s)
  float gyro_y = accel_t_gyro.value.y_gyro;// / 131.0;
  float gyro_z = accel_t_gyro.value.z_gyro;// / 131.0;

  //デバイスデータ計算//
  // acc = gyro = abs(Δx)+abs(Δy)+abs(Δz)
  //絶対値変換
  if(acc_x < 0){
    acc_x = -acc_x;
  }
  if(acc_y < 0){
    acc_y = -acc_y;
  }
  if(acc_z < 0){
    acc_z = -acc_z;
  }
  if(gyro_x < 0){
    gyro_x = -gyro_x;
  }
  if(gyro_y < 0){
    gyro_y = -gyro_y;
  }
  if(gyro_z < 0){
    gyro_z = -gyro_z;
  }

  //Acc計算
  if(acc_x > acc_x_before){
    ret_acc  += acc_x - acc_x_before;  
  }else{
    ret_acc  += acc_x_before - acc_x;
  }
  if(acc_y > acc_y_before){
    ret_acc  += acc_y - acc_y_before;  
  }else{
    ret_acc  += acc_y_before - acc_y;
  }
  if(acc_z > acc_z_before){
    ret_acc  += acc_z - acc_z_before;  
  }else{
    ret_acc  += acc_z_before - acc_z;
  }

  //Gyr計算
  if(gyro_x > gyro_x_before){
    ret_gyro  += gyro_x - gyro_x_before;  
  }else{
    ret_gyro  += gyro_x_before - gyro_x;
  }
  if(gyro_y > gyro_y_before){
    ret_gyro  += gyro_y - gyro_y_before;  
  }else{
    ret_gyro  += gyro_y_before - gyro_y;
  }
  if(gyro_z > gyro_z_before){
    ret_gyro  += gyro_z - gyro_z_before;  
  }else{
    ret_gyro  += gyro_z_before - gyro_z;
  }

  acc_x_before = acc_x;
  acc_y_before = acc_y;
  acc_z_before = acc_z;

  gyro_x_before = gyro_x;
  gyro_y_before = gyro_y;
  gyro_z_before = gyro_z;

  *acc_value = ret_acc;//(uint16_t)ret_acc;
  *gyro_value = ret_gyro;//(uint16_t)ret_gyro;
}

// MPU6050_read
int MPU6050_read(int start, uint8_t *buffer, int size) {
  int i, n, error;
  Wire.beginTransmission(MPU6050_I2C_ADDRESS);
  n = Wire.write(start);
  if (n != 1) {
    return (-10);
  }
  n = Wire.endTransmission(false);// hold the I2C-bus
  if (n != 0) {
    return (n);
  }
  // Third parameter is true: relase I2C-bus after data is read.
  Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true);
  i = 0;
  while (Wire.available() && i < size) {
    buffer[i++] = Wire.read();
  }
  if ( i != size) {
    return (-11);
  }
  return (0); // return : no error
}

// MPU6050_write
int MPU6050_write(int start, const uint8_t *pData, int size) {
  int n, error;
  Wire.beginTransmission(MPU6050_I2C_ADDRESS);
  n = Wire.write(start);// write the start address
  if (n != 1) {
    return (-20);
  }
  n = Wire.write(pData, size);// write data bytes
  if (n != size) {
    return (-21);
  }
  error = Wire.endTransmission(true); // release the I2C-bus
  if (error != 0) {
    return (error);
  }
  return (0);// return : no error
}

// MPU6050_write_reg
int MPU6050_write_reg(int reg, uint8_t data) {
  int error;
  error = MPU6050_write(reg, &data, 1);
  Serial.print("error = ");
  Serial.println(error);
  return (error);
};

TKS

TKS さんが 2021/05/02 に編集をしました。 (メッセージ: 初版)
TKS さんが 2021/05/02 に編集をしました。
TKS さんが 2021/05/02 に編集をしました。
TKS さんが 2021/05/03 に編集をしました。
TKS さんが 2021/05/03 に編集をしました。
TKS さんが 2021/05/03 に編集をしました。
TKS さんが 2021/05/03 に編集をしました。
TKS さんが 2021/05/03 に編集をしました。
TKS さんが 2021/05/03 に編集をしました。
TKS さんが 2021/05/03 に編集をしました。
TKS さんが 2021/05/03 に編集をしました。
TKS さんが 2021/05/09 に編集をしました。
TKS さんが 2021/05/09 に編集をしました。
TKS さんが 2021/05/09 に編集をしました。
TKS さんが 2021/05/09 に編集をしました。
TKS さんが 2021/05/09 に編集をしました。
TKS さんが 2021/05/09 に編集をしました。

ログインしてコメントを投稿する