1. 今回のやりたいこと

タイトルにもありますように、KOZOS上で超音波センサのデータを周期的に取得をしたいと思います。今回使う超音波センサはHC-SR04になります。

2. 環境

使用ボード：nucleo l4r5zi
- Cortex-M4搭載、2MBのFlash、640KのSRAM、USB OTG
開発環境：Atollic TrueSTUDIO® for STM32, Built on Eclipse Neon.1a.(Version: 9.3.0 )
対象OS：KOZOS※
※下記書籍で紹介されているOSになります。ソースコードも展開されています。
12ステップで作る組込みOS自作入門

3. 前提

nucleo l4r5ziにKOZOSをポーティングしています。さらに指定した時間sleepする関数：kz_tsleepを追加実装いたしました。
それぞれについて、下記に説明していますので興味ある方は是非見てみてください。
KOZOSをnucleo l4r5ziにポーティングしてみた
 KOZOSにdly_tskと同じ機能のサービスコールを実装してみた

4. 超音波センサ:HC-SR04

今回は超音波センサとしてHC-SR04を使用します。

HC-SR04は、超音波の反射時間を利用して非接触で測距するモジュールになります。4つピンがあり、上記図の左から電源(Vcc)、Trig(入力)、Echo(出力)、Gnd(GND)となっています。HC-SR04のデータシートは下記にあります。
https://akizukidenshi.com/download/ds/sainsmar/hc-sr04_ultrasonic_module_user_guidejohn_b.pdf

データシートに記載されているタイミングチャートを下記に貼ります。

外部からTrigに対してパルスを入力(上記①)すると超音波センサから超音波パルスが送信(上記②)されます。超音波パルスが送信されるとechoがhigh(上記③)になり、対象物からの反射波を受信するとlow(上記④)になります。マイコン側でechoのhighの時間を測定することで対象物との距離を測定することができます。ここでの時間の単位はusで、3mm単位で測定することができ、測距範囲は2cm～400cmとなっています。

距離の求め方は下記の通りになります。

　距離[cm] = (0.0034[cm/us] * (echoのhighの時間[us]))/2

0.0034は音速を表しています。また、2で割っているのは、echoのhigh時間が往復時間のためです。

5. 制限

前章でechoのhigh時間を測定することで対象物との距離を測定すると説明させていただきました。ここで問題なのが、echoのhigh時間を測定するためにはマイコン側でusオーダーの時間を測定する必要があるということです。

まず、Systick割込みを1usで発生させ、1usのカウンタを作成することでこれを実現しようと考えました。現状、コアは4Mhz（1clock = 0.25us）で動作しており、また、Systick割込みの中では結構な処理を行っているため、Systick割込みを1usに設定したときに、Systick割込みにCPUが占有されてしまい、アプリが動作しなくなるということがありました。

上記のことから、今回は、コア側では1ms単位でechoのhigh時間を測定することにしました。つまり、17cm単位でしか測定できないということになります。nucleo l4r5ziでは、クロックを最大120Mhzに設定できるみたいなのですが、特に17cm単位での測定でも問題ないので、これでよしとしました。

6. nucleo l4r5ziとHC-SR04の接続

nucleo l4r5ziとHC-SR04の接続について説明します。
nucleo l4r5zi側では、HC-SR04と接続するピンをGPIO設定にします。ここで注意すべきなのが、HC-SR04のecho出力が5Vであることです。nucleo l4r5ziの入力電圧は3.3Vとなっており、直接つなげることができません。echoの5V出力を分圧して3.3Vにしてからnucleo l4r5ziへ入力する必要があります。

これらを踏まえて下記のように接続しました。(配線が汚くて申し訳ないです、、、)
実際の図はわかりづらいと思いますので、絵で描いた図も載せておきます。

今回、分圧するためにかなり久々にオームの法則を使いました。頭の体操になったのでよかったです。ちなみに、Trigのほうは、5V入力なのですが、3.3Vでもぎりぎり動作するようです。

7. 超音波センサのデータ取得

超音波センサから周期的にデータを取得するタスクの実装を下記に示します。

us_sensor_main

/*
 * us_sensor.c
 *
 *  Created on: 2022/08/29
 *      Author: User
 */
#include "defines.h"
#include "kozos.h"
#include "consdrv.h"
#include "lib.h"
#include "stm32l4xx_hal_gpio.h"
#include "stm32l4xx_hal_pwr_ex.h"

#define US_SENSOR_NUM  (1)
#define BUF_SIZE       (16)
#define SPEED_OF_SOUND (34) // 34cm/ms

typedef struct{
	kz_thread_id_t tsk_id;
	uint8_t state;
	uint8_t data[BUF_SIZE];
}US_SENSOR_CTL;

/* 制御用ブロック */
US_SENSOR_CTL us_sensor_ctl[US_SENSOR_NUM];

static void us_sensor_init(void);
static uint8_t read_distance(void);

