SPRESENSEを用いたパーソナルモビリティのため路面状態調査IoTシステム

本作品は、ベビーカー・車椅子などのパーソナルモビリティ、もしくは、スーツケース・台車などのパーソナルキャリアのユーザのために、通った道がどの様な路面状態であるか、情報収集することができるIoTシステムです。
皆様はベビーカーやスーツケースがうまく進まなくて困った経験がございませんか？その原因は、歩道の路面の状態が悪いことに起因することが多いです。具体的には、アスファルトに穴やひび割れが開いていて起伏が激しい箇所、通路なのに歩道と道路のつなぎ目の段差が高い縁石が多い箇所、アスファルト舗装がされていない砂利道などで、うまく進まなくなることが多いです。本システムでは、そういった悪路を、音と振動をセンシングすることで検出してGPS情報と共にデータを収集していくシステムです。
本システムをモビリティ・キャリアに設置して路面を通行時に収集した情報を蓄積し、地図上に悪路か悪路でないかを数値を記録、マッピングしていくことで、どの道を通ると心地よく移動することができるか、デジタルデータをもとに判断することができます。

私が路面の状態で一番苦労したのはベビーカーを押すときだったので、本来ならばベビーカーに設置して検証したかったのですが、既に子供も大きくなり手放してしまったため、今回はスーツケースに取り付け、動作確認を行いました。

【使用した機材】
①SPRESENSE Main board
②SPRESENSE EXTENSION board
③Mic-kit for SPRESENSE_LTE
④SPRESENSE SENSOR-EVK−701
⑤SPRESENSE用ELTRESアドオンボード
　・GSP Antenna
　・LPWA Antenna
　・USBケーブル
　・USB出力モバイルバッテリー

【簡単な動作の仕組み】
①Spresenseメインボード＋②拡張ボード＋③マイク拡張ボード＋④加速度センサ＋⑤ELTRESアドオンボードで構成されるIoTシステムを、計測したいモビリティ（ベビーカーなど）・キャリア（スーツケースなど）に取り付けます。

モビリティ・キャリアなどを走行させ、⑤で測位をしながら、③で音を測定、④で振動状態を計測し、②を介して①で情報を集約してエッジ処理をした後、⑤でELTRES（SONYが提唱するLPWA規格）の通信を経由してクラウドにUPLOADします。
ELTRESのクラウドビューア経由で、蓄積した情報の閲覧・ダウンロードができます。

【センシングする物理量】
・位置情報・時間・衛星補足数（GPS）
・音
・加速度

ペイロードとは、ELTRESで1分毎に送信する情報です。16byteという制限の中に詰め込む情報を設計しました。

【ELTRESで1分毎に送信する情報に含むもの】
・GPS測位情報（緯度・経度・電波品質）
・音をFFTスペクトル解析し、ピーク周波数付近のスペクトルの最大値（前回送信時以降の最大値）
・音と加速度が一定値以上であったときをカウント（0.6秒間隔でカウント、1分間で最大100カウント）

【ELTRESのペイロードの構造（16byte：128bit）】
AA BBBBBBBB CCCCCCCCCC DDDDDDDD EE FF

AA　　　　　　識別コード
BBBBBBBB　　緯度（BCD）
CCCCCCCCCC　経度（BCD）
DDDDDDDD　　振動指数
EE　　　　　　　悪路検出カウント(0.6s毎)
FF　　　　　　　電波品質

##システムの動作事例
【実際に送信したペイロードの例】
（シリアルモニタ @ PC）
22:03:30.206 -> setup_payload_gps_maxvalue_count_th
lat:3546.XXX4,lon:13931YYY6,maxValue_max:7.81,count_th:6,pos:1
※XXX・YYYはダミーです

【クラウドで実際に受信したペイロード】
（Clip Viwer Lite @ クラウド）
81 3546XXX4 013931YYY6 40f9da59 06 01
※XXX・YYYはダミーです

AA　　　　　　識別コード　　0x81　
BBBBBBBB　　緯度（BCD）　3546XXX4　※XXXはダミーです
CCCCCCCCCC　経度（BCD）　013931YYY6　※YYYはダミーです
DDDDDDDD　　振動指数　　　40f9da59　10進で示すと、7.80790376663208
EE　　　　　　悪路検出カウント(0.6s毎)　0x06
FF　　　　　　電波品質　　　0x01

両者とも合致していることが確認できます。

