Go to Qiita Advent Calendar 2024 Top
LoginSignup
3
4

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.

@kobatan(kobatan)

リモートID受信機を自作する。（パート５）

Last updated at Posted at 2023-03-25

複数のRIDに対応しました。

OpenDroneIDアプリを参考にしました。
OpenDroneIDアプリでは、発信機の区別を何でしているのか調べたら、BLEの6バイトのMACアドレスで行っていたので、私もそうします。
なので、RID送信機を自作される場合、送信機のBLE MACアドレスはそれぞれ個別の、世界で唯一のアドレスを設定して下さい。

uint8_t addr_coded[6] = {0xC0, 0xDE, 0x52, 0x00, 0x00, 0x01};	// BLE Address (上位2bit[11] = Random Address：Static Device Address) 他の人と被らない数値を設定して下さい。

準備

前回までを、まず作って下さい。

GNSS(GPS)モジュールは取り付けても、付けなくてもどちらでも良いです。
付ければ、操縦者(受信機)の位置もマップに表示されます。

プログラム

新たに[info.html]を[data]フォルダに作ってください。
中身は以下です。

info.html
<!DOCTYPE html>
<html lang="ja">
<head>
<meta charset="UTF-8" name = "viewport"  content = "width=device-width, initial-scale=1">
<title>RID Receiver</title>
<style>
	body	{background-color:rgba(128,128,128,0.322);}
	h1		{font-size: 1rem; margin: 0px; text-align: center; color:rgb(156,5,5); background-color: whitesmoke;}
	h2		{font-size: 1rem; margin: 0px; color:black;}
	span	{color:blue;}
	.pos	{position:absolute; left: 0px; top:0px; width: 1000px; height: 1000px;}
	a		{font-size: 1rem; margin-left: 40px; color:rgb(133, 67, 255);}
</style>
</head>
<!-----------------------------------------HTML----------------------------------------->
<body>
	<h1>RID受信情報</h1>
	<h2>
		名前: <span style="color:rgb(216, 3, 3)" id="name"></span><a id="sat"></a><br>
		MAC: <span id="addr"></span><br>
		RSSI: <span id="rssi">0</span><br>
		Primary PHY: <span id="pPhy"></span><br> 
		Secondary PHY: <span id="sPhy"></span><br>
		Tx Power: <span id="pw"></span><span>dBm</span><br>
		Counter: <span id="cnt"></span><br>
		---------------------------------<br>
		製造番号: <span id="SN"></span><br>
		登録記号: <span id="RN"></span><br>
		機体種別: <span id="UA"></span><br>
		緯度: <span id="lat"></span><br>
		経度: <span id="lng"></span><br>
		速度: <span id="speed"></span><br>
		高度: <span id="alti"></span><br>
		方角: <span id="dir"></span><br>		
		時刻: <span id="time"></span><br>
	<div id="dist"></div>		
	</h2>
</body>
<!-------------------------------------JavaScript--------------------------------------->
<script>
	var mac;						// BLE MAC address

	var R_EARTH = 6378137;			// 地球の赤道半径
	var RAD = Math.PI / 180;		// 1°あたりのラジアン
	function distance(lat1, lon1, lat2, lon2) {		                        // 2点間の距離を求める関数
		lat1 *= RAD;														// 度をラジアンに変換
		lon1 *= RAD;
		lat2 *= RAD;
		lon2 *= RAD;
		var lat_c = (lat1 + lat2) / 2;							            // 緯度の中心値
		var dx = R_EARTH * (lon2 - lon1) * Math.cos(lat_c);
		var dy = R_EARTH * (lat2 - lat1);
		return  Math.round((Math.sqrt(dx * dx + dy * dy)).toFixed(1));		// 2点間の距離を返す
	}

	function InitWebSocket(){																						// Websocketの初期設定
		websock = new WebSocket('ws://'+window.location.hostname+':81/');	// Websocket開始

		websock.onmessage = function(evt){       							// データが送られて来た時の処理
			JSONobj = JSON.parse(evt.data);									// Json文字列を解析
			if(JSONobj.addr == mac){
				document.getElementById('name').innerHTML = JSONobj.name;
				document.getElementById('addr').innerHTML = JSONobj.addr;
				document.getElementById('rssi').innerHTML = JSONobj.rssi;
				document.getElementById('cnt').innerHTML = JSONobj.cnt;
				document.getElementById('SN').innerHTML = JSONobj.SN;
				document.getElementById('RN').innerHTML = JSONobj.RN;
				document.getElementById('UA').innerHTML = JSONobj.UA;		
				document.getElementById('lat').innerHTML = JSONobj.lat;
				document.getElementById('lng').innerHTML = JSONobj.lng;
				document.getElementById('speed').innerHTML = JSONobj.speed;
				document.getElementById('alti').innerHTML = JSONobj.alti;
				document.getElementById('dir').innerHTML = JSONobj.dir;			
				document.getElementById('time').innerHTML = JSONobj.time;                
				document.getElementById('pPhy').innerHTML = JSONobj.pPhy;
				document.getElementById('sPhy').innerHTML = JSONobj.sPhy;
				document.getElementById('pw').innerHTML = JSONobj.pw;
				if(Number(JSONobj.sat) != 0){																				// 補足衛星数がゼロでなかったら	
					document.getElementById('sat').innerHTML = JSONobj.sat + "衛星";	// 衛星数を表示する
				}
				let op_lng = parseFloat(JSONobj.oplng);															// オペレータ位置情報
				let op_lat = parseFloat(JSONobj.oplat);
				if(op_lng > 0 && op_lat > 0){
					let lng = parseFloat(JSONobj.lng);																// RIDの位置情報
					let lat = parseFloat(JSONobj.lat);					
					document.getElementById('dist').innerHTML = "距離: " + String(distance(op_lat, op_lng, lat, lng)) + "m";	// 2点間の距離
				}else{
					document.getElementById('dist').innerHTML == "";
				}
			}
		}
	}

	window.onload = function() {					// ページが読み込まれたら最初に実行する
		InitWebSocket();

 		const url = new URL(window.location.href); 	// URLを取得
		const params = url.searchParams;			// URLSearchParamsオブジェクトを取得
		mac = params.get("mac");		            // パラメータから「MACアドレス」を取得
	}

