SPRESENSE のハイレゾ録音機能で超音波測距をやってみた

SPRESENSEには192kHzで録音再生できるハイレゾ機能があります。192kHzということは超音波を扱えるわけです。ということは巷に出回っている超音波測距センサーをSPRESENSEで制御できるのではないか？と思い、超音波トランスデューサとレシーバを入手し、試してみようと思いました。

超音波トランスデューサ、レシーバを準備する

超音波トランスデューサとレシーバは超音波測距センサーでよく使われているアレです。単体でも売っているんですね。超音波センサーとして買うよりもちょっと安い程度でしょうか。でも単体だとセンサーの素性がよく分かるので勉強になります。

超音波変換器 TCT40-16R/T

SPERESENSEに接続する

超音波トランスデューサとレシーバとSPRESENSEの結線は、こちらのサイトを参考にしました。簡単に接続できるので半田も楽ちんです。

Spresenseではじめるリアルタイム信号処理プログラミング

SPRESENSEのスケッチ

SPRESENSEのスケッチも上記 GitHub にあるので参考にしました。ただし、今回は距離を測るのが目的で、再生は必要ないので Mixer の部分は削除しています。

#include <FrontEnd.h>
#include <MemoryUtil.h>
#include <arch/board/board.h>

#define SAMPLE_SIZE (1024)

//*****
#include <sys/ioctl.h>
#include <stdio.h>
#include <fcntl.h>   
#include <nuttx/timers/pwm.h>
int fd;
struct pwm_info_s info;
//*****

//***
uint32_t fire_time = 0;
uint32_t last_time = 0;
uint32_t current_time = 0;
//***

FrontEnd *theFrontEnd;

const int32_t channel_num = AS_CHANNEL_MONO;
const int32_t bit_length  = AS_BITLENGTH_16;
const int32_t sample_size = SAMPLE_SIZE;
const int32_t frame_size  = sample_size * (bit_length / 8) * channel_num;
bool isErr = false;

void frontend_attention_cb(const ErrorAttentionParam *param) {
  Serial.println("ERROR: Attention! Something happened at FrontEnd");
  if (param->error_code >= AS_ATTENTION_CODE_WARNING) isErr = true;
}

static bool frontend_done_cb(AsMicFrontendEvent ev, uint32_t result, uint32_t detail){
  UNUSED(ev);  UNUSED(result);  UNUSED(detail);  
  return true;
}


static void frontend_pcm_cb(AsPcmDataParam pcm) {
  static uint8_t mono_input[frame_size];

  frontend_signal_input(pcm, mono_input, frame_size);
  signal_process((int16_t*)mono_input, (int16_t*)NULL, sample_size);
  return;
}

void frontend_signal_input(AsPcmDataParam pcm, uint8_t* input, uint32_t frame_size) {
  /* clean up the input buffer */
  memset(input, 0, frame_size);

  if (!pcm.is_valid) {
    Serial.println("WARNING: Invalid data! @frontend_signal_input");
    return;
  }

  //***
  digitalWrite(3, HIGH); // for measuring the TCT40-16T output time (Start) 
  last_time = current_time;
  current_time = micros();
  //***

  if (pcm.size > frame_size) {
    Serial.print("WARNING: Captured size is too big! -");
    Serial.print(String(pcm.size));
    Serial.println("- @frontend_signal_input");
    pcm.size = frame_size;
  } 
  
  /* copy the signal to signal_input buffer */
  if (pcm.size != 0) {
    memcpy(input, pcm.mh.getPa(), pcm.size);
  } else {
    Serial.println("WARNING: Captured size is zero! @frontend_signal_input");
  }  
}

void signal_process(int16_t* mono_input, int16_t* stereo_output, uint32_t sample_size) {

  //***
  /* calculating a distance for the past frame */
  static const int16_t threshold = 300;  // need to adjust
  uint32_t distance_time = 0; 
  uint32_t trigger_time  = 0;
  uint32_t offset_time = fire_time - last_time; 

  for (uint32_t n = 0; n < sample_size-1; ++n) {
    int16_t value = 0;
    value = abs(mono_input[n]); 
    if (value > threshold) {
      trigger_time = n*1000000/(192000); // microseconds
      distance_time = trigger_time - offset_time; 
      break; 
    }
  }

  if (distance_time > 0 && distance_time < 5333 /* about 2m */) {
    float distance_cm = 340.* float(distance_time)/10000;
    Serial.println(distance_cm);
    fire_time = 0;
  }
  
  /* firing super sonig for 1 msec */
  fire_time = micros();
  ioctl(fd, PWMIOC_START, 0);
  delay(1);
  ioctl(fd, PWMIOC_STOP, 0);     
  digitalWrite(3, LOW); // for measuring the TCT40-16T output time (End)  
  //***

  return;
}


