スポンサーリンク

RaspberryPi

iPhoneでドローン(SPRacingF3)を操縦する方法

以前の記事では、

ドローンに積んだフライトコントローラーSP Racing F3のセンサーデータを
RaspberryPiからインターネット経由、MQTTプロトコルで送信。
ブラウザでリアルタイムに計器表示。ということを行いました。

今回は、
iPhoneからインターネット経由でドローン(SP Racing F3)をコントロールする
ということに挑戦してみたいと思います。

考察

SP Racing F3の制御方法は?

一般的に、ドローンはプロポと呼ばれる制御装置で
コントロールします。

プロポで主要なチャネルは
スロットル、エルロン、エレベーター、ラダーの4軸で、
ドローンの上下左右、前後、回転の運動に直結します。

SP Racing F3は、
それぞれのチャネルの入力値を最小1000〜最大2000
で識別します。
※厳密には、設定で、最小値、最大値は変更できます。

SP Racing F3をコントロールするのは
このプロポを使う方法以外にも手段があり、
その一つが、MSPプロトコルを使ってRaspberryPiから操作する方法です。

MSPプロトコルを使う場合、ラズパイがプロポの代わりに
各チャネルの信号値(1000〜2000)を送らなければいけないので、
適切な信号値を定期的に送れば、ドローンは飛行できるのです。

Raspberry Piへの情報伝達はどうするか?

次に問題になってくるのが、
RaspberryPiはどのように最適な入力信号の値(1000〜2000)
を生成するか?です。

例えば、センサーデータを監視して機体の状態を把握し、
状態にあった最適なプロポ信号の値を計算できるロジックを組めば、
自動操縦が実現するのですが、、

そのような高度な計算をするロジックを実装するのは
長い時間がかかりそうなので、、

今回は、インターネット経由でiPhoneから信号値を受け取り、
値をMSPプロトコルに変換してデータを送る
ということをやってみます。

また、インターネット経由で通信する場合、なるべくリアルタイムに、
レイテンシーが少ない方法が理想なので、前回同様、通信手段は
オーバーヘッドが少ないMQTTプロトコルを採用したいと思います。

iPhoneでどうやって各チャネルの値を送らせられるか?

次の課題は、
どうやってiPhoneで信号値を生成するか?
です。

各チャネルの信号値を、例えばテキストフォームに入力する方法では
とても時間がかかり、面倒ですし、ドローンを飛ばすということに対し
現実的ではないですね。

そこで、今回は
iphone傾きセンサーを取得し、各chの信号値に変換。
ということをしています。

センサーの値取得も、MQTTで送信も、全て
クライアントサイドのスクリプト(Javascript)
で完結できます。

実装

※こちらの記事で紹介する手順は、実際にモーターが回るため、プロペラが付いた状態で行うと、誤作動を起こす可能性があり非常に危険です。実践される場合は、自己責任の元、プロペラを外し、細心の注意を払った上で行って下さい。

SP Racing F3の設定

Cleanflightの設定で、
Arm/Disarm設定をAUX1に割り当てます。

scr0817215305

MQTTの準備

MQTTブローカーを用意します。
一番簡単な方法の一つが、HerokuのCloudMQTTを利用する方法です。
詳細は、以前の記事中に記載していますので参照ください。

また、作成したユーザ名に対し、トピック”drone/control/rc”
を利用できるよう設定します。

Raspberry Piの設定

Raspberry Piをドローンに積み、
USBでSP Racing F3と接続します。

そして、Node.jsでプログラム”control_spracing.js”を書きます。

※Heroku CloudMQTTとの通信の箇所は、CloudMQTT上の固有のアクセスエンドポイント、ポート番号、ユーザ名、パスワードを入力してください。

