Go to Qiita Advent Calendar 2024 Top
LoginSignup
1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 1 year has passed since last update.

@hcuymitoh(m y)

KOZOS上で超音波センサのデータを周期的にPCへ送信してみた (実装編)

Posted at

1. 今回のやりたいこと

「周期的に超音波センサのデータを取得し、bluetoothを使ってPCにそのデータを送信する」ということをしたいです。

今回のしたいことにおいて、設計編と実装編に分けて記載します。今回は、実装編になります。
設計編については、下記を参照ください。
KOZOS上で超音波センサのデータを周期的にPCへ送信してみた (設計編)

環境と前提は設計編と変わりませんので割愛します。

2. 距離測定システムの実装

距離測定システムの機能を改めて下記に示します。
各機能の詳細については、設計編を参照ください。

  • システム制御機能
  • 超音波センサ制御機能
  • BlueTooth送信機能
    • 接続状態管理機能
    • データ送信機能
  • コンソール機能

全機能の実装を記載すると長くなりますので、超音波センサ制御機能とBlueTooth送信機能の実装のみ記載します。

2.1 超音波センサ制御機能の実装

超音波センサ制御機能
// マクロ定義
#define US_SENSOR_NUM  (1U)
#define BUF_SIZE       (1U)
#define SPEED_OF_SOUND (34U)   // 34cm/ms
#define US_TSK_PERIOD  (1000U) // 1000ms

// 状態
enum State_Type {
	ST_UNINITIALIZED = 0U,
	ST_INITIALIZED,
	ST_RUN,
	ST_STOP,
	ST_UNDEIFNED,
	ST_MAX,
};
// イベント
enum EVENT_Type {
	EVENT_INIT = 0U,
	EVENT_RUN,
	EVENT_STOP,
	EVENT_MAX,
};
// 関数ポインタ
typedef int (*FUNC)(void);

// 状態遷移用定義
typedef struct {
	FUNC func;
	uint8_t nxt_state;
}FSM;

// 制御用ブロック
typedef struct {
	kz_thread_id_t tsk_id;      // メインタスクのID
	kz_msgbox_id_t msg_id;      // メッセージID
	uint8_t state;              // 状態
	US_CALLBACK callback;       // コールバック
	uint8_t data[BUF_SIZE];     // データ格納用バッファ(未使用)
} US_SENSOR_CTL;
US_SENSOR_CTL us_sensor_ctl[US_SENSOR_NUM];

// プロトタイプ宣言
static void us_init(void);
static void us_get_data_start(void);
static void us_get_data_stop(void);
static void us_get_data(void);
static uint8_t read_distance(void);

// 状態遷移テーブル
static const FSM fsm[ST_MAX][EVENT_MAX] = {
		// EVENT_INIT               EVENT_RUN                    EVENT_STOP                   EVENT_MAX
		{{us_init, ST_INITIALIZED}, {NULL, ST_UNDEIFNED},        {NULL, ST_UNDEIFNED},        {NULL, ST_UNDEIFNED},}, // ST_UNINITIALIZED
		{{NULL, ST_UNDEIFNED},      {us_get_data_start, ST_RUN}, {NULL, ST_UNDEIFNED},        {NULL, ST_UNDEIFNED},}, // ST_INITIALIZED
		{{NULL, ST_UNDEIFNED},      {us_get_data, ST_RUN},       {us_get_data_stop, ST_STOP}, {NULL, ST_UNDEIFNED},}, // ST_RUN
		{{NULL, ST_UNDEIFNED},      {us_get_data_start, ST_RUN}, {NULL, ST_UNDEIFNED},        {NULL, ST_UNDEIFNED},}, // ST_STOP
};

// メインのタスク
// 状態遷移テーブルに従い、処理を実行し、状態を更新する
int US_main(int argc, char *argv[])
{
	US_SENSOR_CTL *this = &us_sensor_ctl[0];
	uint8_t cur_state = ST_UNINITIALIZED;
	uint8_t nxt_state = cur_state;
	US_MSG *msg;
	uint32_t size;
	FUNC func;

	// 自タスクのIDを制御ブロックに設定
	this->tsk_id = kz_getid();
	// メッセージIDを制御ブロックに設定
	this->msg_id = MSGBOX_ID_US_MAIN;

	while(1){
		// メッセージ受信
		kz_recv(this->msg_id, &size, &msg);

		// 処理/次状態を取得
		func = fsm[cur_state][msg->msg_type].func;
		nxt_state = fsm[cur_state][msg->msg_type].nxt_state;

		// msgを解放
		kz_kmfree(msg);

		// 現状態を更新
		this->state = nxt_state;

		// 処理を実行
		if (func != NULL) {
			func();
		}

		// 状態を更新
		if (nxt_state != ST_UNDEIFNED) {
			cur_state = nxt_state;
		}
	}

	return 0;
}