【シリアルモニタの例】
シリアルモニタでは、センシングの内容と悪路検出のカウントの様子が確認できるように表示しています。
また、ペイロードをセットした様子、セット後にカウントをリセットする様子も確認できます。

22:01:30.101 -> peak freq: 0.00 HzSpectrum: 0.02 Spectrum: 16.09
22:01:30.101 -> [measure]acc_x_hf:0.015 mg, acc_y_hf:0.392 mg, acc_z_hf:0.231 mg
22:01:30.101 -> powerX2: 0.00, powerY2: 0.15, powerZ2: 0.05, ThValue2: 2500.00
22:01:30.101 -> detectThrethold_value: 0
22:01:30.101 -> count_th: 26
22:01:30.101 -> peak freq: 0.00 HzSpectrum: 0.03 Spectrum: 16.09
22:01:30.101 -> Shortly before sending, so setup payload if need.
22:01:30.101 -> [measure]acc_x_hf:0.012 mg, acc_y_hf:0.326 mg, acc_z_hf:0.192 mg
22:01:30.148 -> powerX2: 0.00, powerY2: 0.10, powerZ2: 0.03, ThValue2: 2500.00
22:01:30.148 -> detectThrethold_value: 0
22:01:30.148 -> count_th: 26
22:01:30.253 -> setup_payload_gps_maxvalue_count_th lat:3546.XXX3,lon:13931.XXX0,maxValue_max:16.09,count_th:26,pos:1
22:01:30.354 -> peak freq: 0.00 HzSpectrum: 0.02 Spectrum: 0.02
22:01:30.354 -> [measure]acc_x_hf:0.006 mg, acc_y_hf:0.009 mg, acc_z_hf:0.008 mg
22:01:30.354 -> powerX2: 0.00, powerY2: 0.00, powerZ2: 0.00, ThValue2: 2500.00
22:01:30.354 -> detectThrethold_value: 0
22:01:30.354 -> count_th: 0

【Clip Viewer Liteの画面】
位置情報は一部隠しておりますが、本のシステムが送信した結果が反映されていることが確認できております。

そのほか、GPSの位置情報を地図上にマッピングした結果などを表示したかったのですが、自宅周辺の地域情報が漏洩する恐れがあるため、今回は掲載を割愛します。

【工夫したところ】
・ELTRESのペイロードが16byteに限られているので、16byteでできるだけ必要な情報を送信できるように集約することを心掛けました。
・音と加速度をセンシングして、両方でそれぞれ閾値以上の大きな値が得られた場合を悪路検出としてカウントすることとしています。2つの物理量の情報をもとに地面を移動するときの振動をカウントするので、誤動作が少なくできます。
・悪路検出カウントは、0.6秒毎にカウントされます。1分毎にデータを送信するため、1分で最大100カウントになります。パーセンテージで数値で換算できるので、100%悪路・50%悪路など、人間にとってわかりやすい値で確認できます。
・マイクは位置によって音圧が変わりやすいため、ソフト側で閾値の値を変えられるだけでなく、クリップ式のマイクでケースから取り出して音が出る接地面（車輪など）に近い位置に設置できるようにしています。測定対象が変わっても設置の自由度が高く、ソフト側・ハード側の両面で調整が可能です。
・ELTRESで通信しているため、非常に低電力です。また、無線LANなどがない場所でも、クラウドに情報を送ることができます。

【今後の課題・今回盛り込めなかったこと】
・カメラで路面を撮影してELTRESで送信すること。本作品検討当初はカメラモジュールも使って路面を撮影した画像もアップロードしたいと検討・計画しておりましたが、ELTRESのペイロードが小さく、センシングデータや位置情報とともに1分毎に送信するのは困難であったため断念しました。
・SPRESENSEのSubCoreの活用やMicroSDへのデータ書き込みをすること。本作品検討当初は、SubCoreに様々な機能を割り振って利用しようと考えておりましたが、カメラや音のキャプチャ、MicroSDへの書き込みなど、SubCoreでは簡単に利用できない機能も多く、作品製作期間中に使い分け・使いこなしが十分にできずに断念しました。もう少し試行錯誤して使いこなせるようになりたいと思います。
・ELTRESで収集したデータをWebで見やすく表示すること。本作品検討当初は、Clip Viewer LiteのWeb画面で表示データ部分を編集して見やすくできることを期待しておりました。しかし、Clip Viewer Liteはプリセットから選ぶことのみが可能で、Web画面に表示する内容の編集は簡単には行うことができませんでした。そのため、今回はデータの通信と蓄積ツールとして主に利用しました。Clip Viewer Liteの今後の機能拡張に期待いたします。