var mqtt = require('mqtt');
//各chの初期値                                                                            
var rc1 = 1500;
var rc2 = 1500;
var rc3 = 1000;
var rc4 = 1500;
var rc5 = 1000;
var rc6 = 1000;
var rc7 = 1000;
var rc8 = 1000;
//Heroku CloudMQTTとの通信                                                                
var url = 'mqtt://***********';
var options = {
    port: *****,
    clientId: 'mqttjs_' + Math.random().toString(16).substr(2, 8),
    username: '*********',
    password: '*********'
};
var client = mqtt.connect(url, options);
//MQTTサブスクライブ                                                                      
client.on('connect', function() {
    client.subscribe('drone/control/rc');
});
//MQTTメッセージ受信時処理                                                                
client.on('message', function(top, mes){
    var rcs = JSON.parse(mes.toString());
    if ('rc1' in rcs){
        rc1 = rcs.rc1;
    }
    if ('rc2' in rcs){
        rc2 = rcs.rc2;
    }
    if ('rc3' in rcs){
        rc3 = rcs.rc3;
    }
    if ('rc4' in rcs){
        rc4 = rcs.rc4;
    }
    if ('rc5' in rcs){
        rc5 = rcs.rc5;
    }
    if ('rc6' in rcs){
        rc6 = rcs.rc6;
    }
    if ('rc7' in rcs){
        rc7 = rcs.rc7;
    }
    if ('rc8' in rcs){
        rc8 = rcs.rc8;
    }
})
//USBシリアルポート                                                                       
var serial = require('serialport');
serial = new serial('/dev/ttyUSB0',{baudrate:115200});
//接続後、50ms間隔でSP Racing F3へRC値送信                                                
serial.on('open', function(){
    console.log('open /dev/ttyUSB0');
    serial.on('data',function(data){
        myreadResponse(data);
    });
    setInterval(function(){
        sendRequestRc();
    }, 50);
});
serial.on('error', function(err){
    console.log('error : ' + err);
});
//SP Racing F3へのリクエスト処理                                                          
function sendRequestRc(){
    var seg1 = 0x24;
    var seg2 = 0x4D;
    var seg3 = 0x3C;
    var seg4 = 16;
    var seg5 = 200;
    var buf = new Buffer(22);
    buf.fill(seg1, 0);
    buf.fill(seg2, 1);
    buf.fill(seg3, 2);
    buf.writeUInt8(seg4, 3);
    buf.writeUInt8(seg5, 4);
    buf.writeUInt16LE(rc1, 5);
    buf.writeUInt16LE(rc2, 7);
    buf.writeUInt16LE(rc3, 9);
    buf.writeUInt16LE(rc4, 11);
    buf.writeUInt16LE(rc5, 13);
    buf.writeUInt16LE(rc6, 15);
    buf.writeUInt16LE(rc7, 17);
    buf.writeUInt16LE(rc8, 19);
    var crc = buf.readInt8(3);
    for(var i=4; i<21; i++){
        crc ^= buf.readInt8(i);
    }
    buf.writeInt8(crc, 21);
    serial.write(buf, function(err, result){});
}
//SP Racing F3からのレスポンス処理                                                        
function myreadResponse(data){
    var buf = new Buffer(data, 'binary');
    console.log(buf);
}
iPhone操作用Webページの作成

iPhoneで操作するためのWebページを作成します。
今回は、iPhoneを傾けて操作しますが、プロポと同様に
iPhone2台で操作する想定で2ページ分作成します。

左手用
lefthand.html


<html>
<head>
	<meta charset="utf-8">
	<meta http-equiv="X-UA-Compatible" content="IE=edge">
	<meta name="viewport" content="width=device-width, initial-scale=1, maximum-scale=1, user-scalable=no">
	<!-- The above 3 meta tags *must* come first in the head; any other head content must come *after* these tags -->
	<title>Control</title>
        <!-- Bootstrap -->
	<link rel="stylesheet" href="https://maxcdn.bootstrapcdn.com/bootstrap/3.3.7/css/bootstrap.min.css" integrity="sha384-BVYiiSIFeK1dGmJRAkycuHAHRg32OmUcww7on3RYdg4Va+PmSTsz/K68vbdEjh4u" crossorigin="anonymous">

