MagCalib: add DIA and ODI for compass 2/3

wpsliu123 · Jan 21, 2017 · 41955e3 · 41955e3
1 parent adafd41
commit 41955e3
Showing 1 changed file with 14 additions and 0 deletions.
diff --git a/MagCalib.cs b/MagCalib.cs
@@ -1212,6 +1212,13 @@ public static void SaveOffsets2(double[] ofs)
                     if (ofs.Length > 3)
                     {
                         // ellipsoid
+                        MainV2.comPort.setParam("COMPASS_DIA2_X", (float)ofs[3]);
+                        MainV2.comPort.setParam("COMPASS_DIA2_Y", (float)ofs[4]);
+                        MainV2.comPort.setParam("COMPASS_DIA2_Z", (float)ofs[5]);
+
+                        MainV2.comPort.setParam("COMPASS_ODI2_X", (float)ofs[6]);
+                        MainV2.comPort.setParam("COMPASS_ODI2_Y", (float)ofs[7]);
+                        MainV2.comPort.setParam("COMPASS_ODI2_Z", (float)ofs[8]);
                     }
                 }
                 catch
@@ -1248,6 +1255,13 @@ public static void SaveOffsets3(double[] ofs)
                     if (ofs.Length > 3)
                     {
                         // ellipsoid
+                        MainV2.comPort.setParam("COMPASS_DIA3_X", (float)ofs[3]);
+                        MainV2.comPort.setParam("COMPASS_DIA3_Y", (float)ofs[4]);
+                        MainV2.comPort.setParam("COMPASS_DIA3_Z", (float)ofs[5]);
+
+                        MainV2.comPort.setParam("COMPASS_ODI3_X", (float)ofs[6]);
+                        MainV2.comPort.setParam("COMPASS_ODI3_Y", (float)ofs[7]);
+                        MainV2.comPort.setParam("COMPASS_ODI3_Z", (float)ofs[8]);
                     }
                 }
                 catch