Time-of-Flight（ToF）測距

頭部の測距センサです。反射波の時間計測は ESP32の RMT（ Remote Control Transceiver ）を使用しています。

◀ この記事の前に：　Voice driver（操作説明）
▶ この記事の次に：　音声合成ライブラリ

測距のしくみ

超音波測距センサ（距離測定センサ）は電子工作でも良く使われる HC-SR04を使用しました。
測定上の原理は、センサ前面方向に超音波を発してその反射波を捉えます。超音波の発してから反射波を受信するまでの時間が距離と比例しますので、超音波の発してから反射波を受信するまでの時間を測定して計算により距離を求めます。音速が秒速で 340 m/s なので時間を乗じて距離を求めます。往復なので 1/2 として単位換算で 10^-3を乗じています。計算式は以下のとおりです。

距離（ mm ) = 1 / 2 × 340（ m/s ）× 10^-3 × T（ us ）

時間の測定は ESP32の RMT( Remote Control Transceiver ) という、赤外線リモコン信号を制御するための機能を使用します。RMT を使用することで正確にパルス幅を測定できます。

Technical Considerations 　pulseIn() は使用できない
Arduinoには パルス幅を測定する pulseIn()という関数がありますが、ESP32 の 26pinでは使用できません。関数により使用できるピンが限定されていますので注意が必要です。RMTは 26pinでも使用できます。

Technical Considerations 　RMT( Remote Control Transceiver ) とは
ESP32に内蔵された 高精度なタイミング制御用モジュールです。パルス送信、パルス幅受信、信号解析、LED制御、超音波センサのパルス測定、モータ制御などのPWM波形処理など広範囲の用途に使われています。もともとは赤外線リモコン信号の送受信を目的に設計されたため、NECフォーマットなどにも対応できます。

操作手順

測距を使用するときは連続して測定する場合が多いと思います。歩行するときの障害物検知や、動作のきっかけとして手をかざすなど一定時間、測距を行い次の行動に結び付けます。
その使用方法に適するように連続して測距する方式としました。一定の間隔で測距を行い常に直近の測距結果を変数に保管します。測距結果を必要とする処理はその変数を見ることで、直近の測距結果を知ることができます。

測距の省電力化

測距も他の回路と同じく電力を消費します。省電力を目指すために測距用センサの電源を制御することができます。
測距用センサの電源はサーボモータの電源と同じ +5VPの電源系統です。この省電力制御は Body MCU（ Leafony / STM32 ）で行います。

Design Considerations 　測距用センサの電源
測距用センサの電源がサーボモータ制御の電源系統と同じ理由は二つあります。ひとつはどちらも 5Vであること、もうひとつは使用のタイミングによるものです。歩行中に障害物を検知する状況が多いと考えました。歩行時にサーボモータの制御を行います。そのタイミングで測距も必要となりますので同じ系統にしました。
組み込みシステムはリソースが限られます。効率の良い方法が望まれます。搭載する機能を考えてハードウエアの構成を考えることも必要です。ちょっとしたことを積み上げることが、最終的に製品の品質や不具合防止につながります。

注意

測定開始前に startTOF() が、測定終了後に stopTOF() の実行が必要です。
測距センサの電源は、サーボモータの電源系統と同じ +5VPです。測定前に +5VPを ONにする必要があります。測定終了時には +5VPを OFFにする必要があります。但し、サーボモータの動作とも関係するため、+5VP 制御処理を介して ON/OFFする必要があります。+5VPは Body MCUで制御しています。

関数の概要

関数は以下のとおりです。

・外部関数

他のモジュールやファイルの外部からアクセス可能な関数です。

getDistanceTOF()

引数：

なし

戻り値：

距離（ mm ）、-1：　エラー（測定不能）

分類：

API（アプリケーション、他）

機能：

測定距離（ mm ）を取得する。事前に startTOF()で測定を開始して、測定が不要になったときには stopTOF()を実行する。測定期間外はエラーを返します。

関係する関数：

上位関数：　アプリケーション、他
下位関数：　ー
この関数のコードはこちらです。

startTOF()

引数：

なし

戻り値：

なし

分類：

API（アプリケーション、他）

機能：

測定を開始する。

関係する関数：

上位関数：　アプリケーション、他
下位関数：　ー
この関数のコードはこちらです。

stopTOF()

引数：

なし

戻り値：

なし

分類：

API（アプリケーション、他）

機能：

測定距を終了する。

関係する関数：

上位関数：　アプリケーション、他
下位関数：　ー
この関数のコードはこちらです。

