public static void InsertCorrectedAcc(InsertDatum datum, InsertConfig config)
{
Console.WriteLine("CALLED: InsertCorrectedAcc, " + datum);
var tripsTable = new DataTable();
//if (config.Correction == InsertConfig.GpsCorrection.SpeedLPFMapMatching)
//{
// tripsTable = TripsSpeedLPF005MMDao.Get(datum);
//}
//else
//{
// tripsTable = TripsDao.Get(datum);
//}
foreach (DataRow row in tripsTable.Rows)
{
var correctedAccTable = AccCorrector.CorrectAcc(datum.StartTime, datum.EndTime, datum, row);
// Tripsテーブルの1行ごろの処理なので主キー違反があっても挿入されないだけ
// Trip単位で途中で挿入が異常終了した場合は、DELETEが必要
CorrectedAccDao.Insert(correctedAccTable);
}
}
public static DataTable CorrectAcc(DateTime startTime, DateTime endTime, InsertDatum datum, DataRow tripRow)
{
/*** LocationListenerのtimestampとSystem.currentTimeMillis()の差分を計算している
* LocationListenerのtimestampはおそらく、システム時間を参照しているからこの処理は意味ないかも ***/
int millisecTimeDiff = AndroidGpsRawDao.GetMilliSencodTimeDiffBetweenJstAndAndroidTime(
tripRow.Field <DateTime>(TripsDao.ColumnStartTime),
tripRow.Field <DateTime>(TripsDao.ColumnEndTime), datum);
var accRawTable = AndroidAccRawDao.Get(tripRow.Field <DateTime>(TripsDao.ColumnStartTime),
tripRow.Field <DateTime>(TripsDao.ColumnEndTime), millisecTimeDiff, datum);
accRawTable = DataTableUtil.GetTempCorrectedAccTable(accRawTable);
if (accRawTable.Rows.Count == 0)
{
// TODO ログ書き込み
//エラー期間をログファイルに記録
// WriteLog("DRIVER_ID:" + userID.DriverID + "SENSOR_ID:" + userID.SensorID + " " + tripStart + "～" + tripEnd + "加速度データなし", LogMode.acc);
return(null);
}
else if (false) // TODO 挿入済みチェックをすべきか？
{
// WriteLog("DRIVER_ID:" + userID.DriverID + "SENSOR_ID:" + userID.SensorID + " " + tripStart.ToString() + "～" + tripEnd.ToString() + "はすでに挿入済みです\r\n", LogMode.acc);
return(null);
}
// TODO ファイルに記録
//WriteLog("X方向平均(補正前)＝" + dtACC.Compute("AVG(LONGITUDINAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString(), LogMode.acc);
//WriteLog("Y方向平均(補正前)＝" + dtACC.Compute("AVG(LATERAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString(), LogMode.acc);
//WriteLog("Z方向平均(補正前)＝" + dtACC.Compute("AVG(VERTICAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString(), LogMode.acc);
var accurateStoppingAccRawTable = CorrectedAccDao.GetAccurateStoppingAccRaw(millisecTimeDiff, datum);
if (accurateStoppingAccRawTable.Rows.Count == 0)
{
// TODO ログ出力
//WriteLog("DRIVER_ID:" + userID.DriverID + "SENSOR_ID:" + userID.SensorID + " " + tripStart.ToString() + "～" + tripEnd.ToString() + "停止時の加速度データなし or タイムアウトエラーが発生 + \r\n", LogMode.acc);
return(null);
}
// TODO テーブル関数定義の場合、staticフィールドで参照するがよろし
double avgX = double.Parse(accurateStoppingAccRawTable.Rows[0]["ACC_X"].ToString());
double avgY = double.Parse(accurateStoppingAccRawTable.Rows[0]["ACC_Y"].ToString());
double avgZ = double.Parse(accurateStoppingAccRawTable.Rows[0]["ACC_Z"].ToString());
var vector1 = new ThreeDimensionalVector(0, 0, 9.8);
var vector2 = new ThreeDimensionalVector(avgX, avgY, avgZ);
var vectorProduct = new ThreeDimensionalVector(vector1.Y * vector2.Z - vector1.Z * vector2.Y, vector1.Z * vector2.X - vector1.X * vector2.Z, vector1.X * vector2.Y - vector1.Y * vector2.X);
var vasisVP = new ThreeDimensionalVector(vectorProduct.X / MathUtil.CalcVectorAbsoluteValue(vectorProduct),
vectorProduct.Y / MathUtil.CalcVectorAbsoluteValue(vectorProduct),
vectorProduct.Z / MathUtil.CalcVectorAbsoluteValue(vectorProduct));
var angle = Math.Asin(MathUtil.CalcVectorAbsoluteValue(vectorProduct) / (MathUtil.CalcVectorAbsoluteValue(vector1) * MathUtil.CalcVectorAbsoluteValue(vector2)));
// TODO ログ出力
//WriteLog("角度(Z軸補正)=" + angle * 360 / (2 * Math.PI), LogMode.acc);
//WriteLog("回転軸 = (" + vasisVP.X + ", " + vasisVP.Y + ", " + vasisVP.Z + ")", LogMode.acc);
Quaternion p = new Quaternion(0, vector2.X, vector2.Y, vector2.Z);
Quaternion q = new Quaternion(Math.Cos(angle / 2), vasisVP.X * Math.Sin(angle / 2), vasisVP.Y * Math.Sin(angle / 2), vasisVP.Z * Math.Sin(angle / 2));
Quaternion r = new Quaternion(Math.Cos(angle / 2), -vasisVP.X * Math.Sin(angle / 2), -vasisVP.Y * Math.Sin(angle / 2), -vasisVP.Z * Math.Sin(angle / 2));
Quaternion rp = MathUtil.MultiplyQuaternion(r, p);
Quaternion rpq = MathUtil.MultiplyQuaternion(rp, q);
for (int i = 0; i < accRawTable.Rows.Count; i++)
{
Quaternion p1 = new Quaternion(0,
accRawTable.Rows[i].Field <Single>("LONGITUDINAL_ACC"),
accRawTable.Rows[i].Field <Single>("LATERAL_ACC"),
accRawTable.Rows[i].Field <Single>("VERTICAL_ACC"));
Quaternion rp1 = MathUtil.MultiplyQuaternion(r, p1);
Quaternion rpq1 = MathUtil.MultiplyQuaternion(rp1, q);
// TODO SetFieldに書き直し、する価値ないか
accRawTable.Rows[i]["LONGITUDINAL_ACC"] = Single.Parse(rpq1.X.ToString());
accRawTable.Rows[i]["LATERAL_ACC"] = Single.Parse(rpq1.Y.ToString());
accRawTable.Rows[i]["VERTICAL_ACC"] = Single.Parse(rpq1.Z.ToString());
accRawTable.Rows[i]["ALPHA"] = (Single)(angle * 360 / (2 * Math.PI));
accRawTable.Rows[i]["VECTOR_X"] = (Single)vasisVP.X;
accRawTable.Rows[i]["VECTOR_Y"] = (Single)vasisVP.Y;
accRawTable.Rows[i]["ROLL"] = DBNull.Value;
accRawTable.Rows[i]["PITCH"] = DBNull.Value;
accRawTable.Rows[i]["YAW"] = DBNull.Value;
}
// TODO ログ出力
//WriteLog("X方向平均(補正後)＝" + dtACC.Compute("AVG(LONGITUDINAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString(), LogMode.acc);
//WriteLog("Y方向平均(補正後)＝" + dtACC.Compute("AVG(LATERAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString(), LogMode.acc);
//WriteLog("Z方向平均(補正後)＝" + dtACC.Compute("AVG(VERTICAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString() + "\r\n", LogMode.acc);
var tableBreaking = CorrectedAccDao.GetAccurateAccBreakingRaw(tripRow.Field <DateTime>(TripsDao.ColumnStartTime),
tripRow.Field <DateTime>(TripsDao.ColumnEndTime),
datum);
double xsum = 0;
double ysum = 0;
int count = 0;
for (int i = 0; i < tableBreaking.Rows.Count; i++)
{
DateTime end = DateTime.Parse(tableBreaking.Rows[i]["JST"].ToString());
DateTime start = end.AddSeconds(-10);
DataRow[] rows = accRawTable.Select("JST >= #" + start + "# AND JST <= #" + end + "#");
foreach (DataRow row in rows)
{
xsum = xsum + double.Parse(row["LONGITUDINAL_ACC"].ToString());
ysum = ysum + double.Parse(row["LATERAL_ACC"].ToString());
count++;
}
}
if (count == 0)
{
// TODO ログ出力
//WriteLog("DRIVER_ID:" + userID.DriverID + "SENSOR_ID:" + userID.SensorID + " " + "減速データがありません\r\n", LogMode.acc);
return(null);
}
double bx = xsum / count;
double by = ysum / count;
var v3 = new ThreeDimensionalVector(-1, 0, 0);
var v4 = new ThreeDimensionalVector(bx, by, 0);
double innerProduct = v3.X * v4.X + v3.Y * v4.Y;
double cos = innerProduct / (MathUtil.CalcVectorAbsoluteValue(v3) * MathUtil.CalcVectorAbsoluteValue(v4));
double anglexy = Math.Acos(cos);//ラジアン(0～180)
for (int j = 0; j < accRawTable.Rows.Count; j++)
{
double x = double.Parse(accRawTable.Rows[j]["LONGITUDINAL_ACC"].ToString());
double y = double.Parse(accRawTable.Rows[j]["LATERAL_ACC"].ToString());
double z = double.Parse(accRawTable.Rows[j]["VERTICAL_ACC"].ToString());
double x2;
double y2;
double z2;
if (by >= 0)
{
x2 = Math.Cos(anglexy) * x - Math.Sin(anglexy) * y;
y2 = Math.Sin(anglexy) * x + Math.Cos(anglexy) * y;
z2 = z;
}
else
{
x2 = Math.Cos(-anglexy) * x - Math.Sin(-anglexy) * y;
y2 = Math.Sin(-anglexy) * x + Math.Cos(-anglexy) * y;
z2 = z;
}
accRawTable.Rows[j]["LONGITUDINAL_ACC"] = Single.Parse(x2.ToString());
accRawTable.Rows[j]["LATERAL_ACC"] = Single.Parse(y2.ToString());
accRawTable.Rows[j]["VERTICAL_ACC"] = Single.Parse(z2.ToString());
accRawTable.Rows[j]["BETA"] = (Single)(anglexy * 360 / (2 * Math.PI));
}
avgX = double.Parse(accRawTable.Compute("AVG(LONGITUDINAL_ACC)", "JST > #" + tripRow.Field <DateTime>(TripsDao.ColumnStartTime) + "# AND JST < #" + tripRow.Field <DateTime>(TripsDao.ColumnEndTime) + "#").ToString());
avgZ = double.Parse(accRawTable.Compute("AVG(VERTICAL_ACC)", "JST > #" + tripRow.Field <DateTime>(TripsDao.ColumnStartTime) + "# AND JST < #" + tripRow.Field <DateTime>(TripsDao.ColumnEndTime) + "#").ToString());
//WriteLog("平均値 x = " + v4.X + ", y = " + v4.Y, LogMode.acc);
//WriteLog("XY回転角度＝" + "xy = " + anglexy * 360 / (2 * Math.PI), LogMode.acc);
//WriteLog("X方向平均(補正後)＝" + averagex.ToString(), LogMode.acc);
//WriteLog("Y方向平均(補正後)＝" + (double.Parse(dtACC.Compute("AVG(LATERAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString())).ToString(), LogMode.acc);
//WriteLog("Z方向平均(補正後)＝" + averagez.ToString() + "\r\n", LogMode.acc);
avgX = double.Parse(accRawTable.Compute("AVG(LONGITUDINAL_ACC)", "JST > #" + tripRow.Field <DateTime>(TripsDao.ColumnStartTime) + "# AND JST < #" + tripRow.Field <DateTime>(TripsDao.ColumnEndTime) + "#").ToString());
var v5 = new ThreeDimensionalVector(0, 0, 9.8);
var v6 = new ThreeDimensionalVector(avgX, 0, avgZ);
innerProduct = v5.X * v6.X + v5.Z * v6.Z;
cos = innerProduct / (MathUtil.CalcVectorAbsoluteValue(v5) * MathUtil.CalcVectorAbsoluteValue(v6));
anglexy = Math.Acos(cos);//ラジアン(0～180)
for (int j = 0; j < accRawTable.Rows.Count; j++)
{
double x = double.Parse(accRawTable.Rows[j]["LONGITUDINAL_ACC"].ToString());
double y = double.Parse(accRawTable.Rows[j]["LATERAL_ACC"].ToString());
double z = double.Parse(accRawTable.Rows[j]["VERTICAL_ACC"].ToString());
double x2;
double y2;
double z2;
if (avgX >= 0)
{
x2 = Math.Cos(anglexy) * x - Math.Sin(anglexy) * z;
y2 = y;
z2 = Math.Sin(anglexy) * x + Math.Cos(anglexy) * z;
}
else
{
x2 = Math.Cos(-anglexy) * x - Math.Sin(-anglexy) * z;
y2 = y;
z2 = Math.Sin(-anglexy) * x + Math.Cos(-anglexy) * z;
}
accRawTable.Rows[j]["LONGITUDINAL_ACC"] = Single.Parse(x2.ToString());
accRawTable.Rows[j]["LATERAL_ACC"] = Single.Parse(y2.ToString());
accRawTable.Rows[j]["VERTICAL_ACC"] = Single.Parse(z2.ToString());
accRawTable.Rows[j]["GAMMA"] = (Single)(anglexy * 360 / (2 * Math.PI));
}
// TODO ログ出力
//WriteLog("平均値 x = " + v6.X + ", z = " + v6.Z, LogMode.acc);
//WriteLog("XZ回転角度＝" + "xz = " + anglexy * 360 / (2 * Math.PI), LogMode.acc);
//WriteLog("X方向平均(補正後)＝" + double.Parse(dtACC.Compute("AVG(LONGITUDINAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString()).ToString(), LogMode.acc);
//WriteLog("Y方向平均(補正後)＝" + double.Parse(dtACC.Compute("AVG(LATERAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString()).ToString(), LogMode.acc);
//WriteLog("Z方向平均(補正後)＝" + double.Parse(dtACC.Compute("AVG(VERTICAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString()).ToString() + "\r\n", LogMode.acc);
return(accRawTable);
}