void setup() {
  Serial.begin(115200);
  pinMode(3,OUTPUT);

  //***
  /* Setting PWM 40kHz */
  Serial.println("setup /dev/pwm0");
  fd = open("/dev/pwm0", O_RDONLY);
  info.frequency = 40000; // 40kHz
  info.duty      = 0x7fff;
  ioctl(fd, PWMIOC_SETCHARACTERISTICS, (unsigned long)((uintptr_t)&info));
  ioctl(fd, PWMIOC_STOP, 0);  
  //***  

  /* Initialize memory pools and message libs */
  initMemoryPools();
  createStaticPools(MEM_LAYOUT_RECORDINGPLAYER);

  /* setup FrontEnd and Mixer */
  theFrontEnd = FrontEnd::getInstance();
  
  /* set clock mode */
  theFrontEnd->setCapturingClkMode(FRONTEND_CAPCLK_HIRESO);

  /* begin FrontEnd and OuputMixer */
  theFrontEnd->begin(frontend_attention_cb);
  Serial.println("Setup: FrontEnd began");

  /* activate FrontEnd and Mixer */
  theFrontEnd->setMicGain(0);
  theFrontEnd->activate(frontend_done_cb);
  delay(100); /* waiting for Mic startup */
  Serial.println("Setup: FrontEnd activated");

  /* Initialize FrontEnd */
  AsDataDest dst;
  dst.cb = frontend_pcm_cb;
  theFrontEnd->init(channel_num, bit_length, sample_size, AsDataPathCallback, dst);
  Serial.println("Setup: FrontEnd initialized");

  /* Unmute */
  board_external_amp_mute_control(false);
  theFrontEnd->start();
  Serial.println("Setup: FrontEnd started");
}

void loop() { 
  if (isErr == true) {
    board_external_amp_mute_control(true); 
    theFrontEnd->stop();
    theFrontEnd->deactivate();
    theFrontEnd->end();
    Serial.println("Capturing Process Terminated");
    while(1) {};
  }
}

測距の限界値

このコードでは基本的に超音波の出力と受信を192kHzの1024サンプル以内で処理するように作っています（簡単なので）。何を言っているかというと、5.3ミリ秒の間隔でトランスデューサから1ミリ秒 40kHz の超音波を出力し、レシーバーその超音波を受信し、その遅れ時間を測定することで距離を測定します。これを音速で換算すると、

340(m) x 0.0053(秒) =1.8(m)

が限界値になります。まぁ超音波センサーならこれくらいが限界でしょう。

因果律に注意

このプログラムの少しわかりにくいところがプログラムの処理の流れと来たデータのタイミングです。キャプチャしてやってくるデータは一つ前の処理で行った過去のものだということに注意してください。このことを把握していないと、何をやっているのか訳分からなくなると思います。

主な処理は、signal_process関数 で行っています。その他のパートのほどんとは上記Githubの借りパクですのであまり気にしなくてよいと思います。上記サイトを参考にしてください。

超音波出力までの遅れ時間の測定

超音波を出力するまで所要時間はキャプチャした直後の時間を記録(last_time)し、超音波を出力するまでの時間(fire_time)を測定し、そのオフセット分(offset_time)をレシーバが受信した遅れ時間(trigger_time)から差し引く必要があります。だいたい 336マイクロ秒前後という結果が出ました。

念のためオシロで測定してみました。だいたい合っているようです。

超音波の受信判定

超音波の受信判定は、受信データがあるしきい値以上になった場合に受信したと設定しています。この段階ではしきい値は適当に決めています、

次のような形で定規をおいて実測をしてみました。

測定結果です。

定規(cm)	出力値(cm) 1000回平均
3.0	32.6
4.0	34.5
5.0	35.8
6.0	36.6
7.0	37.7
8.0	38.6
9.0	39.8
10.0	41.1
11.0	42.7
12.0	44.4
13.0	46.1
14.0	47.5
15.0	49.3

4cm以降はだいたい1cm単位で刻んでいますが、かなりオフセットがかかっています。しきい値設定の問題とかそういうレベルではなさそうです。