・内部関数

setupTOF()

引数：

なし

分類：

内部関数（初期化）　

機能：

RMTの初期化を行います。

関係する関数：

上位関数：　setup()
下位関数：　ー
この関数のコードはこちらです。

measureDistanceTOF()

引数：

なし

分類：

内部関数　

機能：

測定距離（ mm ）を取得する。

関係する関数：

上位関数：　ー
下位関数：　ー
この関数のコードはこちらです。

procTOF()

引数：

なし

分類：

内部関数（ loop()内、定時処理）

機能：

定期的に測定距離（ mm ）を取得する。

関係する関数：

上位関数：　loop()
下位関数：　measureDistanceTOF()
この関数のコードはこちらです。

データの構造

データの構造は以下のとおりです。

・グローバル定義

グローバル定義は以下のとおりです。

//------------------------------------------------------------------------------
// IO pin name definition
//------------------------------------------------------------------------------

//-----------------------------------------------
// IO pin list
//-----------------------------------------------

...

//---- TOF ----
#define	PIN_UTOF_TRIG		17			// Trigger output
#define	PIN_UTOF_ECHO		39			// Echo input

//------------------------------------------------------------------------------
// Ultra Time-of-Flight sensor
//------------------------------------------------------------------------------

//.... RMT configuration ....
#define RMT_CHANNEL RMT_CHANNEL_0		// GPIO32 - GPIO39

//.... Argument ....
#define UTOF_ON				1			// 
#define UTOF_OFF			0

//---- Error ----
#define UTOF_ERROR			-1			// 

//.... Measurement period ....
// period unit : 100 ms ( f = 10kHz )
//	T = TI_INIT_INTERVAL / TI_CLOCK
// Measured period = T * UTOF_DEF_PERIOD
//
const int	UTOF_DEF_PERIOD = 5;		// 500ms

//.... RMT ....
#define RMT_CHANNEL RMT_CHANNEL_0		// GPIO32 - GPIO39

名称	型	値	説明
PIN_UTOF_TRIG	define	17	センサ HC-SR04 の TRIG信号ピン（発信）（頭部オプション追加時は H-EN-P として使用）
PIN_UTOF_ECHO	define	39	センサ HC-SR04 の ECHO信号ピン（受信）
RMT_CHANNEL	define	RMT_CHANNEL_0	GPIO32 – GPIO39の場合、RMT チャネルは 7
UTOF_ON	define	1	測距する
UTOF_OFF	define	0	測距しない
UTOF_ERROR	define	-1	測距の戻り値（エラー）
UTOF_DEF_PERIOD	int	5	測距周期の初期値（ ×100ms ）変数 gPeriodUTOF に対応

・RMT のコンフィグレーション

RMTのコンフィグレーションに関する定義です。

//.... RMT configuration ....
  rmt_config_t	rmt_rx;

  rmt_rx.channel 						= RMT_CHANNEL;
  rmt_rx.gpio_num 						= (gpio_num_t)PIN_UTOF_ECHO;
  rmt_rx.clk_div 						= 80;		// 1tick = 1us ( 80MHz / 80 = 1MHz )
  rmt_rx.mem_block_num 					= 1;
  rmt_rx.rmt_mode 						= RMT_MODE_RX;
  rmt_rx.rx_config.filter_en 			= true;
  rmt_rx.rx_config.filter_ticks_thresh	= 100;		// Noise reduction below 100us
  rmt_rx.rx_config.idle_threshold 		= 60000;	// End judged when there is no signal for 60ms

要素	設定値 / 説明
.channel	RMT_CHANNEL
.gpio_num	PIN_UTOF_ECHO
.clk_div	80	1tick = 1us ( 80MHz / 80 = 1MHz )
.mem_block_num	1
.rmt_mode	RMT_MODE_RX
.rx_config.filter_en	true	デフォルトの割り込み設定
.rx_config.filter_ticks_thresh	100	フィルタ 100us 未満のノイズ除去
.rx_config.idle_threshold	60000	60ms 無信号で終了判定

・グローバル変数

グローバル変数は以下のとおりです。

//==============================================================================
// Variable definitions in source code
//==============================================================================

//------------------------------------------------------------------------------
// UTOF driver
//------------------------------------------------------------------------------

uint8_t		gEnableUTOF	= UTOF_OFF;			// enable 
int			gPeriodUTOF	= UTOF_DEF_PERIOD;	// measurement period

int			gLoopUTOF	= 0;				// control measurement period