<style>	
	.bgblue {
		background-color: #03A9F4;
	}
        .bgyellow {
		background-color: #FFEB3B;
	}
	</style>

        <script src="https://ajax.googleapis.com/ajax/libs/jquery/2.1.0/jquery.min.js"></script>
        <script src="https://cdnjs.cloudflare.com/ajax/libs/paho-mqtt/1.0.1/mqttws31.min.js" type="text/javascript"></script>
        <script>
        //MQTT接続
        var client = new Paho.MQTT.Client("***エンドポイント***", **ポート番号**, "idm" + Math.floor( Math.random() * 11 ));
        client.onConnectionLost = onConnectionLost;
        client.onMessageArrived = onMessageArrived;
        var options = {
            useSSL: true,
            userName: '***ユーザ名***',
            password: '***パスワード***',
            onSuccess: onConnect
        };
        client.connect(options);
                function onConnect() {
            
        }
        //MQTT接続が失敗したとき
        function onConnectionLost(responseObject) {
            
        }
        //MQTTメッセージを受信したとき
        function onMessageArrived(message) {
           
        }
        var active = false;
        var ch1 = 0;
        var orgch1 = 0;
        var ch5 = 1000;
        var orgch5 = 1000;
	window.addEventListener("deviceorientation", function(event){
		var beta = event.beta;
                var throttle = normalizeThrottle(beta);
                $("#beta").val(throttle);
                ch1 = throttle;
        },true);

        function normalizeThrottle(beta) {
                var diff = Math.floor(beta * 10);
                if(diff < 0){ diff = 0; } else if (diff > 1000){
	                diff = 1000;
                }
                var ret = 1000 + diff;
                return ret;
        }

        $(function(){
	        setInterval(function(){
			if (active == true){
	                       if (ch1 != orgch1 || ch5 != orgch5){
		                      sendRcs();
	                       }
                        }
                }, 200);
        });

        function sendRcs(){
                var mes = '{"rc3":' + ch1 + ',"rc5":' + ch5 + '}';
                var message = new Paho.MQTT.Message(mes);
                message.destinationName = "drone/control/rc";
                client.send(message);
                orgch1 = ch1;
                orgch5 = ch5;
        }
        function arm(){
                if(ch5 > 1500){
	                ch5 = 1100;
	                tdisarm();
                } else {
	                ch5 = 1900;
	                tarm();
                }
                $("#sw1").val(ch5);
        }

        function tarm(){
	        $('#mybody').addClass('bgyellow');
	        $('#armbtn').text('Disarm');
        }
        function tdisarm(){
	        $('#mybody').removeClass('bgyellow');
	        $('#armbtn').text('Arm');
        }

        function activate(){
	        if (active == true){
		        active = false;
		        $("#mybody").addClass("bgblue");
	        } else {
		        active = true;
		        $("#mybody").removeClass("bgblue");
	        }
        }
</script>

</head>
<body id="mybody" class="bgblue">

<div class="container" style="margin-top: 20px;">

<div class="col-sm-12">
                         <button onclick="activate()" class="btn btn-default btn-block btn-lg">Active</button>

<form style="margin-top: 40px;">

<div class="form-group">
                                           <label>スロットル</label>
                                           <input type="text" class="form-control" id="beta">
                                  </div>


<div class="form-group">
                                           <label>Switch1</label>
                                           <input type="text" class="form-control" id="sw1">
                                  </div>

                         </form>


<h3 id="beta"></h3>

                         






                         <button onclick="arm();" class="btn btn-danger btn-block btn-lg" id="armbtn">Arm</button>
                </div>

        </div>

</body>
</html>

そして、右手用
righthand.html


<html>
<head>
	<meta charset="utf-8">
	<meta http-equiv="X-UA-Compatible" content="IE=edge">
	<meta name="viewport" content="width=device-width, initial-scale=1, maximum-scale=1, user-scalable=no">
	<!-- The above 3 meta tags *must* come first in the head; any other head content must come *after* these tags -->
	<title>Control2</title>
	<!-- Bootstrap -->
	<link rel="stylesheet" href="https://maxcdn.bootstrapcdn.com/bootstrap/3.3.7/css/bootstrap.min.css" integrity="sha384-BVYiiSIFeK1dGmJRAkycuHAHRg32OmUcww7on3RYdg4Va+PmSTsz/K68vbdEjh4u" crossorigin="anonymous">

