XXControlを読んでみた

まぁ、読んだ中でいくつかの情報の抜粋ですけど、メモとして残しておきます。
ちなみに、私のはクアッドコプターに分類されるので、それに関する情報のみ。

最後の3行に全部詰まってますんで、必要な人はそこだけ読めば良いかも?

入力は受信機からのサーボコントロール信号
ピン変化割り込み(PCINT0,2)、外部割り込み(INT0,1)で4chのON時間を、
タイマー1を使って測定してる。タイマー1は1MHz動作なので、

RxChannelxにOn時間がマイクロ秒単位で入ることになる。

RxGetChannnels関数において、
RxChannelxはRxChannelxZeroOffsetを引いた上で、
RxInRoll,RxInPitch,RxInYawが4で割られた値
RxInCollectiveに8で割られた値が入る

setup関数においては、ゲインを一定以下にすることで下記設定には入れる
・プロポセンター位置記録
Collective以外がRxChannelxZeroOffsetにRxGetChannnels4回の加算で入る
・ジャイロの方向決め
xxxGyroDirectionに正、逆が入る
以上2項目は設定後EEPROMに保存
・スロットルキャリブレーション
全てのアンプにスロットルと同じ信号を出力する=アンプの出力バランス調整用

loop関数がメインのループ
スロットルが規定値以下の状態が0.5秒間続くと、動作のオン/オフを切り替えるとともに、
オンにしたときにはジャイロをキャリブレートして
ノーマル、アクロ、UFOモード切替をしてモードごとに
StickxxxGain = NORMAL_STICK_XXX_GAIN
をする。ノーマルでは全て50,UFOではヨーだけ90アクロは70

ジャイロのキャリブレーションは32回データーを取得して32で割ることで、
gyroZero[]に変化が無いときの値の平均値を保存する。

ここからはMotorOut1のみに着目するこのモーターは十字の十二時方向を担当する。

ここから先は、受信機からのパルスにおけるOn時間の変動を1msから2msとして、
制御の割もを算出していく

まず、MotorOut1にRxInCollectiveが入る。スロットルのオフセットは1120で決めうちなので、
つまりは
MotorOut1 = (On時間[us]- 1120[us])/8;
と言うことなので、変動幅1000usの内スロットルが占められる割合は110us分つまり11%
あれ、少ない?(最後まで読んでね!)

ロールの処理が来るがこの処理はMotorOut2,3が担当し1には関係が無いので割愛

ピッチの処理であるピッチはMoterOut1,4が担当する
gyroADCにはジャイロの計算結果が入ってる。ADコンバータの生データー(0-1024)
GainInにはゲインのボリューム調整値が入ってる。こちらはADコンの取得値を、
1024から引いて10で割っている。これにマルチコプターの定数ゲイン3を掛けて
gyroADC = gyroADC * GainIn * 3;
これを今度は定数200で割っている
gyroADC /= 200;
これがピッチジャイロの補正値を担当する。思ったよりゲインボリュームの変動幅が大きい。
これは、実際に飛ばしている機体のゲインが知りたいので計算は置いておく。
ちなみにこいつは半分マイナスにも振れる事をお忘れ無く。

次に受信機入力のRxInPitch分を計算している。
RxInPitch= RxInPitch * StickPitchGain / 100;
StickPitchGain はノーマルにおいて50で、RxInPitch は On時間を4で割った値なので、
ノーマルモードにおいて、これはOn時間を8で割った値に相当する。
と言うことなので、変動幅1000usの内ピッチが占められる割合は125us分つまり12.5%
うん。少ない。ちなみにこいつも半分マイナスにも振れる事をお忘れ無く。
(最後まで読んでね!)

で最後に
RxInPitch += gyroADC をして、このRxInPitch を
MotorOut1 += RxInPitch;
MotorOut4 -= RxInPitch;
する。

そして3軸最後のヨーの処理。時計回りに回ってるローターと
反時計回りに回っているローターの出力比を変えることでヨーをコントロールしている。
でまぁ、計算見るとピッチと一緒なんだな。これが、で最後の処理は。
MotorOut1 -= RxInYaw;
MotorOut2 += RxInYaw;
MotorOut3 += RxInYaw;
MotorOut4 -= RxInYaw;

うーん。で、最小値の処理をして、モーターに出力!

そして、最後のモーターのOn時間はなんと
10msに一回実行するルーチンでMotorOutをデクリメントして、MotorOutが0になったら、
モーター停止。つまりは…
各MotorOutの10倍だったとさ。
まさかの跳躍。つまりアンプへの出力を0-100%とすると、ユーザー操作が占められる割合は

スロットルだけで最大110%
ロール、ピッチ、ヨーどれか一つだけで125%最大だってこと。

計算上、人間の操作だけでスロットルもピッチもヨーも、
全体の割合の100%が超えられると。うん。絶対にフルスロットルにしちゃ行けないね、
この子・・・・

というわけで、今日はここまでジャイロによる変動分の計算は、
このプログラムで飛ばしてる機体のトリマの抵抗値を見てから計算してみます。

まぁ、ゲインMAXにして、ジャイロの入力もMAXだとすると、gyroADCはMAXで1572
ってことは1572%んなアホな。あ、こいつも半分負に振れますね。

まぁ、積んでるジャイロ自体が村田製作所のENC-03Rという

http://akizukidenshi.com/catalog/g/gI-02759/

秋月でも売っている代物なので、
0.65mV/deg./sec.というわけで、±300deg./sec.振れたとして、振れ幅は、
0.39Vしかないと、それをADコンバーターに増幅も何もなしで突っ込んでて、
AVRの電源電圧は5Vなので、ADの変動幅は、
1024 * 0.39 / 5
で求まって、80くらいしかないので、実質のgyroADC MAXで122くらい
と言うわけで、122%手動の担当分と同じくらい最大で任せられますよっと、
こんな感じらしい。ちなみにちょっと待った、ジャイロの有効感度は?というと、
0.39/80で0.005くらいなんで、センサーの分解能の10分の1くらいっすね。

まぁ、つまりは意外と大ざっぱでも飛ぶらしいと、でもラジコンは人間様という、
最強の補完回路を外部に搭載してるんで何とかなるんでしょう。

まぁ、スロットルも、ヨーもピッチもロールも人間の操作で100%出力まで行けますよ。
ジャイロだけでも100%出力まで行けますけど、ゲインをトリマで調整して、
割合を調整して下さいね。っていうわりとざっくばらんな設計な様ですね。

ということで、ざっくばらんなコードレビュー?というかメモでした。

コメントを残す

メールアドレスが公開されることはありません。