プログラム仕様

----------------------------------------------------------------

Hotal　プログラム仕様

2022年1月10日から

例 https://drone-ai.blogspot.com/2022/03/blog-post_30.html

----------------------------------------------------------------

（１）プログラムはステートメント（命令文）の並びから構成される。ステートメントは１行に書かれ、複数の行にまたがることはできない。ステートメントの塊はブロックである。ブロックは、それを一つのプログラムとして実行できる。ただし、空行は無視される。１行の中に'#'があると、それも含めそれより右側の記述は無視される。インデントは、空白で行われることが前提である。

（２）実行可能ステートメントは1行から成り立ち、loop、if、マクロを除き、順に実行される。

1,代入文：変数 = 式（評価される、演算子と変数、値を返す関数から成立している）

2,if文

3,loop文

4,関数（実行される）

5.マクロ

のいずれかである。

（３）数値

数値はすべて倍精度数で把握される。小数は小数点を使って表される。浮動小数点は正しく処理されない。

（４）変数

変数は、$から始まる文字列でなければならない。$以降は、アルファベット、数字、'_'アンダーラインのいずれかでなければならない。文字列を変数に入れることはできない。値は全て倍精度である。

例. $abcdef,  $e123,   $abc_d123 など

変数は、最初に現れたところで定義される。同じものが再定義されると、値が無条件に上書きされる

（５）関数

関数は、'^'で始まり、名前、引数をつなげたものである。引数は '('と')'に挟まれる。実引数がない場合も'()'が必要である。関数名は変数名の規則に準じる。引数が複数ある場合は、カンマで区切る。関数は、値を返す機能と機体に関わる何らかの動作を実行するものとがある。また、その両方の場合もある。

（注１）　複数の値を返す関数（getPropellaRpmなど）は、その関数自体は値を返さない（ゼロが返される）が、実行後に、その関数名に '_0', '_1', '_2',・・・がついた一連の変数に値が保存される。次に実行されるときには、上書きされてしまう。持続的に使いたい場合は、それまでに別な名前の変数に代入、保存しておくなどの処理が必要。配列を返す関数が代入文の左辺にある場合、その値はゼロになってしまう。

（６）マクロ

マクロは、プログラムの中（位置はどこでも良い）で定義され

defmacro マクロ名

の行から始まり

endmacro

の行で終わっているブロックである。その部分は、メインの実行行には含まれず、無視される。マクロを呼び出すには、

@マクロ名

と書いた行をメインプログラムの中に置くことにより、その位置でマクロ全体が実行される。何度呼び出されてもよい。マクロの内容は通常の実行ブロックと同じである。変数も、名前空間はないので、同じ名前の変数は、メインと共通になる。変数を区別するためには、変数名の頭にマクロ名をつけるなどの工夫が必要になる。マクロの中で、再帰的にそのマクロを呼び出してもよいが、マクロの中で同じ、あるいは別のマクロを定義してはならない。マクロを正常に呼び出すことができなくなる。

（７）if文

if文は、ステートメントを空白で分解したときに最初にifが記載され、次に論理式が現れるものである論理式が真の場合は、対応するendifか対応するelseのいずれか最初に現れたところまでが実行される。論理式が偽の場合は対応するendifの前に、対応するelseがあれば、そのelseからendifまでが、実行される。elseがなければ、endif以降のブロックの実行に移る。

（８）論理式

論理式は ==, <=, >=, <, >, !=の演算子を持ち。両辺いずれも式として評価され、数値的値の比較が行われ、真偽が判定される。論理式は、現状では単独でしか評価できない。いずれ、論理式も評価できるようにしたい。

（９）loop文

loop文は、ステートメントを空白で分解したときに、最初の項がloopであるもの。続いて、論理式が与えられる。論理式が真である限り、対応するendloopのステートメントまでが繰り返される。論理式が偽になったばあいは、endloopの次のステートメントからの実行に移る。

（１０）今の段階で、実行、評価が可能な関数（今後さらに拡張される）

