Fenrir's BLog

しばらく更新をしていなかったオートパイロットシステム Super Sylphide関連の話です。色々と大人の事情(世の中こういえば許されるらしい?、笑)があって記事を更新していなかったのですが、自分で設計したDSP基板で自分が考えた航法アルゴリズム(注:リンク先は搭載されているアルゴリズムの一部に過ぎません)が走るというのは気持ちがいいものです、という状況にようやく到達しました。そこで現在に至るまでDSPの開発遍歴をまとめておきたいと思います。

まずは象徴的な図を一枚あげます。

番号1～3の順に開発が進行してきた

当初いきなりDSPで動くアルゴリズムをプログラムするのは難しいと考えました。そこでSylphideから取得したセンサの生データをPCで後から処理するためのプログラム analyze.exeをまずは書きました。analyze.exeは最も時間的制約が緩く、データを都合のよい順に並べ替えて処理しているので、オフラインでの解析に相当します。
ここで並べ替えをしている理由について軽く触れておきます。A/D変換を行う加速度計、ジャイロから出力されるデータは時間遅れが無視できるほど小さいため問題がないのですが、GPSなど内部で複雑な処理を行うものについては無視できない時間遅れが発生するため、データの到着時刻と、そのデータが表現する状態の時刻は一致していません。そのためanalyze.exeでは並べ替えによってこの問題に対応しました。
なおanalyze.exeについてはその後高機能化し、複数GPSアンテナによる解析等を行えるようにしました。

その後、それをリアルタイム化したanalyzeRT.exeというPCで動くプログラムをリリースしました。analyzeRT.exeでは、データの到達順に逐次処理、言い換えれば遅れて入ってきたデータについても最新の状態まで追いかけ処理することによって、時間的に常に最新の状態が出力されるようにしてあります。このプログラムは入力として、analyze.exeと同じSylphideで取得された生データログファイル、あるいはSylphideをPCにUSB経由で接続する際に作られる仮想COMポートから流れてくる生データストリーム、の両者を受け取れるようにしてあります。これでリアルタイムのアルゴリズム的な動作検証を行いました。

最後にDSPへのanalyzeRT.exeの移植をし、NAV2_Coreファームウェアを作成しました。行列計算等の基本演算部分はDSPの特性が生かせるPCのコードを変更しましたが、肝心のアルゴリズム部分はPCと共通のヘッダとし、新たな問題がうきる可能性をできうる限り回避しました。PCとコードを共通化できたのは、使用したDSPがC++でコーディング可能であることによっています。
DSPのデバックフェーズでは検証のしやすさを優先させ、Sylphideのデータを直接処理させずに、DSPの統合開発環境(IDE)であるCode Composer Studio (CCS)に搭載されたRTDX(Real-time Data Exchange)の機能を活用し、PCからデータを送りつけてDSPでそれを処理、またPCに回収するという流れを使いました。行ったのは2つの検証です。1つはanalyzeRT.exeの結果と比べことによるDSP内での処理の妥当性、もう1つはCCSに搭載されたプロファイラによる負荷の見積もりです。両内容ともクリアし、後者によれば加速度計、ジャイロによる20Hzの時間更新(行列の積算、加算が中心)と、GPSによる4Hzの観測更新(行列の加算、逆行列計算が中心)で、最適化オプション-O2で約20Mticks/secの演算量でした。使用しているDSPは200MHzで200Mticks/secの演算能力があるのですが、更新レートが同じであればオーバーヘッドを考えても最大でDSPの演算能力の3割以内くらいで済むと予想しています。これなら航法の後に続く誘導、制御にも相当の能力を割り当てられそう、とほっと胸を撫で下ろしています。

文章にまとめてしまうと量をあまりこなしているようには見えませんが、試行錯誤、紆余曲折をしつつ現在に至っていますので、かけた時間はかなりのものになりました。今後の誘導、制御ルーチンの開発では今回までに獲得した検証スキームを有効活用して効率化をはかりたいと思います。

※その後、この時点での航法ルーチンには通信の問題があることが発覚、解消しました。