// 外部公開関数
// 初期化要求メッセージを送信する関数
void US_MSG_init(void)
{
	US_SENSOR_CTL *this = &us_sensor_ctl[0];
	US_MSG *msg;

	msg = kz_kmalloc(sizeof(US_MSG));

	msg->msg_type = EVENT_INIT;
	msg->msg_data = NULL;

	kz_send(this->msg_id, sizeof(US_MSG), msg);

	return;
}

// データ取得開始メッセージを送信する関数
void US_MSG_measure_start(void)
{
	US_SENSOR_CTL *this = &us_sensor_ctl[0];
	US_MSG *msg;

	msg = kz_kmalloc(sizeof(US_MSG));

	msg->msg_type = EVENT_RUN;
	msg->msg_data = NULL;

	kz_send(this->msg_id, sizeof(US_MSG), msg);

	return;
}

/* データ取得メッセージを送信する関数 */
void US_MSG_get_data(void)
{
	US_SENSOR_CTL *this = &us_sensor_ctl[0];
	US_MSG *msg;

	msg = kz_kmalloc(sizeof(US_MSG));

	msg->msg_type = EVENT_RUN;
	msg->msg_data = NULL;

	kz_send(this->msg_id, sizeof(US_MSG), msg);

	return;
}

/* データ取得停止メッセージを送信する関数 */
void US_MSG_measure_stop(void)
{
	US_SENSOR_CTL *this = &us_sensor_ctl[0];
	US_MSG *msg;

	msg = kz_kmalloc(sizeof(US_MSG));

	msg->msg_type = EVENT_STOP;
	msg->msg_data = NULL;

	kz_send(this->msg_id, sizeof(US_MSG), msg);

	return;
}

/* 超音波センサのデータ取得時にコールするコールバックを設定する関数 */
uint32_t US_set_callback(US_CALLBACK callback)
{
	US_SENSOR_CTL *this = &us_sensor_ctl[0];
	
	// パラメータチェック
	// callbackがNULLの場合、-1を返して終了
	if (callback == NULL) {
		return -1;
	}

	// コールバックがまだ登録されていない場合
	if (this->callback == NULL) {
		// コールバックを登録
		this->callback = callback;
	}
	
	return 0;
}

// 内部関数
// 初期化要求メッセージ受信時に実行する関数
static void us_init(void)
{
	// 詳細については、下記参照ください
    // https://qiita.com/hcuymitoh/items/e905dff59f34466c5f5a
}

// データ取得開始メッセージ受信時に実行する関数
static void us_get_data_start(void)
{
	US_SENSOR_CTL *this = &us_sensor_ctl[0];
	uint32_t ret;
	
	// システム制御機能に、1s周期でデータ取得メッセージを送信してもらうように要求
	ret = set_cyclic_message(US_MSG_get_data, US_TSK_PERIOD);
	
	return;
}

// データ取得メッセージ受信時に実行する関数
static void us_get_data(void)
{
	US_SENSOR_CTL *this = &us_sensor_ctl[0];
	uint8_t data = 0;
	
	// 距離を取得
	data = read_distance();
	
	// コールバックを呼ぶ
	if (this->callback != NULL) {
		this->callback(data);
	}
	
	return;
}

// 超音波センサからデータを取得する関数
static uint8_t read_distance(void)
{
	// 詳細については、下記参照ください
    // https://qiita.com/hcuymitoh/items/e905dff59f34466c5f5a
}

// データ取得停止メッセージ受信時に実行する関数
static void us_get_data_stop(void)
{
	US_SENSOR_CTL *this = &us_sensor_ctl[0];
	uint32_t ret;

	// システム制御機能に、1s周期のデータ取得メッセージの送信を停止してもらうように要求
	ret = del_cyclic_message(US_MSG_get_data);

	return;
}