^throttle():

    スロットルレベルを指定する。引数にスロットルレベル（%）を与える。0が最小。100は、ESCのキャリブレーションで、MAXで設定された電圧レベルになる。

^sleep():

    実行を引数秒だけ中断する。秒は実数（小数可）で与える。

^pidbase():

    PIDの基底値を与える（実数）。３つの値をそれぞれ引数とする

^pidscale():

    PIDのスケール値を与える（整数）。スケールは、50がちょうど、PIDの規定値と同じになるように調整される。３つの値をそれぞれ引数とする。内部的には倍精度で処理されるが、実際の速度調整時にはまた整数化されて処理。PIDスケールは、configファイルで、一定高度にならないと実行されない設定もあるので注意。

^motorSpeedAjustment():

    モータースピードの修正値を与える。引数に四つのモーターの修正値を与える。

^getHight():

    その瞬間の高度を整数で取得する。超音波距離センサー HCSR04を使っている

^getAverageHight():

    ４ループ分の平均高度を得る　HCSR04を使っている

^print():

    引数の内容を標準出力する。引数は、文字列または変数を＋でつなげることができる。変数でなければ、文字列とみなしそのまま出力する。変数は、単独か、+ で区切られていなければならない。

^getTakeoffAltitude(): 

    60センチまでの高度をより正確に取るために使う。倍精度で値を返す。光距離センサーGP2Y0E03を使っている。

^getPropellaRpm(): 

    現在のプロペラ回転数を取得する。変数、getPropellaRpm_0. getPropellaRpm_1. getPropellaRpm_2. getPropellaRpm_3 に自動的に代入される。 

プログラムモードとモーター回転数バランスの自動調整

 ドローンの離陸をマニュアルで試みるのは、なかなか困難だ。私が不器用なのか、歳をとって手の運動神経が老化しているのもあるだろう。そこで、離陸はプログラムで制御するようにしている。

https://youtu.be/RCNzAaKuwZU

これは、私の部屋にちょっとした離陸ベースを置いてプログラムで飛ばしたものだ。コマンドは、１行ずつ実行する。途中に、最後で定義されているマクロが挟まれる。$で始まる名前は、変数だ。^で始まるものは関数。@で始まるものがマクロだ。その他、細かいプログラミングルールは、こちらに書いておいた。

離陸直前のスロットルの状態をすこい維持して、その間にプロペラの回転バランスをとるように、モータースピードを調整するマクロ（@setspeedadjustment）が挟んである。

#-------------------------
# pidscale ver.0 2022年2月28日
# プログラムオリジナル作成
# pidscale ver.1 2022年3月20日
# ホバリング高度に到達した時点でPID制御に入る
# pidscale ver.1 2022年3月27日
# モータースピードの調整を自動化するマクロ追加
#
# Attitudeの姿勢制御ループに対するコマンドは、
# 繰り返しの時間を念頭に置かないとコマンドが有効にならない
#-------------------------
#基本定数
# テスト用
#$takeoffthrottle = 30
#$hoveringthrottle_max = 30;
#$hoveringthrottle_min = 30;
# 本番用
$takeoffthrottle = 39
$hoveringthrottle_max = 43;
$hoveringthrottle_min = 37;
#
^motor(on)
# motor on の有効化を0.1秒待つ：必須
^sleep(0.1)
# Arming
^throttle(20)
# sleep の引数は実数値可
^sleep(1.5)
^throttle(30)
# 回転数が上がる時間を稼ぐ
^sleep(0.7)
# モータースピード調整マクロの実行
# このスピードで、1秒使われる
@setspeedadjustment
# 調整に馴染ませる時間をとる
^sleep(0.5)
# 離陸
^print(離陸開始します)
^throttle($takeoffthrottle)
##############################
# 離陸高度チェックループ 開始 
# ループを正しく抜けれるかどうか事前テストすること
##############################
$count = 0
$flag = -1
loop $flag < 0
    $altitude = ^getTakeoffAltitude()
    ^print(離陸：No.+$count+ 現在高度 [ +$altitude+ ])
    # 高度が指定の高さ以上になるまで空回りする
    if $altitude > 30
        ^print(離陸：指定高度の30cmを超えたのでループを抜けます)
        $flag = 1
    endif
    # 10msec 停止する
    ^sleep(0.01)
    $count = $count + 1;
    # 指定高度に0.5秒で到達しなかったら強制的にループを抜ける
    if $count > 50
        ^print(離陸：規定チェック回数を超えたのでループを抜けます)
        $flag = 1
    endif