</script>
</html>

index.htmlを書き換えます。

index.html
<!DOCTYPE html>
<html lang="ja">
<head>
<meta charset="UTF-8" name = "viewport"  content = "width=device-width, initial-scale=1">
<title>RID Receiver</title>
<style>
	body	 {background-color:rgba(128,128,128,0.322);}
	h1		 {font-size: 0.7rem; width: 100%; height: 26px; margin: 0px; vertical-align:middle; padding-top:0px; text-align:center; color:rgb(255, 255, 255); background-color: rgb(60, 60, 255);}
	a		 {vertical-align:middle; margin: 0px; padding-top:0px;}
	button   {font-size: 0.7rem; float: right; margin-right:3px; margin-top: 0px; padding-top: 0px; background-color: rgba(255,255,255,0.5);}
	.map	 {position:absolute; width: 100%; margin: 0px; left: 0px; top:0px; z-index: -1;}
	.mark	 {position:absolute; margin: 0px; padding: 0px; visibility:hidden;}
	table	 {font-size: 0.9rem; width: 100%; margin-top:0px;position:relative;}
	td		 {background-color: rgb(160, 200, 255);}
</style>
</head>
<!-----------------------------------------HTML----------------------------------------->
<body>
	<h1>
		<a id="title">0 機</a><a id="sat" style="margin-left: 80px;"></a><button id="clear">クリア</button>
	</h1>	
	<div id="area" style="position:relative; margin-top:0px; width:100%;">
		<img src="map0" id="map0" alt="" class="map" style="visibility: hidden;">
	 	<img src="map1" id="map1" alt="" class="map" style="visibility: hidden;">
	 	<img src="map2" id="map2" alt="" class="map" style="visibility: hidden;">
		<img src="map"  id="map"  alt="" class="map" style="visibility: visible;">
		<canvas id="cvs" class="map" width="1000px" height="1000px"></canvas>
		<img src="opmark" id="opmark" alt="" class="mark">
		<img src="mark" id="mark" alt="" class="mark">
	</div>
	<table id="tbl">
		<tbody>
		</tbody>
	</table>