/* 超音波センサからのデータを10ms周期で取得するタスク */
int us_sensor_main(int argc, char *argv[])
{
	US_SENSOR_CTL *this = &us_sensor_ctl[0];
	uint8_t idx = 0;

	/* 初期化 */
	us_sensor_init();

	while(1){
		/* 500msのスリープ(これで500ms周期で動作するタスクを実現) */
		kz_tsleep(500);

		/* 距離を取得 */
		this->data[idx] = read_distance();
		idx++;

		/* バッファがFULLになった場合 */
		if(BUF_SIZE == idx){
			idx = 0;
		}
	}

	return 0;
}

static void us_sensor_init(void)
{
	US_SENSOR_CTL *this = &us_sensor_ctl[0];
	GPIO_InitTypeDef GPIO_InitStruct = {0};

	/* 自タスクのidを取得 */
	this->tsk_id = kz_getid();

	/* GPIOの設定 */
	__HAL_RCC_GPIOF_CLK_ENABLE();
	HAL_PWREx_EnableVddIO2();

	/* PF14(GPIO入力) -> HC-SR04 Echo */
	/* PF15(GPIO出力) -> HC-SR04 Trig */
	/*Configure GPIO pin : PF14 */
	GPIO_InitStruct.Pin = GPIO_PIN_14;
	GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
	GPIO_InitStruct.Pull = GPIO_NOPULL;
	HAL_GPIO_Init(GPIOF, &GPIO_InitStruct);

	/*Configure GPIO pin : PF15 */
	HAL_GPIO_WritePin(GPIOF, GPIO_PIN_15, GPIO_PIN_RESET);
	GPIO_InitStruct.Pin = GPIO_PIN_15;
	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
	GPIO_InitStruct.Pull = GPIO_NOPULL;
	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
	HAL_GPIO_Init(GPIOF, &GPIO_InitStruct);

	return;
}

/* 超音波センサから距離を取得 */
static uint8_t read_distance(void)
{
	uint8_t echo;
	uint32_t start_time_s;
	uint32_t end_time_s;
	uint32_t start_time_ms;
	uint32_t end_time_ms;
	uint32_t elapsed_time_ms;
	uint8_t distance;

	/* TrigをHigh */
	HAL_GPIO_WritePin(GPIOF, GPIO_PIN_15, GPIO_PIN_SET);
	/* 1msのwait */
	kz_tsleep(1);
	/* TrigをLow */
	HAL_GPIO_WritePin(GPIOF, GPIO_PIN_15, GPIO_PIN_RESET);

	/* Highの開始時間を取得 */
	do{
		/* echoを取得 */
		echo = HAL_GPIO_ReadPin(GPIOF, GPIO_PIN_14);
		/* 開始時刻を取得 */
		start_time_ms = kz_get_time_ms();
		start_time_s = kz_get_time_s();
	}while(!echo);

	/* Highの終了時間を取得 */
	do{
		/* echoを取得 */
		echo = HAL_GPIO_ReadPin(GPIOF, GPIO_PIN_14);
		/* 開始時刻を取得 */
		end_time_ms = kz_get_time_ms();
		end_time_s = kz_get_time_s();
	}while(echo);

	/* 開始時刻と終了時刻を計算 */
	start_time_ms = start_time_ms + start_time_s * 1000;
	end_time_ms = end_time_ms + end_time_s * 1000;

	/* 経過時間を算出 */
	elapsed_time_ms = end_time_ms - start_time_ms;

	/* 距離を算出 */
	distance = (uint8_t)((elapsed_time_ms * SPEED_OF_SOUND)/2);

	return distance;
}

下記で500msのスリープを行っています。

500msのスリープ

kz_tsleep(500);

これをすることで、us_sensor_main()は500ms周期で動作するタスクになります。

下記で読んだ距離をバッファに格納しています。

呼んだ距離をバッファへ格納する処理

/* 距離を取得 */
this->data[idx] = read_distance();
idx++;

/* バッファがFULLになった場合 */
if(BUF_SIZE == idx){
	idx = 0;
}

us_sensor_initでは、GPIOの設定を行っています。read_distanceが、超音波センサから距離を取得する関数になります。

8. 簡単な動作確認

超音波センサから、17cm、34cm、51cmのところに対象物を置いた時の測定値を確認していきます。ここでの対象物は私自身の手とします。

17cmに対象物を置いたとき
34cmに対象物を置いたとき
51cmに対象物を置いたとき

※汚い画像で大変申し訳ないです。また、メジャーが反対に見えているのも大変申し訳ないです。

この時の測定値を下記に示します。

data[0]が17cm、data[1]が34cm、data[2]が51cmのときの測定値を表しています。それぞれ期待通りに測定することができました。

9. 今後の予定

bluetoothのモジュールもあるので、超音波センサのデータがある程度たまった時にbluetoothでPCにそのデータを送信するということをしたいと考えています。近々、また記事を書く予定です。

KOZOS上で超音波センサのデータを周期的に取得してみた