超音波レシーバの波形を確認

これはなにかあるなということで、超音波トランスデューサとレシーバの波形を確認してみました。

黄色がトランスデューサの波形です。青色がレシーバの波形です。7cm離した状態での測定です。レシーバ（青色）の波形は受信してから徐々に出力があがり、徐々に減衰するような波形となってしまっています。

また、受信波形はトランスデューサの信号の立ち上がりから約400マイクロ秒遅れています。本来であれば、0.07/340≒200マイクロ秒であるはずなのですが、信号が減衰してしまい受信の立ち上がりが遅れてしまっています。

さらに距離が離れるほど、信号の最大振幅も減衰してしまいます。この減衰は固定的なしきい値では遅延時間の測定に影響を及ぼします。次の波形は20cm離した状態です。

キャリブレーション

正確な値を得るには、アダプティブにしきい値を変化させることが必要そうです。受信した信号の最大値をとってしきい値を設定するのと、しきい値の設定に応じて遅延時間(alpha)を調整する必要がありそうです。今回はしきい値を最大値の半分としてみました。

遅延時間の調整は、たとえば 7cm の距離で、出力結果が 7cm に合うようにしてみました。今回は大雑把に1000 マイクロ秒に設定してみました。以下が測定結果です。

定規(cm)	出力値(cm) 1000回平均
3.0	2.7
4.0	3.9
5.0	5.1
6.0	6.4
7.0	7.0
8.0	8.0
9.0	9.2
10.0	10.1
11.0	11.2
12.0	12.2
13.0	13.1
14.0	14.0
15.0	14.9

なんか良い感じで測定値が出ました。これくらいの誤差であれば十分活用可能でしょう。最終的なコードは次のようになりました。

#include <FrontEnd.h>
#include <MemoryUtil.h>
#include <arch/board/board.h>

#define SAMPLE_SIZE (1024)

//*****
#include <sys/ioctl.h>
#include <stdio.h>
#include <fcntl.h>   
#include <nuttx/timers/pwm.h>
int fd;
struct pwm_info_s info;
//*****

//***
uint32_t fire_time = 0;
uint32_t last_time = 0;
uint32_t current_time = 0;
//***

FrontEnd *theFrontEnd;

const int32_t channel_num = AS_CHANNEL_MONO;
const int32_t bit_length  = AS_BITLENGTH_16;
const int32_t sample_size = SAMPLE_SIZE;
const int32_t frame_size  = sample_size * (bit_length / 8) * channel_num;
bool isErr = false;

void frontend_attention_cb(const ErrorAttentionParam *param) {
  Serial.println("ERROR: Attention! Something happened at FrontEnd");
  if (param->error_code >= AS_ATTENTION_CODE_WARNING) isErr = true;
}

static bool frontend_done_cb(AsMicFrontendEvent ev, uint32_t result, uint32_t detail){
  UNUSED(ev);  UNUSED(result);  UNUSED(detail);  
  return true;
}


static void frontend_pcm_cb(AsPcmDataParam pcm) {
  static uint8_t mono_input[frame_size];

  frontend_signal_input(pcm, mono_input, frame_size);
  signal_process((int16_t*)mono_input, (int16_t*)NULL, sample_size);
  return;
}

void frontend_signal_input(AsPcmDataParam pcm, uint8_t* input, uint32_t frame_size) {
  /* clean up the input buffer */
  memset(input, 0, frame_size);

  if (!pcm.is_valid) {
    Serial.println("WARNING: Invalid data! @frontend_signal_input");
    return;
  }

  //***
  digitalWrite(3, HIGH); // for measuring the TCT40-16T output time (Start) 
  last_time = current_time;
  current_time = micros();
  //***

  if (pcm.size > frame_size) {
    Serial.print("WARNING: Captured size is too big! -");
    Serial.print(String(pcm.size));
    Serial.println("- @frontend_signal_input");
    pcm.size = frame_size;
  } 
  
  /* copy the signal to signal_input buffer */
  if (pcm.size != 0) {
    memcpy(input, pcm.mh.getPa(), pcm.size);
  } else {
    Serial.println("WARNING: Captured size is zero! @frontend_signal_input");
  }  
}