【今後の期待・さらなる活用案】
・自分用のデータ収集にとどまらず、取得した情報をシェア・公開することで、行ったことがない・通ったことがない・土地勘のない道でも、乗り心地が良いルートを選んで走行することができる様になることを期待しています。安全・安心に移動ができ、移動することの不安を解消し、外出すること自体がより楽しくなることを期待しています。
・道路や歩道を管理する地方自治体が路面状態の把握のために本システムの情報を活用し、路面の舗装工事・修復工事の優先順位を決めるなど、地域連携が促進されるなどが期待できます。

・SPRESENSEではじめるローパワーエッジAI-Make-PROJECTS-ソニーセミコンダクタソリューションズ株式会社
　音のFFT解析・スペクトラム算出
・M5Stackをつかってライトセーバーを作る
　重力加速度の除去・加速度の検出
・CLIP Viewer Lite内のコンテンツ
　ELTRESの通信理解・ペイロード設計


SPRESENSE‗MainCore
#include <EltresAddonBoard.h>
#include <Audio.h>
#include <FFT.h>
#include <SDHCI.h>
SDClass SD;

#include <Wire.h>
// KX126用ライブラリのインクルード
#include <KX126.h>
#include <Filters.h>

// PIN定義：LED(プログラム状態)
#define LED_RUN PIN_LED0
// PIN定義：LED(GNSS電波状態)
#define LED_GNSS PIN_LED1
// PIN定義：LED(ELTRES状態)
#define LED_SND PIN_LED2
// PIN定義：LED(エラー状態)
#define LED_ERR PIN_LED3

// プログラム内部状態：初期状態
#define PROGRAM_STS_INIT      (0)
// プログラム内部状態：起動中
#define PROGRAM_STS_RUNNING   (1)
// プログラム内部状態：終了
#define PROGRAM_STS_STOPPED   (3)

// プログラム内部状態
int program_sts = PROGRAM_STS_INIT;
// GNSS電波受信タイムアウト（GNSS受信エラー）発生フラグ
bool gnss_recevie_timeout = false;
// 点滅処理で最後に変更した時間
uint64_t last_change_blink_time = 0;
// イベント通知での送信直前通知（5秒前）受信フラグ
bool event_send_ready = false;
// ペイロードデータ格納場所
uint8_t payload[16];

// 最新のGGA情報
eltres_board_gga_info last_gga_info;

// KX126用インスタンス
KX126 kx126(KX126_DEVICE_ADDRESS_1F);
// 最新値（加速度[mg]：x軸）
float last_acc_x = 0;
float last_acc_x_hf = 0;
// 最新値（加速度[mg]：y軸）
float last_acc_y = 0;
float last_acc_y_hf = 0;
// 最新値（加速度[mg]：z軸）
float last_acc_z = 0;
float last_acc_z_hf = 0;

#define FFT_LEN 1024
#define AVG_FILTER (8)

// モノラル、1024サンプルでFFTを初期化
FFTClass<AS_CHANNEL_MONO, FFT_LEN> FFT; 
AudioClass *theAudio = AudioClass::getInstance();

FilterOnePole filterOneHighpassX;
FilterOnePole filterOneHighpassY;
FilterOnePole filterOneHighpassZ;

//FFT用
float maxvalue_th = 1.0;
float maxValue = 0.0;
float maxValue_max = 0.0;
float peakFs = 0.0;

float detectBeginValue_spectrum = 1.0;
float detectEndValue_spectrum = 0.5;

//加速度用
boolean detectThrethold_value = false;
float detectBeginValue = 1.2;
float detectEndValue = 0.7;
boolean IsDetectPower = true;

float acc_th = 50.0;

//カウント用
uint64_t startTime;  
uint64_t currentTime;
const uint64_t time_th = 600;

uint16_t count_th = 0; 

/**
 * @brief イベント通知受信コールバック
 * @param event イベント種別
 */