</body>
<!-------------------------------------JavaScript--------------------------------------->
<script>
	var w ,h;						// マップの幅,高さ
	var cv_top, cv_left, cv_bottom;
	var mk_scale = 1;				// マークのスケール
	var mk_top, mk_left;			// マークの左座標の位置
	var opmk_top, opmk_left;	    // オペレータマーク
	var lng0,lat0,lng1,lat1;
	var curMap = 0;
	var bles = [];					// 機体データ配列
	var cnt = 0;					// 表示切替カウンター

	const area = document.getElementById('area');					// マップエリア
	const title = document.getElementById('title');					// 機体数
	const cvs = document.querySelector('#cvs');						// キャンバス
	const ctx = document.getElementById('cvs').getContext('2d');	// キャンバスのコンテキスト
	const mk = document.getElementById('mark');						// マークのオブジェクト	
	const opmk = document.getElementById('opmark');					// オペレータマークのオブジェクト
	const sat = document.getElementById('sat');					    // オペレータの衛星数
	const tbl = document.getElementById('tbl');						// 機体テーブル
	const btn = document.getElementById('clear');					// クリアボタン
	btn.addEventListener('click', function(){clear()},false);		// クリアボタンが押された時の処理先を指定

	var maps = [
		{ 
			obj: document.getElementById('map0'),	// 自宅周辺の地図
			lat0: 30.000000,   // <------ ここを書き換えてください
			lng0: 100.000000,  // <------ ここを書き換えてください
			lat1: 30.100000,   // <------ ここを書き換えてください
			lng1: 100.100000   // <------ ここを書き換えてください
		},
		{
			obj: document.getElementById('map1'),	// 良く行く場所の地図
			lat0: 31.000000,   // <------ ここを書き換えてください
			lng0: 101.000000,  // <------ ここを書き換えてください
		    lat1: 31.100000,   // <------ ここを書き換えてください
			lng1: 101.100000   // <------ ここを書き換えてください
		},
		{ 
			obj: document.getElementById('map2'),	// 遠征先の地図
			lat0: 32.000000,   // <------ ここを書き換えてください
			lng0: 102.000000,  // <------ ここを書き換えてください
			lat1: 32.100000,   // <------ ここを書き換えてください
			lng1: 102.100000   // <------ ここを書き換えてください
		},	
		{ 
			obj: document.getElementById('map'),	// 日本全体の地図（↑のどれにも当てはまら無い場合に表示する。これを一番最後に入れて下さい。）
			lat0: 23.584126,
			lng0: 122.058105,
			lat1: 45.874712,
			lng1: 149.523926
		}	
	];


	const clear = function(){				// クリア処理
		ctx.clearRect(0, 0, w, h);			// キャンバスのラインを消す
		var n = tbl.rows.length;   			// 表の行数を取得
		for(let i=0; i<n; i++){				// マークを削除
			let elm = document.getElementById("mark"+i);
			elm.remove();
		}
        while (n>0) {						// テーブルの行を削除 
            tbl.deleteRow(n - 1);   		// 末尾行を削除
		    n--;
		}	
		bles.length = 0;					// 配列の中身を全消し
		title.textContent = 0 + " 機";
	}

	function add_mark(no){					// マークを追加
		let mark = document.createElement("img");
		mark.src = "mark";
		mark.id = "mark"+ no;
		mark.alt = "";
		mark.style.position = "absolute";
		mark.style.width = Math.round(mark.width * mk_scale) + 'px';
		mark.style.height = Math.round(mark.height * mk_scale) + 'px';
		mark.style.zIndex = "1";
		area.appendChild(mark);
		return mark;
	}
	
	const func_click = function(n){		    // 機種名を押された時の処理
		var row = tbl.rows.length;
		for (let i=0; i < row; i++){
			tbl.rows[i].cells[0].style.backgroundColor = "rgb(160, 200, 255)";
			bles[i].mark.style.opacity = 0.7;				
		}
		tbl.rows[n].cells[0].style.backgroundColor = "rgb(130, 130, 255)";
		bles[n].mark.style.opacity = 1;	
	}
	
	const func_link = function(addr){	    // 情報ボタンを押された時の処理
		location.href="./info.html?mac=" + addr;
	}

	function add_table(bl){					// テーブルに追加
		let n = tbl.rows.length;
		let tr = document.createElement("tr");
		let td1 = document.createElement("td");
		let td2 = document.createElement("td");	
		let bt2 = document.createElement("button");	
		
		td1.textContent = bl.RN + " " + bl.rssi + "dBm";
		td1.addEventListener('click', function(){func_click(n)},false);
	
		bt2.textContent = "情報";
		td2.appendChild(bt2);
		td2.addEventListener('click', function(){func_link(bles[n].addr)},false);
		tr.appendChild(td1);
		tr.appendChild(td2);
		tbl.appendChild(tr);
	}

	function add_ble(bl){					// BLEを追加する
		let n = bles.push(bl) - 1;			// 配列に追加
		add_table(bl);						// テーブルに追加
		bl.mark = add_mark(n);				// マークを追加
	}
		
	function info(obj){
		let n = obj.parentNode.rowIndex;	// 押された行番号を得る
		location.href = "./info.html?mac="+ bles[n].addr;
	}

	function drawLine(bl,x,y){				// 移動経過のラインを描画
		if(bl.x0 != 0){
			ctx.beginPath();
     	    ctx.moveTo(bl.x0, bl.y0);
     	    ctx.lineTo(x, y);									
			ctx.strokeStyle = '#FF0000';	// ラインの色
  		    ctx.lineWidth = 1;				// ラインの幅
			ctx.stroke();
		}
		bl.x0 = x;
		bl.y0 = y;
	}

	function setMap(n){						// マップ表示を切り替える
		for(let i=0; i < maps.length; i++){
			maps[i].obj.style.visibility = "hidden";
		}
		maps[n].obj.style.visibility = "visible";
		lng0 = maps[n].lng0;
		lat0 = maps[n].lat0;
		lng1 = maps[n].lng1;
		lat1 = maps[n].lat1;
		curMap = n;
		ctx.clearRect(0, 0, w, h);	        // キャンバスのラインを消す
		x0 = 0;
	}
			
	function mapCheck(lng,lat){				// 経度、緯度から表示するマップを切り替える
		let n = maps.findIndex(e => e.lng0 <= lng && e.lat0 <= lat && lng <= e.lng1 && lat <= e.lat1 )
		if(n !== -1){
			if( n != curMap){
				setMap(n);
			}
		}
	}

	function culcXY(lng,lat){				// 経度緯度をxy座標に変換
		let obj ={
			x : Math.round( w * (lng - lng0) / (lng1 - lng0)),
			y : h - Math.round( h * (lat - lat0) / (lat1 - lat0))
		}
		return obj;
	}

	function InitWebSocket(){											    // Websocketの初期設定
		websock = new WebSocket('ws://'+window.location.hostname+':81/');	// Websocket開始

		websock.onmessage = function(evt){							        // データが送られて来た時の処理
			var add = false;
			var bl;

			JSONobj = JSON.parse(evt.data);							    	// 受信データを解析
			if(JSONobj.addr != "00:00:00:00:00:00"){
				bl = bles.find(e=> e.addr === JSONobj.addr);    			// 既に配列データに格納されているか？
				if(bl == undefined){										// 無ければ新たに作る
					bl = new Object();
					add = true;
				}	
				bl.addr = JSONobj.addr;										// データを更新する
				bl.sat = JSONobj.sat;
				bl.rssi = JSONobj.rssi;
				bl.SN = JSONobj.SN;
				bl.RN = JSONobj.RN;
				bl.lat = JSONobj.lat;
				bl.lng = JSONobj.lng;
				if(add)	add_ble(bl);										// 配列に加える	
			}

			var ct = bles.length;											// 受信している機体の個数
			let op_lng = parseFloat(JSONobj.oplng);			                // オペレータ位置情報
			let op_lat = parseFloat(JSONobj.oplat);
			let ox = oy = 0;

			if(op_lng > 0 && op_lat > 0){									// オペレータ位置情報がある場合
				if(ct == 0) mapCheck(op_lng, op_lat);			            // RID機体がゼロの場合、受信機のマップにする。

				let oxy = culcXY(op_lng, op_lat);				            // 経度緯度をxy座標に変換
				ox = oxy.x;
				oy = oxy.y;
				if(curMap == maps.length-1){
					oy += Math.round(36 * mk_scale);			            // 日本地図では何故か縦づれするので補正
				}
				opmk.style.left = ox - opmk_left - 8 + 'px';	            // オペレータマークの位置を変更(マークの左上角座標)	
				opmk.style.top =  oy - opmk_top - 6 + 'px';		
				opmk.style.visibility = "visible";
				sat.textContent = "受信機 " + JSONobj.opsat + " 衛星";
			}else{
				opmk.style.visibility = "hidden";
				sat.textContent = "";
			}

	
			title.textContent = ct + " 機";								    // 表示
	
			for(let i=0; i < ct; i++){										// 機体の個数分のマーク表示
				bl = bles[i];
				let lng = parseFloat(bl.lng);								// RID 位置情報
				let lat = parseFloat(bl.lat);
				mapCheck(lng,lat);											// 現在地に合わせて、マップを変更する
	
				let xy = culcXY(lng, lat);									// 経度緯度をxy座標に変換
				let x = xy.x;																							
				let y = xy.y;																							

				if(curMap == maps.length-1){
					y += Math.round(34 * mk_scale);						    // 日本地図では何故かづれするので補正
					x -= Math.round(6 * mk_scale);
				}
				bl.mark.style.left = x - mk_left - 8 + 'px';				// マークの位置を変更(マークの左上角座標)	
				bl.mark.style.top =  y - mk_top - 6 + 'px';
    			drawLine(bl, x, y);	         								// 軌跡を描画
				
				cnt++;        									        	// 定期的に機体情報の表示項目を変える												
				let str = bl.RN;    										// 登録記号
				if(cnt > 30 && cnt < 60){														
					str =  bl.SN;											// 製造番号
				}else if(cnt > 60){
					cnt = 0;
				}
				str += " " + bl.rssi + "dBm ";
				if(Number(bl.sat) != 0){
					str +=  bl.sat + "衛星";
				} 
				tbl.rows[i].cells[0].textContent = str;						// 機体情報を表示 
			}
		}
	}

	function initScreen(){										// スマホの画面に合わせてサイズを変更する
		w = window.innerWidth;									// 画面の幅をマップのサイズにする
		h = w;
		tbl.style.top = h - 17 + 'px';
		area.height = h;
		cvs.setAttribute("width", w);
		cvs.setAttribute("height", h);
		mk_scale = w / 1000;
		mk_left = Math.round(mk.width * mk_scale/2);
		mk_top = Math.round(mk.height * mk_scale);
		opmk_left = Math.round(opmk.width * mk_scale/2);
		opmk_top = Math.round(opmk.height * mk_scale);
		opmk.style.width = opmk_left * 2 + 'px';
		opmk.style.height = opmk_top + 'px';			
	}

	window.onload = function() {								// ページが読み込まれたら最初に実行する
		initScreen();											// 画面の大きさ調整
		InitWebSocket();										// ウェブソケット初期化
		setMap(0);												// 最初に表示するマップ番号を指定
	}

	window.addEventListener('DOMContentLoaded', () => {
  	    cv_top = Math.round(cvs.getBoundingClientRect().top + window.scrollY);
		cv_left = Math.round(cvs.getBoundingClientRect().left + window.scrollY);
		cv_bottom = Math.round(cvs.getBoundingClientRect().bottom + window.scrollY);		
	})