int			gMesValUTOF	= 0;				// measurement value ( mm )

RingbufHandle_t		rb = NULL;				// Ring buffer

要素	型	初期値	説明
gEnableUTOF	bool	UTOF_OFF	連続測距の ON/OFF状態の保存
gPeriodUTOF	int	UTOF_DEF_PERIOD	測距周期（ × 100 ms ）
gLoopUTOF	int	0	測距周期の処理用カウンタ、loop() 内で使用
gMesValUTOF	int	0	直近の測距値
rb	RingbufHandle_t	NULL	データ取得用のリングバッファライブラリ内で使用（型はライブラリで定義）

コード

コードは以下のとおりです。

・インクルードファイル

必要なインクルードファイルは以下のとおりです。

要素	説明
“driver/rmt.h”	RMT（ Remote Control Transceiver ）の使用

・関数間の関係

処理ごとの関数の関係は以下のとおりです。

関数の関係は以下のとおりです。

・外部関数

getDistanceTOF()

測定開始以降に連続的に測定した値を返します。

注意

測定開始前に startTOF() が、測定終了後に stopTOF() の実行が必要です。

/*----------------------------------------------------------------------------*/
/* Get Distance value
/* param     : -
/* return    : int measured value / -1 : Error
/*----------------------------------------------------------------------------*/
int getDistanceTOF()
{

	return gMesValUTOF;

}

setPeriodTOF( int nPrtiod )

測定間隔を設定します。

/*----------------------------------------------------------------------------*/
/* Set measurering period
/* param     : int nPeriod / measurering period 100ms
/* return    : -
/*----------------------------------------------------------------------------*/
void setSpeedVD( int nPeriod )
{

	gPeriodUTOF = nPeriod;

}

startTOF()

連続測定時の準備を行う。

/*----------------------------------------------------------------------------*/
/* Start measuring distance
/* param     : -
/* return    : -
/*----------------------------------------------------------------------------*/
void startTOF()
{

	//.... sensor power on ....
	//setSavingPower( SP_UT, POWER_ON );			// controlled by Body MCU 

	rmt_rx_start( RMT_CHANNEL, true );
	gEnableUTOF = UTOF_ON;

}

stopTOF()

連続測定時の発音処理の終結を行う。

/*----------------------------------------------------------------------------*/
/* Stop measuring distance
/* param     : -
/* return    : -
/*----------------------------------------------------------------------------*/
void stopTOF()
{

	//.... sensor power off ....
	//setSavingPower( SP_UT, POWER_OFF );			// controlled by Body MCU 

	rmt_rx_stop( RMT_CHANNEL );
	gEnableUTOF = UTOF_OFF;

}

・内部関数

setup() 内

測距の初期化処理を呼び出します。

void setup( void )
{

	･･･

	/* Initialize Voice driver */
	setupTOF();

	･･･

}

setupTOF()

測距の初期化と RMTの設定を行います。

/*----------------------------------------------------------------------------*/
/* Initialize ultra TOF
/* param     : -
/* return    : -
/*----------------------------------------------------------------------------*/
void setupTOF()
{

	//.... local ....
	rmt_config_t			rmt_rx;

	//.... setup RMT ....
	rmt_rx.channel 							= RMT_CHANNEL;
	rmt_rx.gpio_num 						= (gpio_num_t)PIN_UTOF_ECHO;
	rmt_rx.clk_div 							= 80;		// 1tick = 1us ( 80MHz / 80 = 1MHz )
	rmt_rx.mem_block_num 					= 1;
	rmt_rx.rmt_mode 						= RMT_MODE_RX;
	rmt_rx.rx_config.filter_en 				= true;
	rmt_rx.rx_config.filter_ticks_thresh	= 100;		// Noise pulses shorter than 100 µs are filtered out.
	rmt_rx.rx_config.idle_threshold 		= 60000;	// 60ms A timeout is used to detect the end of the signal.

	rmt_config( &rmt_rx );

	//.... install RMT driver ....
	rmt_driver_install( RMT_CHANNEL, 1000, 0 );
	rmt_get_ringbuf_handle( RMT_CHANNEL, &rb );

}

measureDistanceTOF()

距離を測定します。