<style>
	.bgblue {
		background-color: #03A9F4;
	}
	.btn {
		margin-bottom: 20px;
	}
	</style>

	<script src="https://ajax.googleapis.com/ajax/libs/jquery/2.1.0/jquery.min.js"></script>
	<script src="https://cdnjs.cloudflare.com/ajax/libs/paho-mqtt/1.0.1/mqttws31.min.js" type="text/javascript"></script>
	<script>
        //MQTT接続
        var client = new Paho.MQTT.Client("***エンドポイント***", **ポート番号**, "idm" + Math.floor( Math.random() * 11 ));
        client.onConnectionLost = onConnectionLost;
        client.onMessageArrived = onMessageArrived;
        var options = {
            useSSL: true,
            userName: '***ユーザ名***',
            password: '***パスワード***',
            onSuccess: onConnect
        };
        client.connect(options);
        function onConnect() {

        }
        //MQTT接続が失敗したとき
        function onConnectionLost(responseObject) {

        }
        //MQTTメッセージを受信したとき
        function onMessageArrived(message) {

        }
        var mm = 10;
        var active = false;
        var ch1 = 0;
        var ch2 = 0;
        var ch3 = 0;
        var orgch1 = 0;
        var orgch2 = 0;
        var orgch3 = 0;

        window.addEventListener("deviceorientation", function(event){
	        var beta = event.beta;
                var throttle = normalizePitch(beta);
	        var gamma = normalizeRoll(event.gamma);
                var alpha = normalizeHeading(event.alpha);
        	$("#id1").val(throttle);
        	$("#id2").val(gamma);
	        $("#id3").val(alpha);
        	ch1 = throttle;
	        ch2 = gamma;
	        ch3 = alpha;
        },true);

        function normalizePitch(beta) {
	        var hugou;
	        if (beta > 0){
		       hugou = -1;
	        } else {
		       hugou = 1;
	        }
	        beta = Math.abs(beta);
          	var ret;
	        if (beta > mm){
		       var vsa = 500 / (90 - mm);
		       var diff = vsa * (beta - mm);
		       ret = Math.floor(1500 + hugou * diff);
        	} else {
	               ret = 1500;
	        }
		return ret;
        }

        function normalizeRoll(beta) {
	        var hugou;
	        if (beta > 0){
		       hugou = 1;
	        } else {
		       hugou = -1;
	        }
	        beta = Math.abs(beta);
        	var ret;
	        if (beta > mm){
		       var vsa = 500 / (90 - mm);
		       var diff = vsa * (beta - mm);
		       ret = Math.floor(1500 + hugou * diff);
         	} else {
	               ret = 1500;
	        }
        	if (ret > 2000){
		       ret = 2000;
	        } else if(ret < 1000){
		       ret = 1000;
	        }
        	return ret;
        }

        function normalizeHeading(alpha){
	        return alpha;
        }

        $(function(){
	        setInterval(function(){
		        if (active == true){
			        if (ch1 != orgch1 || ch2 != orgch2){
				        sendRcs();
			        }
		        }
	        }, 200);
        });

        function sendRcs(){
	        var mes = '{"rc1":' + ch2 + ',"rc2":' + ch1 + '}';
        	var message = new Paho.MQTT.Message(mes);
            	message.destinationName = "drone/control/rc";
        	client.send(message);
	        orgch1 = ch1;
	        orgch2 = ch2;
        }

        function activate(){
	        if (active == true){
		        active = false;
		        $("#mybody").addClass("bgblue");
	        } else {
		        active = true;
		        $("#mybody").removeClass("bgblue");
	        }
        }

        function changead(){
                var vv = $('#myse').val();
                mm = vv;
        }
</script>
</head>
<body id="mybody" class="bgblue">

<div class="container" style="margin-top: 20px;">
         <button onclick="activate()" class="btn btn-default btn-block btn-lg">Active</button>

<form style="margin-top: 40px;">

<div class="form-group">
    <label>エレベーター</label>
    <input type="text" class="form-control" id="id1">
  </div>


<div class="form-group">
    <label>エルロン</label>
    <input type="text" class="form-control" id="id2">
  </div>


<div class="form-group">
    <label>Heading</label>
    <input type="text" class="form-control" id="id3">
  </div>


<div class="form-group">
  	<label>角度誤差</label>
	<select class="form-control" onchange="changead();" id="myse">