</script>
</html>

main.cppを書き換えます。

main.cpp
#include <Arduino.h>
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEScan.h>
#include <BLEAdvertisedDevice.h>
#include <WiFi.h>
#include <ESPAsyncWebServer.h>
#include <WebSocketsServer.h>
#include <SPIFFS.h>
#include <FS.h>
#include <TinyGPSPlus.h>

const char *ssid="RID_Receiver";
const char *pass="12345678";		   // パスワード（８Byte以上）
const IPAddress ip(192,168,5,1);	   // IPアドレス
const IPAddress subnet(255,255,255,0); // サブネットマスク

// ADタイプ
#define AD_TYPE_FLAG			0x01	// 発信モード
#define AD_TYPE_SHT_LOCAL_NAME	0x08	// 短縮名
#define AD_TYPE_CMP_LOCAL_NAME	0x09	// 名前
#define AD_TYPE_TX_POWER		0x0A	// 送信出力値
#define AD_TYPE_SERVICE_DATA	0x16	// サービスデータ型 [16-bit UUID]

// メッセージ種別
#define MSG_TYPE_BASIC_ID		0x00	// 製造番号、登録記号
#define MSG_TYPE_LOCATION		0x10	// 位置情報
#define MSG_TYPE_AUTH			0x20	// 認証情報
#define MSG_TYPE_SELF_ID		0x30	// 操作目的 [文字列]
#define MSG_TYPE_SYSTEM			0x40	// リモートパイロットの位置情報
#define MSG_TYPE_OPERATOR_ID	0x50	// 操縦者ID [文字列]
#define MSG_TYPE_PACK  			0xF0	// パッケージ

