|
|
@@ -0,0 +1,161 @@
|
|
|
+using System.Collections.Generic;
|
|
|
+using Newtonsoft.Json.Linq;
|
|
|
+using o0.IMU;
|
|
|
+using o0.Geometry;
|
|
|
+using o0;
|
|
|
+
|
|
|
+public class AttitudeJson : JCUnityLib.CustomJson
|
|
|
+{
|
|
|
+ public AttitudeJson() : base()
|
|
|
+ {
|
|
|
+ InitAttitudeImporter();
|
|
|
+ InitAttitudeExporter();
|
|
|
+ }
|
|
|
+
|
|
|
+ void InitAttitudeImporter()
|
|
|
+ {
|
|
|
+ importerDict.Add(typeof(_9AxisPreProcessor), (cs, o) => {
|
|
|
+ JToken jt = new JObject();
|
|
|
+ cs.FetchJsonProperties(jt, o,
|
|
|
+ "AccByteIndex",
|
|
|
+ "GyrByteIndex",
|
|
|
+ "MagByteIndex",
|
|
|
+ "ByteToAcc",
|
|
|
+ "ByteToGyr",
|
|
|
+ "ByteToMag",
|
|
|
+ "GyrCalibrater",
|
|
|
+ "MagCalibrater"
|
|
|
+ );
|
|
|
+ return jt;
|
|
|
+ });
|
|
|
+ importerDict.Add(typeof(MagnetometerAutoCalibrater), (cs, o) => {
|
|
|
+ JToken jt = new JObject();
|
|
|
+ cs.FetchJsonProperties(jt, o,
|
|
|
+ "CountPerLength",
|
|
|
+ "EllipsoidFitting",
|
|
|
+ "FitCountLeft",
|
|
|
+ "FitThreshold",
|
|
|
+ "LastTimestamp",
|
|
|
+ "Lock",
|
|
|
+ "MaxCount",
|
|
|
+ "NewBlock",
|
|
|
+ "NewBlockAccumulation",
|
|
|
+ "Variance",
|
|
|
+ "VectorByBlock"
|
|
|
+ );
|
|
|
+ return jt;
|
|
|
+ });
|
|
|
+ importerDict.Add(typeof(EllipsoidFitting), (cs, o) => {
|
|
|
+ JToken jt = new JObject();
|
|
|
+ cs.FetchJsonProperties(jt, o, "Center", "CorrectMatrixArray", "Radius");
|
|
|
+ return jt;
|
|
|
+ });
|
|
|
+ importerDict.Add(typeof(Dictionary<Vector<int>, Vector<double>>), (cs, o) => JToken.FromObject(o));
|
|
|
+ importerDict.Add(typeof(MeanMaintainer<Vector<double>>), (cs, o) => {
|
|
|
+ JToken jt = new JObject();
|
|
|
+ cs.FetchJsonProperties(jt, o, "Count", "Mean");
|
|
|
+ return jt;
|
|
|
+ });
|
|
|
+ importerDict.Add(typeof(ByteToShortByVariance[]), (cs, o) => {
|
|
|
+ var oArr = (ByteToShortByVariance[]) o;
|
|
|
+ var ja = new JArray();
|
|
|
+ for (int i = 0; i < oArr.Length; i++)
|
|
|
+ {
|
|
|
+ var jo = new JObject();
|
|
|
+ cs.FetchJsonProperties(jo, oArr[i], "Count", "Reverse", "Short", "ThresholdRate");
|
|
|
+ ja.Add(jo);
|
|
|
+ }
|
|
|
+ return ja;
|
|
|
+ });
|
|
|
+ importerDict.Add(typeof(ByteToShorts), (cs, o) => {
|
|
|
+ JToken jt = new JObject();
|
|
|
+ cs.FetchJsonProperties(jt, o, "ByteToShort");
|
|
|
+ return jt;
|
|
|
+ });
|
|
|
+ importerDict.Add(typeof(Vector<int>), (cs, o) => JToken.FromObject(o));
|
|
|
+ importerDict.Add(typeof(Vector<double>), (cs, o) => JToken.FromObject(o));
|
|
|
+ }
|
|
|
+
|
|
|
+ void InitAttitudeExporter()
|
|
|
+ {
|
|
|
+ exporterDict.Add(typeof(_9AxisPreProcessor), (cs, jt) => {
|
|
|
+ var o = new _9AxisPreProcessor();
|
|
|
+ FetchFieldValue(o, jt,
|
|
|
+ "AccByteIndex",
|
|
|
+ "GyrByteIndex",
|
|
|
+ "MagByteIndex",
|
|
|
+ "ByteToAcc",
|
|
|
+ "ByteToGyr",
|
|
|
+ "ByteToMag",
|
|
|
+ "GyrCalibrater",
|
|
|
+ "MagCalibrater"
|
|
|
+ );
|
|
|
+ return o;
|
|
|
+ });
|
|
|
+ exporterDict.Add(typeof(MagnetometerAutoCalibrater), (cs, jt) => {
|
|
|
+ var o = new MagnetometerAutoCalibrater();
|
|
|
+ cs.FetchFieldValue(o, jt,
|
|
|
+ "CountPerLength",
|
|
|
+ "EllipsoidFitting",
|
|
|
+ "FitCountLeft",
|
|
|
+ "FitThreshold",
|
|
|
+ "LastTimestamp",
|
|
|
+ "Lock",
|
|
|
+ "MaxCount",
|
|
|
+ "NewBlock",
|
|
|
+ "NewBlockAccumulation",
|
|
|
+ "Variance",
|
|
|
+ "VectorByBlock"
|
|
|
+ );
|
|
|
+ return o;
|
|
|
+ });
|
|
|
+ exporterDict.Add(typeof(EllipsoidFitting), (cs, jt) => {
|
|
|
+ var Center = cs.ParseByExporter<Vector<double>>(jt["Center"]);
|
|
|
+ var CorrectMatrix = cs.ParseByExporter
|
|
|
+ <MathNet.Numerics.LinearAlgebra.Matrix<double>>(jt["CorrectMatrixArray"]);
|
|
|
+ var Radius = cs.ParseByExporter<Vector<double>>(jt["Radius"]);
|
|
|
+ return new EllipsoidFitting(Center, CorrectMatrix, Radius);
|
|
|
+ });
|
|
|
+ exporterDict.Add(typeof(Dictionary<Vector<int>, Vector<double>>), (cs, jt) => {
|
|
|
+ var o = new Dictionary<Vector<int>, Vector<double>>();
|
|
|
+ foreach (JProperty item in jt)
|
|
|
+ o[cs.ParseByExporter<Vector<int>>(JArray.Parse(item.Name))]
|
|
|
+ = cs.ParseByExporter<Vector<double>>(item.Value);
|
|
|
+ return o;
|
|
|
+ });
|
|
|
+ exporterDict.Add(typeof(MeanMaintainer<Vector<double>>), (cs, jt) => {
|
|
|
+ var o = new MeanMaintainer<Vector<double>>();
|
|
|
+ cs.FetchFieldValue(o, jt, "Count", "Mean");
|
|
|
+ return o;
|
|
|
+ });
|
|
|
+ exporterDict.Add(typeof(ByteToShortByVariance[]), (cs, jt) => {
|
|
|
+ var o = new ByteToShortByVariance[3];
|
|
|
+ var arr = (JArray) jt;
|
|
|
+ for (int i = 0; i < o.Length; i++)
|
|
|
+ {
|
|
|
+ var item = arr[i];
|
|
|
+ var byteToShortByVariance = new ByteToShortByVariance();
|
|
|
+ cs.FetchFieldValue(byteToShortByVariance, item, "Count", "Reverse", "Short", "ThresholdRate");
|
|
|
+ o[i] = byteToShortByVariance;
|
|
|
+ }
|
|
|
+ return o;
|
|
|
+ });
|
|
|
+ exporterDict.Add(typeof(ByteToShorts), (cs, jt) => {
|
|
|
+ ByteToShorts o = new ByteToShorts(3);
|
|
|
+ cs.FetchFieldValue(o, jt, "ByteToShort");
|
|
|
+ return o;
|
|
|
+ });
|
|
|
+ exporterDict.Add(typeof(MathNet.Numerics.LinearAlgebra.Matrix<double>), (cs, jt) => {
|
|
|
+ JArray arr = (JArray) jt;
|
|
|
+ int rcCount = arr.Count;
|
|
|
+ MathNet.Numerics.LinearAlgebra.Matrix<double> matrix
|
|
|
+ = MathNet.Numerics.LinearAlgebra.CreateMatrix.Dense<double>(rcCount, rcCount);
|
|
|
+ for (var i = 0; i < rcCount; ++i)
|
|
|
+ for (var j = 0; j < rcCount; ++j)
|
|
|
+ matrix[i, j] = arr[i][j].Value<double>();
|
|
|
+ return matrix;
|
|
|
+ });
|
|
|
+ exporterDict.Add(typeof(Vector<int>), (cs, jt) => new Vector<int>(jt.ToObject<int[]>()));
|
|
|
+ exporterDict.Add(typeof(Vector<double>), (cs, jt) => new Vector<double>(jt.ToObject<double[]>()));
|
|
|
+ }
|
|
|
+}
|