void eltres_event_cb(eltres_board_event event) {
  switch (event) {
  case ELTRES_BOARD_EVT_GNSS_TMOUT:
    // GNSS電波受信タイムアウト
    Serial.println("gnss wait timeout error.");
    gnss_recevie_timeout = true;
    break;
  case ELTRES_BOARD_EVT_IDLE:
    // アイドル状態
    Serial.println("waiting sending timings.");
    digitalWrite(LED_SND, LOW);
    break;
  case ELTRES_BOARD_EVT_SEND_READY:
    // 送信直前通知（5秒前）
    Serial.println("Shortly before sending, so setup payload if need.");
    event_send_ready = true;
    break;
  case ELTRES_BOARD_EVT_SENDING:
    // 送信開始
    Serial.println("start sending.");
    digitalWrite(LED_SND, HIGH);
    break;
  case ELTRES_BOARD_EVT_GNSS_UNRECEIVE:
    // GNSS電波未受信
    Serial.println("gnss wave has not been received.");
    digitalWrite(LED_GNSS, LOW);
    break;
  case ELTRES_BOARD_EVT_GNSS_RECEIVE:
    // GNSS電波受信
    Serial.println("gnss wave has been received.");
    digitalWrite(LED_GNSS, HIGH);
    gnss_recevie_timeout = false;
    break;
  case ELTRES_BOARD_EVT_FAULT:
    // 内部エラー発生
    Serial.println("internal error.");
    break;
  }
}

/**
 * @brief GGA情報受信コールバック
 * @param gga_info GGA情報のポインタ
 */
void gga_event_cb(const eltres_board_gga_info *gga_info) {
  Serial.print("[gga]");
  last_gga_info = *gga_info;
  if (gga_info->m_pos_status) {
    // 測位状態
    // GGA情報をシリアルモニタへ出力
    Serial.print("utc: ");
    Serial.println((const char *)gga_info->m_utc);
    Serial.print("lat: ");
    Serial.print((const char *)gga_info->m_n_s);
    Serial.print((const char *)gga_info->m_lat);
    Serial.print(", lon: ");
    Serial.print((const char *)gga_info->m_e_w);
    Serial.println((const char *)gga_info->m_lon);
    Serial.print("pos_status: ");
    Serial.print(gga_info->m_pos_status);
    Serial.print(", sat_used: ");
    Serial.println(gga_info->m_sat_used);
    Serial.print("hdop: ");
    Serial.print(gga_info->m_hdop);
    Serial.print(", height: ");
    Serial.print(gga_info->m_height);
    Serial.print(" m, geoid: ");
    Serial.print(gga_info->m_geoid);
    Serial.println(" m");
  } else {
    // 非測位状態
    // "invalid data"をシリアルモニタへ出力
    Serial.println("invalid data.");
  }
}

//閾値以上の加速度検知
boolean DetectThrethold(float acc_x_hf, float acc_y_hf, float acc_z_hf, float detectPowerValue)
{  
   float powerX2 = acc_x_hf * acc_x_hf;
   float powerY2 = acc_y_hf * acc_y_hf;
   float powerZ2 = acc_z_hf * acc_z_hf;
   float powerValue2 = detectPowerValue * detectPowerValue;

    Serial.print("powerX2: " + String(powerX2) + ",   ");
    Serial.print("powerY2: " + String(powerY2) + ",   ");
    Serial.print("powerZ2: " + String(powerZ2) + ",   ");
    Serial.println("ThValue2: " + String(powerValue2) + "   ");

   if(powerX2 > powerY2 && powerX2 > powerZ2){
      if(powerX2 > powerValue2){
         Serial.print("powerX2: " + String(powerX2) + ",   ");
         Serial.println("ThValue2: " + String(powerValue2) + "   ");
         return true;
      }
   }
   else if (powerY2 > powerX2 && powerY2 > powerZ2){
      if (powerY2 > powerValue2){
         Serial.print("powerY2: " + String(powerY2) + ",   ");
         Serial.println("ThValue2: " + String(powerValue2) + "   ");
         return true;
      }
  }
  else if (powerZ2 > powerY2 && powerZ2 > powerX2){
     if (powerZ2 > powerValue2){
         Serial.print("powerZ2: " + String(powerZ2) + ",   ");
         Serial.println("ThValue2: " + String(powerValue2) + "   ");
         return true;
     }
  }
  return false;
}