// UAS ID Type
#define ID_TYPE_SerialNo		0x10	// 製造番号種別
#define ID_TYPE_ASSIGED_REG		0x20	// 登録番号種別
#define ID_TYPE_UUID			0x30	// UTM Assigned UUID

// 機体種別
#define UA_TYPE_NON							0	// なし
#define UA_TYPE_AEROPLANE					1	// 飛行機
#define UA_TYPE_HELICOPTER					2	// ヘリコプター
#define UA_TYPE_GYROPLANE					3	// ジャイロプレーン
#define UA_TYPE_HYBRID_LIFT					4	// 垂直離陸固定翼機（ハイブリッドリフト）
#define UA_TYPE_ORINITHOPTER				5	// 羽ばたき機（オルニソプター）
#define UA_TYPE_GLIDER						6	// グライダー（滑空機）
#define UA_TYPE_KITE						7	// カイト（凧）
#define UA_TYPE_FREE_BALLOON				8	// 自由気球
#define UA_TYPE_CAPITIVE_BALLOON			9	// 係留気球
#define UA_TYPE_AIRSHIP						10	// 飛行船
#define UA_TYPE_FREE_FALL_PARACHUTE			11	// パラシュート
#define UA_TYPE_ROCKET						12	// ロケット
#define UA_TYPE_TETHERED_POWERED_AIRCRAFT   13	// テザー式動力航空機
#define UA_TYPE_GROUND_OBSTACLE				14	// 地上障害物
#define UA_TYPE_OTHER						15	// その他

// 位置情報
#define LOC_STA_NON			0x00	// 不明
#define LOC_STA_GROUND	    0x10	// 地面
#define LOC_STA_AIRBRONE	0x20	// 飛行中
#define LOC_FLAG_HT			0x04	// Height Type 0:Above Takeoff 1:AGL(Above Ground Level)地面からの高度
#define LOC_FLAG_EW 		0x02	// Eeast/West Direction 0:<180 1>=180
#define LOC_FLAG_SM 		0x01	// Speed Multiplier 0:x0.25 1:x0.75

#pragma pack(push,1)	// データを１バイト単位に詰めて配置
typedef struct{
	uint8_t type;				// Location(MSG_TYPE_LOCATION)
	uint8_t status;				// 飛行中、方角E/W、速度倍率などの状態
	uint8_t dir;				// 方角
	uint8_t speed;				// 速度
	uint8_t Ver_speed;			// 垂直速度
	uint32_t lat;				// 緯度
	uint32_t lng;				// 経度							
	uint16_t Pressur_Altitude;	// 気圧高度
	uint16_t Geodetic_Altitude;	// GPS高度
	uint16_t Height;			// 地面からの高さ
	uint8_t V_H_Accuracy;		// 垂直、水平速度精度
	uint8_t B_S_Accuracy;		// 気圧、速度精度
	uint16_t timestamp;			// 現在時間の分以下小数点第１位までの秒数X10
	uint8_t T_Accuracy;			// 時間精度
	uint8_t resv3;
} location;

typedef struct{
	uint8_t type;			// 認証情報
	uint8_t Auth_Type;		// Authentication Message [認証情報]
	uint8_t page_count;		// Page0
	uint8_t Length;        	// headからのサイズ
	time_t timestamp_auth;	// 現在時刻(2019.1.1からの秒数)
	uint8_t auth_head;      // ヘッダ
	char auth_data[16];		// 認証データ
}AUTH;
#pragma pack(pop)

TinyGPSPlus gps;
AsyncWebServer server(80);
WebSocketsServer webSocket = WebSocketsServer(81);

uint32_t scanTime = 100; //In 10ms (1000ms)
BLEScan* pBLEScan;

String phy[3] ={"1M PHY","2M PHY","Coded PHY"};
String ua[16] ={"なし","飛行機","ヘリコプター","ジャイロプレーン","ハイブリッドリフト","羽ばたき機","グライダー","カイト（凧）","自由気球","係留気球","飛行船","パラシュート","ロケット"," テザー式動力航空機","地上障害物","その他"};
uint8_t msg_size = 25;
location *loc;
AUTH *auth;
uint8_t counter, ua_type;
char name[30],serNo[23],regNo[23],addr[18],sat[4];
char s_lat[20],s_lng[20],s_time[22],s_speed[20],s_alti[20], s_dir[6],op_lat[20],op_lng[20],op_sat[10];
int8_t rssi;
uint8_t primary_phy, secondary_phy, tx_power;
String jsonTxt;
bool dataReady = false;
uint16_t ss=0;