endloop
##############################
# 離陸高度チェックループ 終了 
##############################
^print(PID制御を開始します)
# PID制御に入る
# 最高高度に入る直前だと思われる
^pidscale(50,20,50)
^throttle($hoveringthrottle_max)
^sleep(0.7)
^throttle($hoveringthrottle_min)
##############################
# ホバリングループ 開始 
# $hoveringthrottle_min では、下降してしまうので
# スロットルを調整する
##############################
# 変数再初期化
$count = 0
$flag = -1
loop $flag < 0
    # 60cm 以上の高度を想定していないので、こちらの高度センサーを使う
    $altitude = ^getTakeoffAltitude()
    ^print(ホバリング：No.+$count+ 現在高度 [ +$altitude+ ])
    # 高度が下がったら再上昇を試みる
    if $altitude < 45
        ^print(ホバリング：高度が45cm以下になったのでスロットルを上昇させます)
        ^throttle($hoveringthrottle_max)
    endif
    if $altitude > 55
        ^print(ホバリング：高度が55cm以上になったのでスロットルを低下させます)
        ^throttle($hoveringthrottle_min)
    endif
    # 10msec 停止する
    ^sleep(0.01)
    $count = $count + 1;
    # ホバリングを指定時間試みたらループを抜ける
    if $count > 100
        ^print(ホバリング：1秒経過したのでループを抜けます)
        $flag = 1
    endif
endloop
##############################
# ホバリングループ 終了
##############################
# ^sleep(0.5)
#########################
# 着陸に入る
# スロットルを少しずつ下げる
#########################
^print(着陸します)
^throttle($hoveringthrottle_min)
^sleep(0.3)
^throttle(30)
^sleep(0.4)
^throttle(25)
^sleep(0.4)
# throttleの有効化の前にmotor off にならないように待つ。0.1秒以上
# また、モーターが完全に停止するまでのログを取るという意味もある。それで3秒にしている
^print(モーターを停止させます)
^throttle(0)
^sleep(2)
^motor(off)
# メインプログラム終了

defmacro setspeedadjustment
#################
# スピード調整設定マクロ
# スロットル関係の関数は使っていないので、
# ここに入る前にスロットルは調整すべき
 #################
^print(スピード調整設定マクロを実行します)
# 調整スピードをゼロに設定：初期化
^motorSpeedAjustment(0.0,0.0,0.0,0.0)
# 各プロペラ回転数の合計
$psum_0 = 0
$psum_1 = 0
$psum_2 = 0
$psum_3 = 0
^print(調整スピードをゼロに設定：初期化 ループの開始)
# 誤差取得のループ
$flag = -1
$count = 0
loop $flag < 0
    #プロペラ回転数を取得する
    ^getPropellaRpm()
    ^print(No.+$count+ 回転数 + $getPropellaRpm_0 +,+ $getPropellaRpm_1 +,+ $getPropellaRpm_2 +,+ $getPropellaRpm_3)
    # 取得した回転数を加える
    $psum_0 = $psum_0 + $getPropellaRpm_0
    $psum_1 = $psum_1 + $getPropellaRpm_1
    $psum_2 = $psum_2 + $getPropellaRpm_2
    $psum_3 = $psum_3 + $getPropellaRpm_3
    $count = $count + 1
    if $count >= 100
        # 1秒測る
        $paverage_0 = $psum_0 / $count
        $paverage_1 = $psum_1 / $count
        $paverage_2 = $psum_2 / $count
        $paverage_3 = $psum_3 / $count
        ^print(1秒経過で測定終了 平均： + $paverage_0 +,+ $paverage_1 +,+ $paverage_2 +,+ $paverage_3)
        # 終了フラグを立てる
        $flag = 1
    endif
    ^sleep(0.01)