void setup_payload_gps_maxvalue_count_th(float maxValue_max, uint16_t count_th){
  String lat_string = String((char*)last_gga_info.m_lat);
  String lon_string = String((char*)last_gga_info.m_lon);
  int index;
  uint32_t gnss_time;
  uint32_t utc_time;

  // GNSS時刻(epoch秒)の取得
  EltresAddonBoard.get_gnss_time(&gnss_time);
  // UTC時刻を計算（閏秒補正）
  utc_time = gnss_time - 18;

  // 設定情報をシリアルモニタへ出力
  Serial.print("[setup_payload_gps_maxvalue_count_th]");
  Serial.print("(utc:");
  Serial.print(utc_time);
  Serial.print(") lat:");
  Serial.print(lat_string);
  Serial.print(",lon:");
  Serial.print(lon_string);
  Serial.print(",maxValue_max:");
  Serial.print(maxValue_max);
  Serial.print(",count_th:");
  Serial.print(count_th);
  Serial.print(",pos:");
  Serial.print(last_gga_info.m_pos_status);
  Serial.println();

  // ペイロード領域初期化
  memset(payload, 0x00, sizeof(payload));
  // ペイロード種別[GPSペイロード]設定
  payload[0] = 0x81;
  // 緯度設定
  index = 0;
  payload[1] = (uint8_t)(((lat_string.substring(index,index+1).toInt() << 4)
                + lat_string.substring(index+1,index+2).toInt()) & 0xff);
  index += 2;
  payload[2] = (uint8_t)(((lat_string.substring(index,index+1).toInt() << 4)
                + lat_string.substring(index+1,index+2).toInt()) & 0xff);
  index += 2;
  index += 1;   // skip "."
  payload[3] = (uint8_t)(((lat_string.substring(index,index+1).toInt() << 4)
                + lat_string.substring(index+1,index+2).toInt()) & 0xff);
  index += 2;
  payload[4] = (uint8_t)(((lat_string.substring(index,index+1).toInt() << 4)
                + lat_string.substring(index+1,index+2).toInt()) & 0xff);
  // 経度設定
  index = 0;
  payload[5] = (uint8_t)(lon_string.substring(index,index+1).toInt() & 0xff);
  index += 1;
  payload[6] = (uint8_t)(((lon_string.substring(index,index+1).toInt() << 4)
                + lon_string.substring(index+1,index+2).toInt()) & 0xff);
  index += 2;
  payload[7] = (uint8_t)(((lon_string.substring(index,index+1).toInt() << 4)
                + lon_string.substring(index+1,index+2).toInt()) & 0xff);
  index += 2;
  index += 1;   // skip "."
  payload[8] = (uint8_t)(((lon_string.substring(index,index+1).toInt() << 4)
                + lon_string.substring(index+1,index+2).toInt()) & 0xff);
  index += 2;
  payload[9] = (uint8_t)(((lon_string.substring(index,index+1).toInt() << 4)
                + lon_string.substring(index+1,index+2).toInt()) & 0xff);
  // 時刻(EPOCH秒)設定
//  payload[10] = (uint8_t)((utc_time >> 24) & 0xff);
//  payload[11] = (uint8_t)((utc_time >> 16) & 0xff);
//  payload[12] = (uint8_t)((utc_time >> 8) & 0xff);
//  payload[13] = (uint8_t)(utc_time & 0xff);

//  // 拡張用領域(0固定)設定
//  payload[14] = 0x00;
//  // 品質設定
//  payload[15] = last_gga_info.m_pos_status;

  //float maxValue_maxをセット
  uint32_t raw;
  raw = *((uint32_t*)&maxValue_max);
  payload[10] = (uint8_t)((raw >> 24) & 0xff);
  payload[11] = (uint8_t)((raw >> 16) & 0xff);
  payload[12] = (uint8_t)((raw >> 8) & 0xff);
  payload[13] = (uint8_t)((raw >> 0) & 0xff);

  //uint16_t count_th をセット（下1byte）
  payload[14] = (uint8_t)(count_th & 0xff);

  // 品質設定
  payload[15] = last_gga_info.m_pos_status;
  }


/**
 * @brief setup()関数
 */