void setJson(){
	jsonTxt ="{\"addr\":\""		+ String(addr) +	"\",";
	jsonTxt += "\"name\":\""	+ String(name) +	"\",";	
	jsonTxt += "\"sat\":\""		+ String(sat) +		"\",";		
	jsonTxt += "\"rssi\":\""	+ String(rssi) +	"\",";
 	jsonTxt += "\"cnt\":\""		+ String(counter) + "\",";	
	jsonTxt += "\"SN\":\""		+ String(serNo) +	"\",";
	jsonTxt += "\"RN\":\""		+ String(regNo) +	"\",";
	jsonTxt += "\"UA\":\""		+ ua[ua_type] +	    "\",";
	jsonTxt += "\"lat\":\""		+ String(s_lat) + 	"\",";
	jsonTxt += "\"lng\":\""		+ String(s_lng) +	"\",";
	jsonTxt += "\"speed\":\""	+ String(s_speed) +	"\",";
	jsonTxt += "\"alti\":\""	+ String(s_alti) +	"\",";
	jsonTxt += "\"dir\":\""		+ String(s_dir) +	"\",";	
	jsonTxt += "\"time\":\""	+ String(s_time) +	"\",";
	jsonTxt += "\"pPhy\":\""	+ phy[primary_phy] +	"\",";
	jsonTxt += "\"sPhy\":\""	+ phy[secondary_phy] +"\",";
	jsonTxt += "\"pw\":\""		+ String(tx_power) +	"\",";
	jsonTxt += "\"opsat\":\""	+ String(op_sat) + 	"\",";
	jsonTxt += "\"oplat\":\""	+ String(op_lat) + 	"\",";
	jsonTxt += "\"oplng\":\""	+ String(op_lng) +	"\"}";	
	dataReady = true;
}

time_t tm2019;	// 1900年から2019年までの秒数
void get_tm2019(){
  struct tm stm;
  stm.tm_year = 2019 - 1900;
  stm.tm_mon  = 0;
  stm.tm_mday = 1;
  stm.tm_hour = 0;
  stm.tm_min  = 0;
  stm.tm_sec  = 0;
  tm2019 = mktime( &stm );
}

void gps_read(void){				// GNSSのデータを読む
	float lat = 0;
	float lng = 0;

	if(gps.satellites.isValid()){   // 補足衛星数
	  sprintf(op_sat,"%d", gps.satellites.value());
	}

    if (gps.location.isValid()){    // 緯度経度
  	  lat = gps.location.lat();
  	  lng = gps.location.lng();
	}
	sprintf(op_lat,"%10.7f",lat);
	sprintf(op_lng,"%10.7f",lng);		
	if(lat > 0)	setJson();
}

uint8_t *decode_msg(uint8_t *data){	// メーッセージデータをデコードする
	uint8_t *adr = data;	
	uint8_t type,size,n;
	uint8_t id_type,status,dir;
	float lat,lng,alti;
    uint16_t h,m,s;
	uint16_t speed;
	time_t tim;
	struct tm t; 
	
	type = data[0] & 0xF0;
	
	switch(type){
		case MSG_TYPE_PACK:							// パケットデータ
			msg_size = data[1];
			n 	 = data[2];
			adr  = &data[3];
			for(int i=0;i<n;i++){
				adr = decode_msg(adr);
			}
			break;
			
		case MSG_TYPE_BASIC_ID:
			id_type = data[1] & 0xF0;
			if(id_type == ID_TYPE_SerialNo){		// 製造番号
				memcpy(serNo, &data[2], 20);
				ua_type = data[1] & 0x0F;			// 機体種別
				adr += msg_size;
			}
			if(id_type == ID_TYPE_ASSIGED_REG){	    // 登録記号
				memcpy(regNo, &data[2], 20);
				adr += msg_size;
			}
			break;
		
		case MSG_TYPE_LOCATION:						// 位置情報
			loc = (location *)&data[0];

			status = loc->status; 
			lat = (float)loc->lat/10000000;	        // 緯度
			lng = (float)loc->lng/10000000;	        // 経度
			sprintf(s_lat,"%10.7f",lat);
			sprintf(s_lng,"%10.7f",lng);

			h = loc->timestamp%36000;				// 時刻
			m = h/600;
			s = (h%600)/10;
			ss = h%10;
			s_time[20] = '0' + ss;

			speed = loc->speed;						// 速度
			if(speed<254){
				if(status & LOC_FLAG_SM){
					speed = speed * 0.75 + 255/4;
				}else{
					speed /= 4; 
				}
			}
			sprintf(s_speed,"%4dm/s",speed);

			alti = loc->Geodetic_Altitude;	        // 高度
			alti = alti/2-1000;
			sprintf(s_alti,"%7.1fm",alti);
			
			dir = loc->dir;							// 方向
			if(status & LOC_FLAG_EW){
				dir += 180;
			}
			sprintf(s_dir,"%3d°", dir);

			adr += msg_size;
			break;

		case MSG_TYPE_AUTH:							// 認証情報
			auth = (AUTH *)&data[0];
			tim = auth->timestamp_auth + tm2019;
			tim += 32400;							// 9*60*60 （秒で9時間進める。日本時間用）
			t = *localtime(&tim);
			sprintf(s_time,"%04d/%02d/%02d %02d:%02d:%02d.%1d", t.tm_year + 1900, t.tm_mon +1 , t.tm_mday, t.tm_hour, t.tm_min, t.tm_sec, ss);
			adr += msg_size;		
			break;

		default:
			adr += msg_size;
			break;			
	}
	return adr;
}

