かみやんの技術者ブログ

主にプログラムの話です

TDS01Vのプログラム(C#、ソース付き)

今日は、昨日届いた3Dセンサー TDS01VをPC側制御ソフトのAibiUIに接続するプログラムを書いた。
GPS(アンタレス48)やLRF(Top-URG)では、シリアル接続して、要求を出したらデバイス側からデータが一方的にずっと流れてくるので、SerialPortの受信イベントをハンドリングすればよかったのだが、このTDS01Vは毎回毎回、「データくれ」要求を出さないといけないので、イベントドリブンでは不便。ということでマルチスレッドにした。

まず、フローは、昨日のエントリで書いたように、

  1. ソフトウェアリセットをかける
  2. 計測条件設定で、周期(最大10Hz)、基準気圧(0m気圧)、偏角(方位のオフセット)を設定
  3. センサ情報項目設定で、地磁気、加速度、気圧のどれを計測したいか設定
  4. 磁気センサ初期化要求
  5. 計測開始
  6. 状態要求で情報が更新されるまで待つ
  7. センサ情報要求
  8. 6.へ戻る、または、計測停止


である。これをコードにすると、

//3Dセンサー(TDS01V)通信クラス
//Copyright: ibis inc.
//Author  : Eiji Kamiya 2009/10/17
//Licence : BSD
namespace AibiUI.App.IO.TdsSerial {
    class TdsSerial {
        //プロパティ
        public SerialPort SerialPort{get;set;}
        //メンバ変数
        private Thread thread;

        //TDS01Vのメインスレッド
        private void Start()
        {
            try {
                SerialPort.NewLine = "\r\n";
                SerialPort.DiscardInBuffer();
                SerialPort.DiscardOutBuffer();
                SerialPort.ReadTimeout = 1000;//1秒以内にレスポンスをもらう
                Tds01v tds = new Tds01v(SerialPort);
                //リセット
                tds.Send(Tds01v.CommandType.Reset);
                Thread.Sleep(300);
                //計測条件設定
                tds.SendSetCondition(0.1f, 1040.0f, 0.0f);
                //センサ情報項目設定
                tds.SendSetSensorElement(Tds01v.SensorElementType.CompassVector);
                //磁気センサ初期化要求
                tds.Send(Tds01v.CommandType.InitializeCompass);
                //計測開始
                tds.Send(Tds01v.CommandType.Start);
                while (true) {
                    try {
                        //状態要求をして、データ更新済みフラグが立つまで待つ
                        tds.WaitForUpdateState(1000);
                        //センサ情報要求
                        string line = tds.Send(Tds01v.CommandType.QueryData);
                        OnLineReceived(ParseCompassVector(line));//Formに通知
                        Thread.Sleep(10);
                    } catch (FormatException ex) {
                        Debug.WriteLine("TDS " + ex.Message);
                    }
                }
            } catch (FormatException ex) {
                MessageBox.Show(ex.Message, "Error at TdsSerial Thread");
            } catch (IOException ex) {
                Debug.WriteLine("TdsSerial Thread : " + ex.Message);
            } catch (InvalidOperationException ex) {
                Debug.WriteLine("TdsSerial Thread : " + ex.Message);
            }
        }
        
        //スレッド停止
        private void StopThread()
        {
            if (thread != null && thread.IsAlive) {//スレッドが生きているとき
                thread.Interrupt();
                thread.Join();
            }
            thread = null;
        }

        //スレッド開始
        public void StartThread()
        {
            StopThread();
            thread = new Thread(new ThreadStart(Start));
            thread.Start();
        }
    }
}

StartThread()をGUI側から呼べばOK。Start()のOnLineReceived()は各自Formへの通達方法に変えてください。俺は、デリゲート経由でGUI側へ通知しています。
あと、Start()の中で使われているTds01vクラスは、下記。

//3Dセンサー(TDS01V)クラス
//Copyright: ibis inc.
//Author  : Eiji Kamiya 2009/10/17
//Licence : BSD
using System;
using System.Collections.Generic;
using System.Text;
using System.IO;
using System.IO.Ports;
using System.Threading;

namespace AibiUI.Library.Device.Compass
{
    class Tds01v
    {
        //コマンドタイプ
        public enum CommandType : int
        {
            SetCondition        =0x05,//計測条件設定
            SetSensorElement    =0x0D,//センサ情報項目設定
            Reset               =0x0F,//リセット(引数なし)
            Start               =0x21,//計測開始(引数なし)
            Stop                =0x23,//計測停止(引数なし)
            InitializeCompass   =0x27,//地磁気センサー初期化要求(引数なし)
            QueryData           =0x29,//センサ情報要求(引数なし)
            QueryState          =0x2B,//状態要求(引数なし)
            QueryDetailState    =0x2D,//詳細状態要求(引数なし)
            UnlockEEPROM        =0x35,//EEPROMプロテクト解除
            InitializeEEPROM    =0x3B,//EEPROM初期化要求
            Diagnostic          =0x49,//自己診断開始
            Ping                =0x55,//通信回線試験要求(引数なし)
            QueryDiagnostic     =0x59,//自己診断状態要求(引数なし)
            QueryVersion        =0x5F,//ROMバージョン要求(引数なし)
        }

        //センサ情報項目タイプ
        public enum SensorElementType : int{
            CompassVector       =0x83,//地磁気:ベクトルデータ(6byte)
            CompassAzimuth      =0x84,//地磁気:方位角情報(2byte)
            AccelVector         =0x93,//加速度:ベクトルデータ(6byte)
            AccelPitchRoll      =0x94,//加速度:傾斜角情報(4byte)
            PressureAirPressure =0xA3,//気圧:気圧情報(2byte)
            PressureAltitude    =0xA4,//気圧:高度情報(2byte)
            AllVector           =0xF3,//全センサ:ベクトルデータ(地磁気、加速度)(12byte)
            AllSensor           =0xF4,//全センサ:計測データ(方位角、傾斜角、気圧、高度、温度、電圧)(14byte)
            AllData             =0xF7,//全センサ:全データ(ベクトルデータ+計測データ)(26byte)
        }

        //メンバ変数
        private SerialPort serialPort;//シリアルポート

        //コンストラクタ
        public Tds01v(SerialPort sp)
        {
            serialPort = sp;
        }

        //計測条件設定コマンド送信
        //period : 周期計測時間(単位:秒)、0.1単位で最大25.5秒まで。0にすると1回計測
        //pressure : 基準気圧(単位:hPa)、710.0〜1062.0
        //angle : 偏角設定(単位:度)、0.0〜360.0
        public void SendSetCondition(float period, float pressure, float angle)
        {
            byte[] buf = Encoding.ASCII.GetBytes(string.Format(
                "{0:X2}{1:X2}{2:X4}{3:X4}\r\n", (int)CommandType.SetCondition,
                (int)(period * 10.0f), (int)(pressure * 10.0f), (int)(angle * 10.0f)));
            serialPort.Write(buf, 0, buf.Length);
            ReceiveAck(CommandType.SetCondition);
        }

        //センサ情報項目設定コマンド送信
        public void SendSetSensorElement(SensorElementType type)
        {
            byte[] buf = Encoding.ASCII.GetBytes(string.Format(
                "{0:X2}{1:X2}\r\n", (int)CommandType.SetSensorElement, (int)type));
            serialPort.Write(buf, 0, buf.Length);
            ReceiveAck(CommandType.SetSensorElement);
        }

        //状態要求コマンド送信
        public byte SendQueryState()
        {
            string line=Send(CommandType.QueryState);
            if (line.Length != 2) {//2文字でないとき
                throw new FormatException("QueryState : Response length must be 2.");
            }
            return byte.Parse(line, System.Globalization.NumberStyles.HexNumber);
        }

        //状態要求コマンドを送って、計測データ更新済みフラグがたつまで待つ
        public void WaitForUpdateState(int timeout)
        {
            DateTime t = DateTime.Now;
            byte state;
            do {
                state = SendQueryState();
                if ((t - DateTime.Now).Milliseconds > timeout) {//タイムアウトのとき
                    throw new FormatException("QueryState : Timeout for updating");
                }
                Thread.Sleep(1);
            } while ((state & 0x10) == 0);
        }

        //引数なしのコマンド送信
        public string Send(CommandType cmd)
        {
            byte[] buf = Encoding.ASCII.GetBytes(string.Format("{0:X2}\r\n", (int)cmd));
            serialPort.Write(buf, 0, buf.Length);
            if (IsResponseData(cmd)) {//データを返すコマンドのとき
                return serialPort.ReadLine();
            } 
            //ACKを返すコマンドのとき
            ReceiveAck(cmd);
            return null;
        }
        
        //コマンドに対するレスポンスがACKか?NAKのときFormatException
        public void ReceiveAck(CommandType cmd)
        {
            if (IsResponseData(cmd)) {//データを返すコマンドのとき
                throw new FormatException(cmd + " response is data.");
            }
            string res = serialPort.ReadLine();
            if (res.Length != 2) {//2文字以上のとき
                throw new FormatException(cmd.ToString() + ": ACK length must be 2.");
            }
            int n = int.Parse(res, System.Globalization.NumberStyles.HexNumber);
            if (((~((int)cmd))&0xff) != n) {//NAKのとき(resがcmdの反転でないとき)
                throw new FormatException(cmd.ToString() + " is received NAK.");
            }
            return;//ACK
        }

        //データを返すコマンドか?
        private static bool IsResponseData(CommandType cmd)
        {
            if (cmd == CommandType.QueryData ||
                cmd == CommandType.QueryState ||
                cmd == CommandType.QueryDetailState ||
                cmd == CommandType.QueryDiagnostic ||
                cmd == CommandType.QueryVersion) {//データを返すコマンドのとき
                return true;
            }
            return false;
        }
    }
}

エラーのときは、FormatExceptionを投げていますが、特にこの型に意味はありません。自分でExceptionクラスを作るのが面倒なので、適当にFormatExceptionを選びました。
あと、俺はTDS01Vの地磁気センサ、加速度計、気圧計のうち地磁気センサしか使わないので、レスポンスクラスとか、レスポンスのパーサクラスとかは作ってありません。また、地磁気は方位で得る方法ではなく、磁気ベクトルで得る方法で使っています。これの方が、磁力が大きすぎるときとか小さすぎるときなど異常を感知できるので。
その代わり、自分でキャリブレーションGUIを作って補正してあります(この辺のエントリ参照)。

で、ロボットに積んで動かしてみました。搭載位置は、GPSのポールの中点。GPSの30cm下。PCの30cm上。


上図、TDS01Vでのキャリブレーション画面。まずまず円形。


上図、HMC1052Lでのキャリブレーション画面。こっちのほうが精度がありそう。

比較のために、HMC1052Lのときに撮った画面も掲載します。HMC1052Lは、5V電源のみONのときの絵です。12VもONにすると円にならず。

さて、次はロータリーエンコーダ+TDS01Vでの自己姿勢推定が、どのぐらいの精度になるか。センサフュージョンとその可視化の実装だな。ロータリーエンコーダのみよりは、よくなることは分かっているが実用レベルまで上がるかどうか。
つくばチャレンジ2009まで、あと33日!時間がない!

追記:最新のソースは、「AibiUI公開」で公開しました。そちらを参照してください。