void setup() {
  // シリアルモニタ出力設定
  Serial.begin(115200);

  // LED初期設定
  pinMode(LED_RUN, OUTPUT);
  digitalWrite(LED_RUN, HIGH);
  pinMode(LED_GNSS, OUTPUT);
  digitalWrite(LED_GNSS, LOW);
  pinMode(LED_SND, OUTPUT);
  digitalWrite(LED_SND, LOW);
  pinMode(LED_ERR, OUTPUT);
  digitalWrite(LED_ERR, LOW);

  // ELTRES起動処理
  eltres_board_result ret_el = EltresAddonBoard.begin(ELTRES_BOARD_SEND_MODE_1MIN,eltres_event_cb, gga_event_cb);
  if (ret_el != ELTRES_BOARD_RESULT_OK) {
    // ELTRESエラー発生
    digitalWrite(LED_RUN, LOW);
    digitalWrite(LED_ERR, HIGH);
    program_sts = PROGRAM_STS_STOPPED;
    Serial.print("cannot start eltres board (");
    Serial.print(ret_el);
    Serial.println(").");
    return;
  }

  while (!SD.begin()) { Serial.println("Insert SD card"); }

  // ハミング窓、モノラル、オーバーラップ50%
  FFT.begin(WindowHamming, AS_CHANNEL_MONO, (FFT_LEN/2));

  Serial.println("Init Audio Recorder");
  theAudio->begin();
  // 入力をマイクに設定
  theAudio->setRecorderMode(AS_SETRECDR_STS_INPUTDEVICE_MIC);
  // 録音設定：PCM (16ビットRAWデータ)、
  // DSPコーデックの場所の指定 (SDカード上のBINディレクトリ)、
  // サンプリグレート 48000Hz、モノラル入力
  int err = theAudio->initRecorder(AS_CODECTYPE_PCM 
    ,"/mnt/sd0/BIN", AS_SAMPLINGRATE_48000, AS_CHANNEL_MONO);                             
  if (err != AUDIOLIB_ECODE_OK) {
    Serial.println("Recorder initialize error");
    while(1);
  }

  Serial.println("Start Recorder");
  theAudio->startRecorder(); // 録音開始

  // 加速度センサ初期設定
  Wire.begin();
  byte rc;
  rc = kx126.init();
  if (rc != 0) {
    // センサ初期設定エラー
    EltresAddonBoard.end();
    digitalWrite(LED_RUN, LOW);
    digitalWrite(LED_ERR, HIGH);
    program_sts = PROGRAM_STS_STOPPED;
    Serial.print("cannnot start sensor.(");
    Serial.print(rc);
    Serial.println(")");
    return;
  }

  //フィルタ設定
  float filterFrequency = 2;
  float windowLength = 20.0/filterFrequency;
  filterOneHighpassX = FilterOnePole(HIGHPASS,filterFrequency,0);
  filterOneHighpassY = FilterOnePole(HIGHPASS,filterFrequency,0);
  filterOneHighpassZ = FilterOnePole(HIGHPASS,filterFrequency,0);

  // 正常
  program_sts = PROGRAM_STS_RUNNING;
}

/**
 * @brief loop()関数
 */