/*----------------------------------------------------------------------------*/
/* Measuring distance
/* param     : -
/* return    : Distance ( mm ) / -1: Error
/*----------------------------------------------------------------------------*/
int measureDistanceTOF()
{

	//.... transmit trigger pulse ....
	digitalWrite( PIN_UTOF_TRIG, HIGH );
	delayMicroseconds( 10 );						// 10 us pulse period
	digitalWrite( PIN_UTOF_TRIG, LOW );

	//.... capture period of echo pulse ....
	size_t item_size = 0;
	rmt_item32_t* items = (rmt_item32_t*) xRingbufferReceive( rb, &item_size, pdMS_TO_TICKS(1000) );

	//.... calculate distance ....
	if ( items ) {
		uint32_t duration = items[0].duration0;
		int distance = ( duration * 0.343 ) / 2.0;	// speed of sound 0.0343 cm/us
		vRingbufferReturnItem( rb, (void*) items );
		return distance;

	} else {
		return UTOF_ERROR;

	}

}

procTOF()

連続測定時の測定を行います。フォアグランド（ loop ）内から呼び出されます。

/*----------------------------------------------------------------------------*/
/* TOF interval processing
/* param     : -
/* return    : -
/*----------------------------------------------------------------------------*/
void procTOF()
{

	//....  UTOF period ....
	gLoopUTOF += 1;
	if ( gLoopUTOF >= gPeriodUTOF ){ 		 // 100 ms x 5 = 0.5 s
		gLoopUTOF = 0;

		if ( gEnableUTOF == UTOF_ON ){
			nDistance = getDistanceTOF();
			Serial.println( nDistance );

		} else {
		nDistance = UTOF_ERROR;
		}
	}

}

・RMT 初期化の確認方法

RMTの初期化に問題がありそうな場合は、RMT の設定を行うたびに確認します。以下は例です。

rmt_config_t			rmt_rx;

...

rmt の設定（ 省略 ）

...

//.... RMT 設定 ....
if (rmt_config(&rmt_rx) != ESP_OK) {
  Serial.println("Error: RMT設定失敗");
}

//.... RMT ドライバの導入 ....
if (rmt_driver_install(RMT_CHANNEL, 1000, 0) != ESP_OK) {
  Serial.println(""Error: RMTドライバーインストールエラー");
}

//.... 利用可能メモリの確認 ....
Serial.print("利用可能メモリ: ");
Serial.println(esp_get_free_heap_size());

//.... リングバッファの取得チェック ....
rmt_get_ringbuf_handle(RMT_CHANNEL, &rb);
if (rb == NULL) {
  Serial.println("Error: リングバッファの取得失敗");
}

参考情報

参考になる情報は以下のとおりです。

このホームページ内

TOF（超音波による測距）ハードウエア：　TOFの回路などに関する説明

他のWebサイト

超音波の調査：　超音波による測距のしくみを説明 / 沼津高専電子制御工学科
ESP32と超音波センサー HC-SR04 で物体の距離を計測する：　製作記 / Avinton
M5Stackで距離を測る：　製作記 / スイッチサイエンス
M5StickC(ESP32)の赤外線(RMT)受信を調べた：　製作記 / Lang-ship
RMT：　詳細な説明、ESPRESSIF / ESP-IDF Programming Guide
赤外線送受信(RMT)：　メンバ関数の詳しい説明 / M5StickC非公式日本語リファレンス
超音波距離センサ HC-SR04：　販売 / スイッチサイエンス
HC-SR04 仕様：　PDF /
HC-SR04 仕様：　PDF / 秋月電子

Time-of-Flight（ToF）測距

測距のしくみ

操作手順

測距の省電力化

注 意

関数の概要

・外部関数

getDistanceTOF()

引 数：

戻り値：

分 類：

機 能：

関係する関数：

startTOF()

引 数：

戻り値：

分 類：

機 能：

関係する関数：

stopTOF()

引 数：

戻り値：

分 類：

機 能：

関係する関数：

・内部関数

setupTOF()

引 数：

分 類：

機 能：

関係する関数：

measureDistanceTOF()

引 数：

分 類：

機 能：

関係する関数：

procTOF()

引 数：

分 類：

機 能：

関係する関数：

データの構造

・グローバル定義

・RMT のコンフィグレーション

・グローバル変数

コード

・インクルードファイル

・関数間の関係

・外部関数

getDistanceTOF()

注 意

setPeriodTOF( int nPrtiod )

startTOF()

stopTOF()

・内部関数

setup() 内

setupTOF()

measureDistanceTOF()

procTOF()

・RMT 初期化の確認方法

参考情報

このホームページ内

他のWebサイト

注意

引数：

分類：

機能：

引数：

分類：

機能：

引数：

分類：

機能：

引数：

分類：

機能：

引数：

分類：

機能：

引数：

分類：

機能：

注意