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通信モジュールの接続図を下記に示します。
私は、あまり配線を気にせずやってしまうため、汚くなってしまいます。大変申し訳ありません。

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

4. 最後に

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

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