void loop() {

  static const uint32_t buffering_time = FFT_LEN*1000/AS_SAMPLINGRATE_48000;
  static const uint32_t buffer_size = FFT_LEN*sizeof(int16_t);
  static const int ch_index = AS_CHANNEL_MONO-1;
  static char buff[buffer_size];
  static float pDst[FFT_LEN];
  uint32_t read_size;
  
  // buffer_sizeで要求されたデータをbuffに格納する
  // 読み込みできたデータ量は read_size に設定される
  int ret_au = theAudio->readFrames(buff, buffer_size, &read_size);
  if (ret_au != AUDIOLIB_ECODE_OK && ret_au != AUDIOLIB_ECODE_INSUFFICIENT_BUFFER_AREA) {
    Serial.println("Error err = " + String(ret_au));
    theAudio->stopRecorder();
    while(1);
  }

  switch (program_sts) {
    case PROGRAM_STS_RUNNING:
      // プログラム内部状態：起動中
      if (gnss_recevie_timeout) {
        // GNSS電波受信タイムアウト（GNSS受信エラー）時の点滅処理
        uint64_t now_time = millis();
        if ((now_time - last_change_blink_time) >= 1000) {
          last_change_blink_time = now_time;
          bool set_value = digitalRead(LED_ERR);
          bool next_value = (set_value == LOW) ? HIGH : LOW;
          digitalWrite(LED_ERR, next_value);
        }
      } else {        
        digitalWrite(LED_ERR, LOW);
      }

      if (event_send_ready) {
        // 送信直前通知時の処理
        event_send_ready = false;
        setup_payload_gps_maxvalue_count_th(maxValue_max,count_th);

        // 送信ペイロードの設定
        EltresAddonBoard.set_payload(payload);

        // count_thの数値、maxValue_maxの数値をリセット
        count_th = 0; //add
        maxValue_max = 0;      
      }
      
      //マイク入力　FFT
      // 読み込みサイズがbuffer_sizeに満たない場合
      if (read_size < buffer_size) {
        delay(buffering_time); // データが蓄積されるまで待つ
        return;
      } 

      FFT.put((q15_t*)buff, FFT_LEN);  // FFTを実行
      FFT.get(pDst, ch_index);  // チャンネル0番の演算結果を取得
#ifdef AVG_FILTER
      avgFilter(pDst);  // 周波数データを平滑化
#endif
      // 周波数と最大値の近似値を算出
      peakFs = get_peak_frequency(pDst, &maxValue);
      if(maxValue>maxValue_max){
        maxValue_max = maxValue;
      }
      
      Serial.print("peak freq: " + String(peakFs) + " Hz");
      Serial.print("Spectrum: " + String(maxValue)+ "   ");
      Serial.println("Spectrum: " + String(maxValue_max));
      
      // 加速度センサから測定値を取得し、最新値を更新
      measure_acceleration();
      detectThrethold_value = DetectThrethold(last_acc_x_hf, last_acc_y_hf, last_acc_z_hf, acc_th);
      Serial.print(" detectThrethold_value: ");
      Serial.print(detectThrethold_value);
      Serial.println("  ");      
      if(maxValue > maxvalue_th && detectThrethold_value){
        currentTime = millis();
        if (currentTime - startTime >= time_th){
          //カウント時の処理
          count_th = count_th +1;
          Serial.println("count_th: " + String(count_th) + " ");
          //LED3点滅（カウントに応じて明暗する 実際には一瞬光る：if(gnss_recevie_timeout) のelse処理で強制Low化）
          bool set_value_count = digitalRead(LED_ERR);
          bool next_value_count = (set_value_count == LOW) ? HIGH : LOW;
          digitalWrite(LED_ERR, next_value_count);
          startTime = currentTime;
        }else{
          Serial.println("count_th: " + String(count_th) + " uncount");
        }
      }else{
        Serial.println("count_th: " + String(count_th) + " ");    
      }
   
      break;
     
    case PROGRAM_STS_STOPPED:
      // プログラム内部状態：終了
      break;
  }

  // 次のループ処理まで待機
//  delay(100);
  delay(10);

}

/**
 * @brief 加速度ペイロード設定
 * @param acc_x 加速度[mg]（x軸）
 * @param acc_y 加速度[mg]（y軸）
 * @param acc_z 加速度[mg]（z軸）
 */
//void setup_payload_acceleration(float acc_x, float acc_y, float acc_z) {
void setup_payload_acceleration(float last_acc_x_hf, float last_acc_y_hf, float last_acc_z_hf) {

  // 設定情報をシリアルモニタへ出力
  Serial.print("[setup_payload_acceleration]");
  Serial.print("last_acc_x_hf:");
  Serial.print(last_acc_x_hf, 3);
  Serial.print(" mg, last_acc_y_hf:");
  Serial.print(last_acc_y_hf, 3);
  Serial.print(" mg, last_acc_z_hf:");
  Serial.print(last_acc_z_hf, 3);
  Serial.print(" mg");
  Serial.println();

  // ペイロード領域初期化
  memset(payload, 0x00, sizeof(payload));
  // ペイロード種別[加速度ペイロード]設定
  payload[0] = 0x83;
  // x軸
  uint32_t raw;
  raw = *((uint32_t*)&last_acc_x_hf);
  payload[1] = (uint8_t)((raw >> 24) & 0xff);
  payload[2] = (uint8_t)((raw >> 16) & 0xff);
  payload[3] = (uint8_t)((raw >> 8) & 0xff);
  payload[4] = (uint8_t)((raw >> 0) & 0xff);
  // y軸
  raw = *((uint32_t*)&last_acc_y_hf);
  payload[5] = (uint8_t)((raw >> 24) & 0xff);
  payload[6] = (uint8_t)((raw >> 16) & 0xff);
  payload[7] = (uint8_t)((raw >> 8) & 0xff);
  payload[8] = (uint8_t)((raw >> 0) & 0xff);
  // z軸
  raw = *((uint32_t*)&last_acc_z_hf);
  payload[9] = (uint8_t)((raw >> 24) & 0xff);
  payload[10] = (uint8_t)((raw >> 16) & 0xff);
  payload[11] = (uint8_t)((raw >> 8) & 0xff);
  payload[12] = (uint8_t)((raw >> 0) & 0xff);
}

