ドローン（クアッドコプター）の制御プログラムが、いろいろと込み合ってきました。処理速度に余裕を持たせるため、いろいろ見直してます。現状が思いつくままに記述したのでだいぶ無駄が多いのですけどね・・。
その中でオープンソースとなっているmultiwii2.4のソースコード眺めていたらatan2（逆正接）の近似式のようなものを見つけました。逆平方根などはよく見かけるのですがatan2は初めて見ました。（私が知らないだけ？？）
multiwii2.4のIMU.cppの中、112行目辺りから130行目辺りに記載のある関数です。
何しているのかよくわかりませんが、検算してみたら確かにatan2の近似値です。それほど精度（分解能）ないですがドローン（クアッドコプター）の姿勢算出くらいであれば十分そう。この関数では ×10deg （0-1800）で値返します。
IDEのatan2関数と処理速度の比較をしてみたところ、効果は▲200μsと結構な高速化です。（micros()で挟んで計測してるだけなのでおおざっぱです。）
そのほか、今の制御ソースはだいぶ無駄が多いので、三角関数の処理や、式の展開など・・。またなるべく直接レジストリ操作してみたりと、（ここまでくるとArduinoIDE使ってる意味がだいぶなくなってきてる気がしますが・・・。）
記述をだいぶ見直して、もとの半分程度まで処理速度短縮できました。（無駄が多かっただけです。物理量の計算だけして、その値使ってない処理まであるほどでした・・・。）
最近、ArduinoUNOが少し非力に感じてきました。こんなこと思うとは思ってなかったんですけどね。まだしばらくはUNOにこだわってみたいと思いますが・・。
これでもう少し制御内容を追加できそうです・・。
コメント