void signal_process(int16_t* mono_input, int16_t* stereo_output, uint32_t sample_size) {

  //***
  /* calculating a distance for the past frame */
  int16_t threshold = 0;  
  uint32_t distance_time = 0; 
  uint32_t trigger_time  = 0;
  const uint32_t alpha = 1000; // adjustment parameter (microseconds)
  uint32_t offset_time = fire_time - last_time + alpha; 
  static int counter = 0;

  int16_t max = 0;
  for (int n = 0; n < sample_size; ++n) {
    float fvalue = float(mono_input[n]);
    int16_t amp = abs(mono_input[n]);
    if (abs(mono_input[n] > max))  max = amp; 
  }
  threshold = int16_t(max/2);

  for (uint32_t n = 0; n < sample_size-1; ++n) {
    int16_t value = 0;
    value = abs(mono_input[n]); 
    if (value > threshold) {
      trigger_time = n*1000000/(192000); // microseconds
      distance_time = trigger_time - offset_time ; 
      break; 
    }
  }

  const int ave_count = 1000;
  if (distance_time > 0 && distance_time < 5333 /* about 2m */) {
    static float ave_distance = 0;
    float distance_cm = 340.* float(distance_time)/10000;
    // Serial.println(String(offset_time) + "," + String(distance_cm));
    ave_distance += distance_cm;
    fire_time = 0;
    if (counter++ == ave_count) {
      ave_distance /= ave_count;
      Serial.println(ave_distance);
      counter = 0;
      ave_distance = 0;
    }
  }

  /* firing super sonig for 1 msec */
  fire_time = micros();
  ioctl(fd, PWMIOC_START, 0);
  delay(1);
  ioctl(fd, PWMIOC_STOP, 0);     
  digitalWrite(3, LOW); // for measuring the TCT40-16T output time (End)  
  //***

  return;
}

void setup() {
  Serial.begin(115200);
  pinMode(3,OUTPUT);

  //***
  /* Setting PWM 40kHz */
  Serial.println("setup /dev/pwm0");
  fd = open("/dev/pwm0", O_RDONLY);
  info.frequency = 40000; // 40kHz
  info.duty      = 0x7fff;
  ioctl(fd, PWMIOC_SETCHARACTERISTICS, (unsigned long)((uintptr_t)&info));
  ioctl(fd, PWMIOC_STOP, 0);  
  //***  

  /* Initialize memory pools and message libs */
  initMemoryPools();
  createStaticPools(MEM_LAYOUT_RECORDINGPLAYER);

  /* setup FrontEnd and Mixer */
  theFrontEnd = FrontEnd::getInstance();
  
  /* set clock mode */
  theFrontEnd->setCapturingClkMode(FRONTEND_CAPCLK_HIRESO);

  /* begin FrontEnd and OuputMixer */
  theFrontEnd->begin(frontend_attention_cb);
  Serial.println("Setup: FrontEnd began");

  /* activate FrontEnd and Mixer */
  theFrontEnd->setMicGain(0);
  theFrontEnd->activate(frontend_done_cb);
  delay(100); /* waiting for Mic startup */
  Serial.println("Setup: FrontEnd activated");

  /* Initialize FrontEnd */
  AsDataDest dst;
  dst.cb = frontend_pcm_cb;
  theFrontEnd->init(channel_num, bit_length, sample_size, AsDataPathCallback, dst);
  Serial.println("Setup: FrontEnd initialized");

  /* Unmute */
  board_external_amp_mute_control(false);
  theFrontEnd->start();
  Serial.println("Setup: FrontEnd started");
}

void loop() { 
  if (isErr == true) {
    board_external_amp_mute_control(true); 
    theFrontEnd->stop();
    theFrontEnd->deactivate();
    theFrontEnd->end();
    Serial.println("Capturing Process Terminated");
    while(1) {};
  }
}

今回はPWMによる超音波トランスデューサを使いましたが、SPRESENSEはハイレゾ再生できるので超音波出力可能なスピーカーがあれば、もっとも細かい制御ができそうです。

作ってみた感想

今回はアテ感で遅延量を1000マイクロ秒に設定したのですが、オシロを見ると400マイクロ秒くらいです。この600マイクロ秒の差がなぜ出るのか？謎を残しました。また、トランスデューサの波形は1ミリ秒に対しレシーバの波形が2ミリ秒程度あります。レシーバ筒内の反響と減衰が影響しているのが分かります。ここがまた距離測定を難しくしています。市販の超音波センサを解析することで、なにかヒントが得られるかもしれません。今後、調査したいと思います。