メインのタスクであるUS_main()は、kz_recv()でメッセージが送信されるまで待機します。
このタスクにメッセージを送信するAPIをUS_MSG_*という名前で作成しています。これらのAPIは外部へ公開されているため、他アプリはこれらのAPIをコールすることでメッセージを送信することができます。US_main()はメッセージを受信すると、状態遷移テーブルに従い、状態を更新し、処理を実行します。
例として、現状態が初期化済み(ST_INITIALIZED)の時にデータ取得メッセージ(EVENT_RUN)を受信したときの動作を説明します。

データ取得メッセージを送信するAPIは下記になります。

データ取得メッセージを送信する関数
/* データ取得メッセージを送信する関数 */
void US_MSG_get_data(void)
{
	US_SENSOR_CTL *this = &us_sensor_ctl[0];
	US_MSG *msg;

	msg = kz_kmalloc(sizeof(US_MSG));

	msg->msg_type = EVENT_RUN;
	msg->msg_data = NULL;

	kz_send(this->msg_id, sizeof(US_MSG), msg);

	return;
}

kz_kmalloc()でメッセージ用のメモリを確保し、kz_send()でUS_main()に対して、データ取得メッセージ(EVENT_RUN)を送信します。
状態遷移テーブルについては、一つの要素を{[実行する処理], [次状態]}とし、これがメッセージ*状態の数だけあります。今回の例のように、現状態が初期化済み（ST_INITIALIZED）で、データ取得メッセージ(EVENT_RUN)を受信したときはus_get_data_start()を実行します。

状態遷移テーブル
// 状態遷移テーブル
static const FSM fsm[ST_MAX][EVENT_MAX] = {
		// EVENT_INIT               EVENT_RUN                    EVENT_STOP                   EVENT_MAX
		{{us_init, ST_INITIALIZED}, {NULL, ST_UNDEIFNED},        {NULL, ST_UNDEIFNED},        {NULL, ST_UNDEIFNED},}, // ST_UNINITIALIZED
		{{NULL, ST_UNDEIFNED},      {us_get_data_start, ST_RUN}, {NULL, ST_UNDEIFNED},        {NULL, ST_UNDEIFNED},}, // ST_INITIALIZED
		{{NULL, ST_UNDEIFNED},      {us_get_data, ST_RUN},       {us_get_data_stop, ST_STOP}, {NULL, ST_UNDEIFNED},}, // ST_RUN
		{{NULL, ST_UNDEIFNED},      {us_get_data_start, ST_RUN}, {NULL, ST_UNDEIFNED},        {NULL, ST_UNDEIFNED},}, // ST_STOP
};

2.2 BlueTooth送信機能の実装

基本的なつくりは、超音波センサ制御機能と同様です。
状態遷移テーブルに従い、処理を実行します。

BlueTooth送信機能
// マクロ定義
#define BT_NUM   (1U)
#define BUF_SIZE (32U)

// 状態
enum State_Type {
	ST_UNINITIALIZED = 0U,
	ST_INITIALIZED,
	ST_CONNECTED,
	ST_DISCONNECTED,
	ST_UNDEIFNED,
	ST_MAX,
};

// イベント
enum EVENT_Type {
	EVENT_INIT = 0U,
	EVENT_CONNECT,
	EVENT_RUN,
	EVENT_DISCONNECT,
	EVENT_GET_STATUS,
	EVENT_MAX,
};

// 関数ポインタ
typedef int (*FUNC)(void);

// 状態遷移用定義
typedef struct {
	FUNC func;
	uint8_t nxt_state;
}FSM;

// バッファ定義
typedef struct {
	uint8_t data[BUF_SIZE];
	uint8_t rd_idx;
	uint8_t wr_idx;
}BUF;

// 制御用ブロック
typedef struct {
	BUF primary;   // データ格納用バッファ (基本このバッファを使用)
	BUF secondary; // データ格納用バッファ (未使用)
}TRANSFER_CTL;
typedef struct {
	kz_thread_id_t tsk_id;      // メインタスクのID
	kz_msgbox_id_t msg_id;      // メッセージID
	uint8_t state;              // 状態
	TRANSFER_CTL trans_ctl;     // 送信データの情報
} BT_CTL;
BT_CTL bt_ctl[BT_NUM];

// プロトタイプ宣言
static void bt_init();
static void bt_send_data();
static void bt_get_status();

