走行制御ライブラリ

走行制御ライブラリ実装に伴う、ライブラリ開発者向けドキュメント

• 制御レイヤー構成
• 通信仕様
• 走行コマンド用通信
• 書き込み応答への遅延評価
• 三角関数のテーブル作成

---

制御レイヤー構成

走行制御システムは、複数レイヤーから成る。ホスト環境で動作するのは「ユーザアプリケーション」レイヤーであり、それ以下のレイヤーはターゲット環境にて動作する。

制御レイヤー構成

各レイヤーの意味は「リアルタイムシステム実現のための自立オブジェクト指向」(岩橋正実), CQ出版 を参考にすること。

vxv_tools/src/runCtrl/tRunCtrl のソースコードのレイヤー所属を以下に示す。

整数型の算術関数

• iatan2.h isincos.h isqrt.h

通信まわり

• transferCtrl.h nodeAccess.h packetHandleTarget.h

タスク管理

• tRunCtrl.h taskCtrl.h

走行モード管理

• modeCtrl.h

並進制御・回転制御

• velocityCtrl.h

自己位置推定

• bodyPosition.h velocityInfo.h coordinateCtrl_target.h

車輪制御

• bodyCtrl.h wheelCtrl.h

モータ制御

• motorCtrl.h

エンコーダ制御

• encCtrl.h

以上

---

三角関数のテーブル作成

自己位置計算等の計算に用いられる整数型の三角関数は、テーブルによって実装される。このテーブルは Ruby スクリプトの create_isincos.rb によって行われ、そのテーブルの分解能、すなわちテーブルサイズは変更可能である。

#!/usr/bin/ruby
#
# isin(), icos() を作るためのスクリプト
# Satofumi KAMIMURA
# $Id$

# 絶対値の最大値、角度分割数の設定
angle_div = 65536       # [0, 65535]
divid_shift = 15
divid_max = 1 << divid_shift    # [-32767, +32767]
angle_quad = angle_div >> 2

# 分割具合の設定 (0で誤差なし)
#table_shift_value = 5 
table_shift_value = 0

if ARGV[0] == "header"
print <<-"EOB"
#ifndef I_SIN_COS_H
#define I_SIN_COS_H

enum {
  ISINCOS_ANGLE_DIVIDED = #{angle_div}, /* [0, #{angle_div}] */
  ISINCOS_VALUE_MAX = #{divid_max},     /* [-#{divid_max}, +#{divid_max}] */
  ISINCOS_VALUE_SHIFT = #{divid_shift},
};


#include "cpp_extern_macro.h"
BEGIN_C_DECLS;

extern int isin(unsigned short div16);
extern int icos(unsigned short div16);

END_C_DECLS;
#endif /* !I_SIN_COS_H */
EOB
  exit
end

# テーブル部分の出力

# get this file name
__FILE__ =~ /\/*(.+)$/
thisFile = $1

print <<-"EOB"
/*
  \\brief 整数型の sin, cos 関数

  This program generated by #{thisFile}

  \\author Satofumi KAMIMURA

  $Id$

  angle_max = #{angle_div-1}
  divid_max = #{divid_max}
*/

#include "isincos.h"