endloop
# ４つのモーターについてさらに平均を求める
$totalaverage = ($paverage_0 + $paverage_1 + $paverage_2 + $paverage_3)/4
^print(全体平均回転数 + $totalaverage + rpm)
# 平均との差から調整値を求める
# 調整係数
$adjusttingdeflator = 200
$adjust_0 = ($paverage_0 - $totalaverage)/$adjusttingdeflator
$adjust_1 = ($paverage_1 - $totalaverage)/$adjusttingdeflator
$adjust_2 = ($paverage_2 - $totalaverage)/$adjusttingdeflator
$adjust_3 = ($paverage_3 - $totalaverage)/$adjusttingdeflator
# 調整値をセットする
^print(モータースピード調整値： + $adjust_0 +,+ $adjust_1 +,+ $adjust_2 +,+ $adjust_3)
^motorSpeedAjustment($adjust_0,$adjust_1,$adjust_2,$adjust_3)
# マクロの終了
^print(スピード調整設定マクロを終了します)
endmacro

剛体のシミュレーションとフライトコントローラーのパラメータをリンクさせる

先の 剛体としてののドローンのシミュレーションでは、無駄時間を考慮してもPD制御で安定化が図れることがわかった。

新たな機体が組み上がったので、そのフライトコントローラーのシステム（以下Hotalとよぶ）にシミュレーションのパラメーターを適応させることにした。

Hotalでは、加速度センサーから取られた値を相補フィルターにかけて、ラジアンで機体角度を出してきている。その値を100倍して（スロットルの%をイメージしている）PあるいはD制御のパラメーターをかけて、PWMモジュール（PCA9685）にその値を送り、ESCに回転速度要求信号として送り込んでいる。

PWMモジュールに送った値とモーターの回転数との関係がわからなければ、この関係をとらえられないので最新のフライトのログをもとに回帰分析をした。

飛行を開始した時点から終了までの、モーター１だけを取り出してまず相関関係をみた。

まあ、ばらついているが正の相関を確認できる。

回帰分析した結果は、

これをもとに計算する。シミュレーションの300に対するHotalのPパラメーターを$H_{p}$とすると、

$$100H_{p}\times 117.48=300$$

よって、

$$H_{p}=0.0255$$

となるDパラメータも同じである。

これは直感的に、いいところをついたパラメータとなっている。このパラメータの前後で実機のフライトコントローラーのPD制御パラメータを探れば良い。

raspberrypi 起動時にIPアドレスをAQM0802に表示する

 基本、wifiでドローンと繋げているので、以前は固定IPをラズパイに設定してつないでいた。しかし、ラズパイの固定IP設定はトラブルが多いので、やめた。ルーターDHCPからIPアドレスをとるとある程度固定していて、たまに変わっても１増やすくらいなので、それでもいいのだが、やはり面倒。

そこで、DHCPで取った IPアドレスを起動時にLCD、キャラクタディスプレイに表示させるようにした。

LCDは、AQM0802という定番のものを使った。基盤付きのやつがいい。最初間違って、Arudinoのようのものを買ったが、プルアップ電圧が違うようで、I2Cアドレスの認識すらできなかった。ラズパイ用にセットアップされたものが820円で秋月電子で売っている。

https://akizukidenshi.com/catalog/g/gK-11354/

これを使った。自分でプログラムを書かなければと思っていたが、全く同じことをやっている方がおられた。

https://qiita.com/JHiyama/items/158ab35ed0247a7fc406

助かった。

なお、無線LANのみの場合はほぼそのままでいいが、同時にケーブルを繋いでいる場合は、f2にしなければ正しく取れない。無線LANのIPアドレスは２番目に書かれているので。