// 状態遷移テーブル
static const FSM fsm[ST_MAX][EVENT_MAX] = {
		// EVENT_INIT               EVENT_CONNECT         EVENT_RUN                      EVENT_DISCONNECT         EVENT_GET_STATUS
		{{bt_init, ST_INITIALIZED}, {NULL, ST_UNDEIFNED}, {NULL, ST_UNDEIFNED},          {NULL, ST_UNDEIFNED},    {bt_get_status, ST_UNINITIALIZED}, }, // ST_UNINITIALIZED
		{{NULL, ST_UNDEIFNED},      {NULL, ST_CONNECTED}, {bt_send_data, ST_UNDEIFNED},  {NULL, ST_DISCONNECTED}, {bt_get_status, ST_INITIALIZED}, },   // ST_INITIALIZED
		{{NULL, ST_UNDEIFNED},      {NULL, ST_CONNECTED}, {bt_send_data, ST_CONNECTED},  {NULL, ST_DISCONNECTED}, {bt_get_status, ST_CONNECTED}, },     // ST_CONNECTED
		{{NULL, ST_UNDEIFNED},      {NULL, ST_CONNECTED}, {NULL, ST_UNDEIFNED},          {NULL, ST_DISCONNECTED}, {bt_get_status, ST_DISCONNECTED}, },  // ST_DISCONNECTED
};

int BT_main(int argc, char *argv[])
{
	BT_CTL *this = &bt_ctl[0];
	uint8_t cur_state = ST_UNINITIALIZED;
	uint8_t nxt_state = cur_state;
	BT_MSG *msg;
	int32_t size;
	FUNC func;

	// 制御ブロックの初期化
	memset(bt_ctl, 0, sizeof(bt_ctl));

　　// 自タスクのIDを制御ブロックに設定
	this->tsk_id = kz_getid();
　　// メッセージIDを制御ブロックに設定
	this->msg_id = MSGBOX_ID_BTMAIN;

	while(1){
		// メッセージ受信
		kz_recv(this->msg_id, &size, &msg);

		// 処理/次状態を取得
		func = fsm[cur_state][msg->msg_type].func;
		nxt_state = fsm[cur_state][msg->msg_type].nxt_state;

		// メッセージを解放
		kz_kmfree(msg);

		this->state = nxt_state;

		// 処理を実行
		if (func != NULL) {
			func();
		}

		// 状態遷移
		if (nxt_state != ST_UNDEIFNED) {
			cur_state = nxt_state;
		}
	}

	return 0;
}

// 外部公開関数
/* 初期化メッセージを送信する関数 */
void BT_MSG_init(void)
{
	BT_CTL *this = &bt_ctl[0];
	BT_MSG *msg;

	msg = kz_kmalloc(sizeof(BT_MSG));

	msg->msg_type = EVENT_INIT;
	msg->msg_data = NULL;

	kz_send(this->msg_id, sizeof(BT_MSG), msg);

	return;
}

// 接続済みメッセージを送信する関数
void BT_MSG_connect(void)
{
	BT_CTL *this = &bt_ctl[0];
	BT_MSG *msg;

	// 現状態がCONNECTEDの場合、イベントを発行しない
	if (this->state == ST_CONNECTED) {
		return;
	}

	msg = kz_kmalloc(sizeof(BT_MSG));

	msg->msg_type = EVENT_CONNECT;
	msg->msg_data = NULL;

	kz_send(this->msg_id, sizeof(BT_MSG), msg);

	return;
}

// データ送信要求メッセージを送信する関数
void BT_MSG_send_data(uint8_t *data, uint8_t size)
{
	BT_CTL *this = &bt_ctl[0];
	BT_MSG *msg;
	uint8_t i;

	msg = kz_kmalloc(sizeof(BT_MSG));

	// 内部バッファにコピー
	for (i = 0; i<size; i++) {
		this->trans_ctl.primary.data[this->trans_ctl.primary.wr_idx++] = data[i];
		this->trans_ctl.primary.wr_idx = this->trans_ctl.primary.wr_idx & (BUF_SIZE - 1);
	}

	msg->msg_type = EVENT_RUN;
	msg->msg_data = this->trans_ctl.primary.data;

	kz_send(this->msg_id, sizeof(BT_MSG), msg);

	return;
}