/**
 * @brief 加速度センサから加速度を取得し、最新値を更新
 */
void measure_acceleration(void) {
  byte rc;
  float acc[3];

  // 加速度センサから測定値(単位:g)を取得
  rc = kx126.get_val(acc);
  if (rc != 0) {
    Serial.print("cannot read measurement (");
    Serial.print(rc);
    Serial.println(")");
    return;
  }
  // 最新値の更新
  last_acc_x = acc[0] * 1000;
  last_acc_y = acc[1] * 1000;
  last_acc_z = acc[2] * 1000;

  // 最新値をシリアルモニタへ出力
//  Serial.print("[measure]acc_x:");
//  Serial.print(last_acc_x, 3);
//  Serial.print(" mg, acc_y:");
//  Serial.print(last_acc_y, 3);
//  Serial.print(" mg, acc_z:");
//  Serial.print(last_acc_z, 3);
//  Serial.print(" mg");
//  Serial.println();

  filterOneHighpassX.input(last_acc_x);
//  float last_acc_x_hf = filterOneHighpassX.output();
  last_acc_x_hf = filterOneHighpassX.output();
  filterOneHighpassY.input(last_acc_y);
//  float last_acc_y_hf = filterOneHighpassY.output();
  last_acc_y_hf = filterOneHighpassY.output();
  filterOneHighpassZ.input(last_acc_z);
//  float last_acc_z_hf = filterOneHighpassZ.output();
  last_acc_z_hf = filterOneHighpassZ.output();

  
  // 最新値をシリアルモニタへ出力
  Serial.print("[measure]acc_x_hf:");
  Serial.print(last_acc_x_hf, 3);
  Serial.print(" mg, acc_y_hf:");
  Serial.print(last_acc_y_hf, 3);
  Serial.print(" mg, acc_z_hf:");
  Serial.print(last_acc_z_hf, 3);
  Serial.print(" mg");
  Serial.println();
}

#ifdef AVG_FILTER
void avgFilter(float dst[FFT_LEN]) {
  static float pAvg[AVG_FILTER][FFT_LEN/2];
  static int g_counter = 0;
  if (g_counter == AVG_FILTER) g_counter = 0;
  for (int i = 0; i < FFT_LEN/2; ++i) {
    pAvg[g_counter][i] = dst[i];
    float sum = 0;
    for (int j = 0; j < AVG_FILTER; ++j) {
      sum += pAvg[j][i];
    }
    dst[i] = sum / AVG_FILTER;
  }
  ++g_counter;
}
#endif

float get_peak_frequency(float *pData, float* maxValue) {
  uint32_t idx;
  float delta, delta_spr;
  float peakFs;
  // 周波数分解能(delta)を算出
  const float delta_f = AS_SAMPLINGRATE_48000/FFT_LEN;

  // 最大値と最大値のインデックスを取得
  arm_max_f32(pData, FFT_LEN/2, maxValue, &idx);
  if (idx < 1) return 0.0;

  // 周波数のピークの近似値を算出
  delta = 0.5*(pData[idx-1]-pData[idx+1])
    /(pData[idx-1]+pData[idx+1]-(2.*pData[idx]));
  peakFs = (idx + delta) * delta_f;

  // スペクトルの最大値の近似値を算出
  delta_spr = 0.125*(pData[idx-1]-pData[idx+1])
    *(pData[idx-1]-pData[idx+1])
    /(2.*pData[idx]-(pData[idx-1]+pData[idx+1]));
  *maxValue += delta_spr;
  return peakFs;
}

MasaOhta

MasaOhta さんが 2022/09/26 に編集をしました。 (メッセージ: 初版)
MasaOhta さんが 2022/09/26 に編集をしました。
MasaOhta さんが 2022/09/26 に編集をしました。
MasaOhta さんが 2022/09/26 に編集をしました。
MasaOhta さんが 2022/09/26 に編集をしました。

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

SPRESENSEを用いたパーソナルモビリティのため路面状態調査IoTシステム

【作品概要（2022 SPRESENSE™ 活用コンテスト応募作品）】

設置した様子

機材と動作の仕組み

ペイロードの設計

製作を通して（工夫・課題・期待）

参考にした書籍・Webサイト

ソースコード