<option value='1'>1</option>
<option value='2'>2</option>
<option value='3'>3</option>
<option value='4'>4</option>
<option value='5'>5</option>
<option value='6'>6</option>
<option value='7'>7</option>
<option value='8'>8</option>
<option value='9'>9</option>
<option value='10'>10</option>
	</select>
    </div>

</form>

</div>

</body>
</html>

動作確認

ドローンのバッテリーをつなぎ、RaspberryPi上のプログラム”control_spracing.js”を起動します。

作成したWebページ2つに、左手用iPhone、右手用iPhoneからアクセスします。

Arm / スロットル

まずは、左手用。
iPhoneを下に傾けた状態が、スロットル最小値1000の状態です。
ページを開いた初期の状態では、MQTTで送信されません。

IMG_1176

Activeをタップすると、背景が白くなります。
これで、現在の状態はリアルタイムにMQTTブローカーに送信されている状態です。
=SP Racing F3 まで、iPhone上の信号が常に送り続けている状態になります。

IMG_1177

そして、Armします。
Armすると、モーターが回る状態になります!
背景は黄色くなります。

IMG_1178

少しずつiPhoneを傾けてみると、
若干の反応の遅れはありましたが、、
モーターが回りました!

エレベーター / エルロン

続いて、右手用righthand.htmlです。

同様に、右手用iPhoneからWebページ”righthand.html”にアクセスします。

IMG_1179

左手用同様に、Activeをタップすると、MQTT送信が有効になります。

IMG_1180

ちなみに、角度誤差を変更すると、
水平位置の角度誤差の幅を調整できます。
IMG_1181

室内なので、試験飛行は抑えますが、
今回の検証で、iPhoneの傾きでドローンの操縦ができることが
実証できました。

今後、外で飛ばす機会に
iPhoneドローンのレポートもしてみたいと思ってます。

ピックアップ記事

  1. プログラミングができる市販ドローン”Parrot Bebop2…
  2. DJIとIntelの最新技術の結晶 80gの小型ドローンTello
  3. フライトコントローラーSP Racing F3をRaspberryPiで操作
  4. MQTTでドローンの自動操縦を考える(1)
  5. 210サイズドローンの制作

関連記事

  1. フライトコントローラー

    フライトコントローラー(SP Racing F3)にGPSモジュール(ublox neo-6m)追加…

    今回は、前からやりたかったGPSモジュールの追加です。GPSで位…

  2. RaspberryPi

    MQTTでドローンの自動操縦を考える(1)

    ドローンの自動操縦への一歩として、前回、を紹介しましたが、今回…

  3. RaspberryPi

    フライトコントローラーSP Racing F3をRaspberryPiで操作

    フライトコントローラーSP Racing F3をプロポではなく、Ra…

  4. Webプログラミング

    Parrot Bebop2のカメラ映像をリアルタイムにYouTube配信する方法

    Node.jsを使い、Parrot Bebop2をどこまでコントロール…

  5. RaspberryPi

    ラズベリーパイはじめました

    秋葉原の千石電器に行き、前から気になっていたRaspberryPi3…

  6. フライトコントローラー

    新世代フライトコントローラーSP Racing F3

    今まで何も問題なく大活躍中のCC3Dですが、これからのフライトコント…

おすすめ記事

最近の記事

  1. トイドローンTelloでGo言語プログラミング!フロントカメ…
  2. DJIとIntelの最新技術の結晶 80gの小型ドローンTe…
  3. Parrot Bebop2のカメラ映像をリアルタイムにYou…
  4. Parrot bebop2をNode.jsで操作してみた
  5. プログラミングができる市販ドローン”Parrot…

月別アーカイブ

  1. Webプログラミング

    Parrot Bebop2のカメラ映像をリアルタイムにYouTube配信する方法…
  2. ドローン練習

    ドローンシミュレーター”Liftoff”でレベルアップを…
  3. ドローンのパーツ

    かっこいいアンテナホルダーを自作する方法
  4. 市販ドローン

    Parrot bebop2をNode.jsで操作してみた
  5. RaspberryPi

    iPhoneでドローン(SPRacingF3)を操縦する方法
PAGE TOP