// 非接続メッセージを送信する関数
void BT_MSG_disconnect(void)
{
	BT_CTL *this = &bt_ctl[0];
	BT_MSG *msg;

	// 現状態がDISCONNECTEDの場合、イベントを発行しない
	if (this->state == ST_DISCONNECTED) {
		return;
	}

	msg = kz_kmalloc(sizeof(BT_MSG));

	msg->msg_type = EVENT_DISCONNECT;
	msg->msg_data = NULL;

	kz_send(this->msg_id, sizeof(BT_MSG), msg);

	return;
}

// 状態取得メッセージを送信する関数
void BT_MSG_get_status(void)
{
	BT_CTL *this = &bt_ctl[0];
	BT_MSG *msg;

	msg = kz_kmalloc(sizeof(BT_MSG));

	msg->msg_type = EVENT_GET_STATUS;
	msg->msg_data = NULL;

	kz_send(this->msg_id, sizeof(BT_MSG), msg);

	return;
}

// 内部関数
// 初期化要求メッセージ受信時に実行する関数
static void bt_init(void)
{
	// BlueToothの初期化
	bt_dev_send_use(SERIAL_BLUETOOTH_DEVICE);

	return;
}

//　データ送信要求メッセージ受信時に実行する関数
static void bt_send_data(void)
{
	BT_CTL *this = &bt_ctl[0];
	uint8_t val[3];
	char data[3];

	// 3桁目の数字を取得
	val[2] = this->trans_ctl.primary.data[this->trans_ctl.primary.rd_idx] / 100;
	// 2桁目の数字を取得
	val[1] = (this->trans_ctl.primary.data[this->trans_ctl.primary.rd_idx] - val[2]*100) / 10 ;
	// 1桁目の数字を取得
	val[0] = (this->trans_ctl.primary.data[this->trans_ctl.primary.rd_idx] - val[2]*100 - val[1]*10);

	// バッファのリードインデックスを更新
	this->trans_ctl.primary.rd_idx++;
	this->trans_ctl.primary.rd_idx = this->trans_ctl.primary.rd_idx & (BUF_SIZE - 1);

	// アスキーコードに変換
	data[0] = val[2] + 0x30;
	data[1] = val[1] + 0x30;
	data[2] = val[0] + 0x30;

	// BlueTooth経由でPCへ送信
	bt_dev_send_write(data);

	return;
}

//　状態取得メッセージ受信時に実行する関数
void bt_get_status(void)
{
	BT_CTL *this = &bt_ctl[0];

	switch(this->state){
	case ST_UNINITIALIZED:
		consdrv_send_write("ST_UNINITIALIZED\n");
		break;
	case ST_INITIALIZED:
		consdrv_send_write("ST_INITIALIZED\n");
		break;
	case ST_CONNECTED:
		consdrv_send_write("ST_CONNECTED\n");
		break;
	case ST_DISCONNECTED:
		consdrv_send_write("ST_DISCONNECTED\n");
		break;
	case ST_UNDEIFNED:
		consdrv_send_write("ST_UNDEIFNED\n");
		break;
	}

	return;
}

3. 動作の様子

BlueTooth経由でデータを受信することを確認するために、PCにBlueTooth用のターミナルをインストールする必要があります。
今回は下記をインストールしました。
https://apps.microsoft.com/store/detail/bluetooth-serial-terminal/9WZDNCRDFST8?hl=ja-jp&gl=jp

実際のボード、超音波センサ、BlueTooth通信モジュールの接続図を下記に示します。
私は、あまり配線を気にせずやってしまうため、汚くなってしまいます。大変申し訳ありません。
実際の接続図.PNG

  • 動作時のPC画面
    距離測定システム動作時のPC画面を下記に示します。
    左がBlueTooth経由で受信したデータを表示するためのアプリで、右がコンソールになります。
    コンソールに"command>"と表示されていますが、これはnucleo-l4r5ziからの出力で、nucleo-l4r5ziがコマンドを受け付ける状態になっていることを表しています。
    　起動時.PNG
    コンソールに"measure start"と打ち込みます。
    blueToothアプリのほうに超音波センサのデータが表示されるようになります。
    measure start.PNG

4. 最後に

今回、設計編と実装編の記事を記載してみました。
本件について、何か不明点・アドバイスがあればコメントしていただけると嬉しいです。

次回は、また別の記事を記載する予定です。

1
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?