void decode_data(uint8_t *data, uint8_t size){	// ADVデータをデコードする
	uint8_t *adr, len, type;

	adr = data;
	while(adr < data+size){
		len = adr[0];
		if(len==0)break;

		type = adr[1];
		switch(type){
			case AD_TYPE_CMP_LOCAL_NAME:	// 名前データ
				memcpy(name, &adr[2], 28);
				break;

			case AD_TYPE_SHT_LOCAL_NAME:	// 短縮名データ
				memcpy(sat, &adr[2], 3);	// 衛星補足数
				break;

			case AD_TYPE_SERVICE_DATA:		// サービスデータ
				counter = adr[5];
				decode_msg(&adr[6]);		// メッセージをデコードする
				break;

			default:
				break;
		}
		adr += len+1;
	}
}

/**
* @brief extend adv report parameters
*/
//typedef struct {
//    esp_ble_gap_adv_type_t event_type;              /*!< extend advertising type */
//    uint8_t addr_type;                              /*!< extend advertising address type */
//    esp_bd_addr_t addr;                             /*!< extend advertising address */
//    esp_ble_gap_pri_phy_t primary_phy;              /*!< extend advertising primary phy */
//    esp_ble_gap_phy_t secondly_phy;                 /*!< extend advertising secondary phy */
//    uint8_t sid;                                    /*!< extend advertising sid */
//    uint8_t tx_power;                               /*!< extend advertising tx power */
//    int8_t rssi;                                    /*!< extend advertising rssi */
//    uint16_t per_adv_interval;                      /*!< periodic advertising interval */
//    uint8_t dir_addr_type;                          /*!< direct address type */
//    esp_bd_addr_t dir_addr;                         /*!< direct address */
//    esp_ble_gap_ext_adv_data_status_t data_status;  /*!< data type */
//    uint8_t adv_data_len;                           /*!< extend advertising data length */
//    uint8_t adv_data[251];                          /*!< extend advertising data */
//} esp_ble_gap_ext_adv_reprot_t;

// BLEがアドバタイジングデータを受信したとき呼ばれる
class MyBLEExtAdvertisingCallbacks: public BLEExtAdvertisingCallbacks {
	void onResult(esp_ble_gap_ext_adv_reprot_t report) {
		if(report.event_type & ESP_BLE_GAP_SET_EXT_ADV_PROP_LEGACY){
//      Serial.println("BLE4.2");
		}else{	// BLE 5.x の時
			esp_bd_addr_t &a = report.addr;			// BLE Address[6Bytes]
			sprintf(addr,"%02X:%02X:%02X:%02X:%02X:%02X",a[0],a[1],a[2],a[3],a[4],a[5]);
			rssi = report.rssi;
			primary_phy = report.primary_phy -1;
			secondary_phy = report.secondly_phy -1; 
			tx_power = report.tx_power;
			decode_data(report.adv_data, report.adv_data_len);
			setJson();
		}
	}
};

void webSocketEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t length) {
	switch(type) {
		case WStype_DISCONNECTED:
			break;
		case WStype_CONNECTED:
			break;
		case WStype_TEXT:
			break;
		case WStype_BIN:
			break;
		case WStype_ERROR:			
		case WStype_FRAGMENT_TEXT_START:
		case WStype_FRAGMENT_BIN_START:
		case WStype_FRAGMENT:
		case WStype_FRAGMENT_FIN:
			break;
	}
}

//----- GNSS( GPS ) 設定用 ---------------------------------------------
#define BAUDRATE	115200	// GNSS ボーレート
#define RATE		100		// GNSS 更新レート 100ms(10Hz)
#define RX_PIN 20
#define TX_PIN 21

#pragma pack(push,1)
typedef struct{
	uint8_t sync1 = 0xB5;
	uint8_t sync2 = 0x62;
	uint8_t cl = 0x06;
	uint8_t id = 0x00;
	uint16_t len = 20;	
	uint8_t port = 1;
	uint8_t rs = 0;
	uint16_t txReady = 0x0000;
	uint32_t mode = 0x000008D0;			// 8bit 1stopbit noPlity
	uint32_t baudrate = BAUDRATE;		// ボーレート
	uint16_t inProtoMaske = 0x0007;		// RTCM2,NMEA,UBX
	uint16_t outProtoMaske = 0x0003; 	// NMEA,UBX
	uint16_t flags = 0;
	uint16_t rs2 = 0;
	uint8_t ckA,ckB;
}UBX_CFG_PRT;