static unsigned short sin_table[] = {
EOB

# 中の数値の表示
for i in 0 .. angle_div.to_i
  if i > (angle_quad.to_i >> table_shift_value)
    break
  end
  sin_val = divid_max.to_i \
  * Math::sin((2.0 * Math::PI * (i << table_shift_value).to_f / angle_div.to_f))
  print ' ', sin_val.to_i, ','
  if (i+1) % 10 == 0
    print "\n"
  end
end
print "};\n\n"

# isin(), icos() の部分の出力
print <<-"EOB"

static int sin_table_func(int n) {
EOB
if table_shift_value == 0
  print "  return sin_table[n];\n"
else
  print "  return sin_table[n >> #{table_shift_value}];\n"
end
print <<-"EOB"
}

int isin(unsigned short div16) {
  int ret;

  if (div16 <= #{angle_quad}) {
    ret = sin_table_func(div16);
  } else if (div16 <= #{2 * angle_quad}) {
    ret = sin_table_func(#{2 * angle_quad} - div16);
  } else if (div16 <= #{3 * angle_quad}) {
    ret = -sin_table_func(div16 -#{2 * angle_quad});
  } else {
    ret = -sin_table_func(#{4 * angle_quad} - div16);
  }
  return ret;
}


int icos(unsigned short div16) {
  div16 += #{3 * angle_quad};
EOB
if angle_div-1 != 0xffff
  print "  div16 &= #{angle_div - 1};\n"
end
print <<-"EOB"
  return -1 * isin(div16);
}
EOB

テーブルサイズが小さい状態において精度が要求される場合には、テーブル間の値を線形補間等で計算すべきである。テーブル生成は、整数型の atan() の生成にも用いられている。

以上

---

通信仕様

ホスト側(PC)からターゲット(SH7045F)側に送るデータの種類について定義を行う。

通信の種類

ホスト側からターゲットへの通信は、以下の３種類とする。

• 走行コマンドの送受信
• SCIP 規格の V コマンドを送信、応答を受け取る
• S-format のファイルを送信、ターゲットの制御プログラムを上書き

これらの通信規格については相互に関連性がないため、ターゲットに送信する側のプログラムは、必要な規格のプロトコルのみ扱えればよいとする。 これは、ターゲット側が受動的であり、メッセージを動的にホスト側に返すことがない、という仮定において成り立つ。

走行コマンドの送受信

走行制御コマンドのための通信規格であり、通信のエラー検出やパケットの再送機能を持つ。 通信規格は、走行コマンド用通信 にて述べる。

SCIP規格パケットの送受信

通信デバイスの先に繋がっているのが何かを識別するために、SCIP 規格における V コマンドが実装されるとする。

V コマンドの定義は以下の通り。

ホストからターゲットへの V コマンドパケット

ターゲットからホストへの応答

走行制御コントローラを識別するための情報として、ターゲットから送られるファームウェアバージョンの情報に "tRunCtrl" なる文字列を含めることとする。

S-Formatファイルの送信

通常の S-Formatファイル、つまり "S" から始まる文字列を送信することで、プログラムを RAM上にプログラムを配置し、転送後に実行されるとする。 転送先の RAM 番地の定義は、ターゲット側のファームウェアで任意に決定してよい。

---

走行コマンド用通信

ライブラリ(PC) とコントローラ(SH7045F)間で通信するデータの種類、及び通信のパケット形式について規定する。

通信設定

通信デバイスにはシリアルデバイスを用いるが、SH7045F側に USB-Serial変換チップを搭載することにより、ホスト側での USB デバイスを用いた通信をサポートする。
通信形式は、115200 bps, 8bit, ignore-parity, 1stop-bit とする。

通信の種類

通信の種類は、

• 通信先デバイスの構造体データを取得する
• 通信先デバイスの構造体データを更新する

であるとし、上記の各々の通信に対して以下の機能を持つとする。

• 受信データのエラーを検出し、受信エラーを返す機能
• 応答のタイムアウトを検出し、要求を再送する機能

パケット形式

ヘッダ部

• first_tag … パケット開始位置を識別するための固定タグ
• type … 読み込み、書き込み、イベント通知、応答を識別するタグ
• length … パケット長
• unique_id … ユニークIDを持つ
• reply … 応答コードを持つ
• target_id … アクセスする構造体への識別IDを持つ
• header_checksum … ヘッダ部のみのチェックサムを持つ

データ部

• data_num … データ組の数
• データ組
  • data_offset … アクセス構造体へのオフセット
  • data_size … アクセスする変数のバイト数
  • data_value(1,2,4 byte) … 更新する変数の値(読み込み時もダミーデータを持つ)

フッタ部

• checksum … 以上のチェックサム

追加パケット

ただし、これ以外にも SKIP の V コマンドのみは処理できなければならない。 V コマンドとは、

"V\r\r"

からなる文字列であり、応答として

"V\r"
"ステータス" "\r"
"ベンダ情報" "\r"
"製品情報" "\r"
"ファームウェア情報" "\r"
"プロトコルバージョン" "\r"
"センサ固有シリアル番号" "\r\r"

という文字列を返す。

トランザクション

要求に対して応答が帰ってきた時点で、一連の通信を完了する。基本的に、要求はライブラリ側からしか発生しない。

通信時の手順を書き出すと以下の通り。

要求送信(要求側)

• 要求毎に従ったフォーマットでパケットを作成し、応答側に送信する
• 要求の送信毎に、受信バッファ、送信バッファをクリアする
• 要求のパケット毎に、unique_id の一意性を保証する

要求受信(応答側)

• ヘッダ部の checksum を用いてヘッダ情報の正当性を確認する
• ヘッダ部の checksum に失敗した場合、そのパケットは棄却する
• パケットが棄却された場合、受信バッファから first_tag の探索を行う
• 過去に処理した unique_id にて要求が送られてきた場合、その要求は無視する
• 応答側は、type によって要求の種別を判断する
• target_id の指定する構造体が存在しない場合、エラー応答を返す
• パケットの checksum を行い、パケットの正当性を確認する
• パケットの checksum に失敗した場合、パケットを棄却し、エラー応答を返す
• 要求に対するデータ処理を行い、reply に正常終了を格納して応答パケットを返す

応答受信(要求側)

• ヘッダ部の checksum を用いてヘッダ情報の正当性を確認する
• ヘッダ部の checksum に失敗した場合、そのパケットは無視する
• 応答のタイムアウトが発生した場合、要求パケットを再送する
• エラー応答を受信した場合、要求パケットを再送する
• 再送時には、unique_id は更新しない
• 応答の unique_id が要求時の unique_id と異なる場合、その応答は無視する
• 要求パケットの再送は、一定回数であきらめ、通信失敗をユーザに返す
• パケットの checksum を行い、パケットの正当性を確認する
• パケットの checksum に失敗した場合、パケットを棄却し、エラー応答を返す
• 応答に対するデータ処理を行い、正常終了をユーザに返す

以上

---

書き込み応答への遅延評価

ライブラリ側からの書き込み要求に対し、処理が正常に行われれば、ターゲット側は書き込み応答を返す。ライブラリ側はこの応答を読み捨てて次の処理を行う。また、応答が一定時間内に届かなかった場合は、先に出した書き込み要求を再度発行する必要がある。

正常時の書き込みトランザクション

タイムアウトにより、書き込み要求の再送を行った場合

つまり、書き込み要求の応答が正常に戻ってくる、という仮定の下では、書き込み応答パケットを待つ必要はない。以下では、書き込み応答の評価を先送りする場合の実装例について述べる。

書き込み応答の不到着を検出

書き込み応答の遅延評価を行う場合の処理としては、

• 書き込み要求の送信後
  • 受信すべきユニークID のパケット、そのときに送信したパケットの内容を保持
  • 応答の到着を待たずに送信処理から戻る
• 以後の通信トランザクションにて、パケットを受信したとき
  • 保持中のパケットのユニークID と一致すれば、その送信トランザクションは完了
  • 時系列的に届くはずのパケットの次のパケットのユニークID が届いた場合
    • とどいたユニークID までの送信コマンドを再送

という手順を取る。書きコマンドを２つ転送し、応答を待っている場合の保持パケットのイメージは以下の通り。待機中のパケットと到着したユニークID の評価は、キューの先頭から行われる。

保持中の書き込みパケット

上記実装の問題点を以下にあげる。

• 応答の評価が次コマンド発行時になるため、タイムアウト評価が遅れうる
  • リアルタイム性の保証の消失
• 書き込みの再送を行ったときに、想定した動作になるかの保証がない

従って、上記実装はオプションで切り替えられるとし、通信時間の遅延が極端に気になる場合に用いるとよい。