用例③：POC-加速度＆傾斜計＆ジャイロ・センサ小型USBモジュール

privatevoid ConfigureYocto3d(string serial)
{
YDataLogger logger = YDataLogger.FindDataLogger(serial + ".dataLogger");
logger.set_recording(YDataLogger.RECORDING_OFF);
logger.forgetAllDataStreams();
YQt q1 = YQt.FindQt(serial + ".qt1");
q1.set_logFrequency("30/s");
YQt q2 = YQt.FindQt(serial + ".qt2");
q2.set_logFrequency("30/s");
YQt q3 = YQt.FindQt(serial + ".qt3");
q3.set_logFrequency("30/s");
YQt q4 = YQt.FindQt(serial + ".qt4");
q4.set_logFrequency("30/s");
logger.set_autoStart(YDataLogger.AUTOSTART_ON);
logger.get_module().saveToFlash();
}

四次元数

　バイクの傾きを計測するためにtilt1機能の使用よりもジャイロスコープの使用が推奨されます。tilt1機能は加速度計を使用しているため、バイクの加速や減速に影響を受けてしまいます。また、正確性を重視した作りゆえに１秒あたりのリフレッシュが10回のみとなります。

　横方向・進行方向・勾配の３要素でモジュールの位置が表されます。四次元数について知識を持っていなくてもAPIが計算してくれるため心配は要りません。モジュールが動き出したらすぐに横方向・進行方向・勾配を計測し始めるようにコールバック機能をつけるだけで完成です。

　データロガーは直接３つの角度を記録することはできませんが、４つの四次元数を記録することはできます。ゆえにその記録を以下のコードを使用して再計算すれば良いのです。

List<double> ComputeAngles(double w, double x, double y, double z, doubledefaultRoll)
{
  double sqw = 0;
  double sqx = 0;
  double sqy = 0;
  double sqz = 0;
  double norm = 0;
  double delta = 0;
  double head = 0;
  double pitch = 0;
  double roll = 0;
List<double> res = new List<double>();
sqw = w * w;
sqx = x * x;
sqy = y * y;
sqz = z * z;
norm = sqx + sqy + sqz + sqw;
delta = y * w - x * z;
  if(delta > 0.499 * norm)
  {
roll = defaultRoll;
pitch = 90.0;
head = Math.Round(2.0 * 1800.0 / Math.PI * Math.Atan2(x, w)) / 10.0;
  }
  else
  {
  if(delta < -0.499 * norm)
  {
roll = defaultRoll;
pitch = -90.0;
head = Math.Round(-2.0 * 1800.0 / Math.PI * Math.Atan2(x, w)) / 10.0;
  }
  else
  {
roll = Math.Round(1800.0 / Math.PI *
Math.Atan2(2.0 * (w * x + y * z), sqw - sqx - sqy + sqz)) / 10.0;
pitch = Math.Round(1800.0 / Math.PI *
Math.Asin(2.0 * delta / norm)) / 10.0;
head = Math.Round(1800.0 / Math.PI *
Math.Atan2(2.0 * (x * y + z * w), sqw + sqx - sqy - sqz)) / 10.0;
  }
  }
res.Add(roll);
res.Add(pitch);
res.Add(head);
  Console.WriteLine("w:" + w + " x:" + x + " y:" + y + " z:" + z + "-> r:" + roll);
  return res;
}

データロガー情報を取り出す

角度を計算するには２つのステップを踏む必要があります。はじめにモジュールのフラッシュメモリに蓄積した４つの四次元数を取り出します。

private List<YMeasure> LoadQT(string hwid)
{
YQt qt = YQt.FindQt(hwid);
YDataSet dataset = qt.get_recordedData(0, 0);
  int progress=0;
  do{
progress = dataset.loadMore();
  }while(progress <100);
  return dataset.get_measures();
}

次に四次元数から角度を計算します。今回の実験では一秒間に30回の計測とし、データロガーはすべて計測の記録と同時に起こるのでqt_w,qt_x,qt_y,qt_zは同じ計測数となります。

private List<int> GetAnglesList(string serial)
{
List<YMeasure> qt_w = LoadQT(serial + ".qt1");
List<YMeasure> qt_x = LoadQT(serial + ".qt2");
List<YMeasure> qt_y = LoadQT(serial + ".qt3");
List<YMeasure> qt_z = LoadQT(serial + ".qt4");
  int count = qt_w.Count;
List<int> angles = new List<int>(count);
  for(int i = 0; i < count; i++)
  {
List<double> pos = ComputeAngles(qt_w[i].get_averageValue(),
qt_x[i].get_averageValue(),
qt_y[i].get_averageValue(),
qt_z[i].get_averageValue(),
  0);
angles.Add((int)Math.Round(pos[0]));
  }
  return angles;
}