typedef struct {
	uint8_t sync1 = 0xB5;
	uint8_t sync2 = 0x62;
	uint8_t cl = 0x06;
	uint8_t id = 0x00;
	uint16_t len = 1;	
	uint8_t d = 1;
	uint8_t ckA = 0x08;
	uint8_t ckB = 0x22;
}UBX_CFG_PRT_POLL;

typedef struct{
	uint8_t sync1 = 0xB5;
	uint8_t sync2 = 0x62;
	uint8_t cl = 0x06;
	uint8_t id = 0x08;
	uint16_t len = 6;
	uint16_t measRate = RATE;	// 更新時間(ms)
	uint16_t navRate = 1;		// 比率
	uint16_t timeRef = 1;		// GPS time	
	uint8_t ckA,ckB;
}UBX_CFG_RATE;
#pragma pack(pop)

int set_cs(uint8_t *cmd){	// チェックサムを計算
	int i,n;
	uint16_t len;
	uint8_t ca=0;
	uint8_t cb=0;
	len = cmd[4]+cmd[5]*16;
	for(i=2; i<len+4+2; i++){
		ca = (ca + cmd[i]) & 0xFF;
		cb = (cb + ca) & 0xFF;
	}
 	cmd[i++]= ca;
 	cmd[i] = cb;
 	return len+8;	        // 全体の長さを返す
}

void send_UBX(uint8_t *buf){
	int len;
	len = set_cs(buf);		// チェックサムを計算
	for(int i=0; i<len; i++){
		Serial1.write(buf[i]);
	}
	Serial1.println("");
	delay(300);
}

UBX_CFG_PRT 		cfg_prt;
UBX_CFG_PRT_POLL	cfg_prt_poll;
UBX_CFG_RATE		cfg_rate;
const  uint32_t init_speed[5] = { 115200, 57600, 38400, 19200, 9600 };

void gnss_setup(){
	// GNSSのボーレートを設定する 
	for (int i = 0; i < 5; i++){	// 現在の速度が判らないので、可能性のある速度でコマンドを送信する。どれかで通じるはず。
		Serial1.begin(init_speed[i], SERIAL_8N1, RX_PIN, TX_PIN);
		while(!Serial1){}
		send_UBX((uint8_t *)&cfg_prt);						// ボーレート設定コマンド
		send_UBX((uint8_t *)&cfg_prt_poll);					// 確認用コマンド
		Serial1.end();
		delay(300);
	}
	Serial1.begin(BAUDRATE, SERIAL_8N1, RX_PIN, TX_PIN);
	while(!Serial1){}
	send_UBX((uint8_t *)&cfg_rate);							// 更新レートを10Hz(100ms間隔)に設定
}
//-------------------------------------------------------------------------------------------

void setup() {
//	Serial.begin(115200);
	gnss_setup();			// GNSSモジュールの初期設定

  if(!SPIFFS.begin(true)){  // SPIFFSのセットアップ
		return;
  }
  get_tm2019();             // 2019年までの秒数を計算しておく
	
  sprintf(addr,"%02X:%02X:%02X:%02X:%02X:%02X",0,0,0,0,0,0);

  BLEDevice::init("");
  pBLEScan = BLEDevice::getScan();
  pBLEScan->setExtendedScanCallback(new MyBLEExtAdvertisingCallbacks());	// BLE コールバックルーチンを指定
  pBLEScan->setExtScanParams();
  delay(1000);
  pBLEScan->startExtScan(scanTime, 1); // scan duration in n * 10ms, period - repeat after n seconds (period >= duration)

  WiFi.softAP(ssid,pass);
  WiFi.softAPConfig(ip,ip,subnet);

  server.on("/", HTTP_GET, [](AsyncWebServerRequest *request){
    request->send(SPIFFS, "/index.html");
  });
  server.on("/info.html", HTTP_GET, [](AsyncWebServerRequest *request){
    request->send(SPIFFS, "/info.html");
  });
  server.on("/map", HTTP_GET, [](AsyncWebServerRequest *request){
    request->send(SPIFFS, "/map.jpg", "image/jpg");
  });
  server.on("/map0", HTTP_GET, [](AsyncWebServerRequest *request){
    request->send(SPIFFS, "/map0.jpg", "image/jpg");
  });
  server.on("/map1", HTTP_GET, [](AsyncWebServerRequest *request){
    request->send(SPIFFS, "/map1.jpg", "image/jpg");
  });
  server.on("/map2", HTTP_GET, [](AsyncWebServerRequest *request){
    request->send(SPIFFS, "/map2.jpg", "image/jpg");
  });
  server.on("/mark", HTTP_GET, [](AsyncWebServerRequest *request){
    request->send(SPIFFS, "/mark.png", "image/png");
  });
  server.on("/opmark", HTTP_GET, [](AsyncWebServerRequest *request){
    request->send(SPIFFS, "/opmark.png", "image/png");
  });

  server.begin();
  webSocket.begin();
  webSocket.onEvent(webSocketEvent);
}

void loop() {
	webSocket.loop();

	while(Serial1.available()){				// UARTにGNSSのデータが来てたら全て読み込む
		if(gps.encode(Serial1.read())){
			gps_read();
		}
	}

	if(dataReady){
		webSocket.broadcastTXT(jsonTxt);	// WebSocketでデータ送信
		dataReady = false;
	}
}

以上です。

3
4
4

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
3
4

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?