init
This commit is contained in:
594
Common/DTS.Common.DAS.Concepts/Test/Module/TiltAxes.cs
Normal file
594
Common/DTS.Common.DAS.Concepts/Test/Module/TiltAxes.cs
Normal file
@@ -0,0 +1,594 @@
|
||||
using DTS.Common.Utilities;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.ComponentModel;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using static DTS.Common.Enums.DASFactory.DFConstantsAndEnums;
|
||||
|
||||
namespace DTS.Common.DAS.Concepts
|
||||
{
|
||||
|
||||
public partial class Test
|
||||
{
|
||||
/// <summary>
|
||||
/// A container for DTS generic module concepts.
|
||||
/// </summary>
|
||||
public sealed partial class Module
|
||||
{
|
||||
public class TiltAxis
|
||||
{
|
||||
public int AxisNumber { get; set; }
|
||||
public double CurrentTilt { get; set; }
|
||||
public string Label { get; set; }
|
||||
public int Inversion { get; set; } = 1;
|
||||
public bool IsIgnored { get; set; } = false;
|
||||
public double MountedOffset { get; set; } = 0.0;
|
||||
public double TargetOffset { get; set; } = 0.0;
|
||||
}
|
||||
|
||||
public class TiltAxesHelper
|
||||
{
|
||||
bool UseForTiltCalculation { get; set; } = true;
|
||||
|
||||
public Dictionary<int, TiltAxis> AxisConfigurations { get; set; }
|
||||
= new Dictionary<int, TiltAxis>();
|
||||
|
||||
public TiltAxis AxisOne { get; set; }
|
||||
public TiltAxis AxisTwo { get; set; }
|
||||
|
||||
public double LevelTolerance { get; set; } = 0.5;
|
||||
public TiltAxesHelper()
|
||||
{
|
||||
AxisConfigurations = new Dictionary<int, TiltAxis>
|
||||
{
|
||||
{ 1, new TiltAxis() },
|
||||
{ 2, new TiltAxis() },
|
||||
{ 3, new TiltAxis() }
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Constructor to build Helper from FW Attributes.
|
||||
/// </summary>
|
||||
public TiltAxesHelper(
|
||||
string TiltAxes,
|
||||
double MountOffsetAxisOne,
|
||||
double MountOffsetAxisTwo,
|
||||
double TargetAxisOne,
|
||||
double TargetAxisTwo,
|
||||
double LevelTolerance,
|
||||
int AxisIgnored,
|
||||
bool UseForTiltCalculation)
|
||||
: this()
|
||||
{
|
||||
this.LevelTolerance = LevelTolerance;
|
||||
// Parse the TiltAxes string from e.g. "IXIYIZ"
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
// X, Y, or Z
|
||||
AxisConfigurations[i].Label = TiltAxes[i * 2].ToString();
|
||||
// N = negative = -1 otherwise positive
|
||||
AxisConfigurations[i].Inversion =
|
||||
TiltAxes[i * 2 + 1] == 'N' ? -1 : 1;
|
||||
}
|
||||
|
||||
if (AxisIgnored < 3)
|
||||
AxisConfigurations[AxisIgnored].IsIgnored = true;
|
||||
|
||||
// References to used axes depending on ignored axis
|
||||
if (AxisIgnored == 1)
|
||||
{
|
||||
AxisOne = AxisConfigurations[2];
|
||||
AxisTwo = AxisConfigurations[3];
|
||||
}
|
||||
else if (AxisIgnored == 2)
|
||||
{
|
||||
AxisOne = AxisConfigurations[1];
|
||||
AxisTwo = AxisConfigurations[3];
|
||||
}
|
||||
else
|
||||
{
|
||||
AxisOne = AxisConfigurations[1];
|
||||
AxisOne = AxisConfigurations[2];
|
||||
}
|
||||
|
||||
AxisOne.MountedOffset = MountOffsetAxisOne;
|
||||
AxisTwo.MountedOffset = MountOffsetAxisTwo;
|
||||
AxisOne.TargetOffset = TargetAxisOne;
|
||||
AxisOne.TargetOffset = TargetAxisTwo;
|
||||
}
|
||||
|
||||
|
||||
public string AxesToString()
|
||||
{
|
||||
string s = "";
|
||||
foreach (var a in AxisConfigurations)
|
||||
s = s + (a.Value.Label) + (a.Value.Inversion == -1 ? "N" : "P");
|
||||
return s;
|
||||
}
|
||||
}
|
||||
|
||||
public enum TiltAxesSimple
|
||||
{
|
||||
XYZ = 0,
|
||||
XZY = 1,
|
||||
YXZ = 2,
|
||||
YZX = 3,
|
||||
ZXY = 4,
|
||||
ZYX = 5,
|
||||
}
|
||||
|
||||
public static TiltAxesSimple SimplifyTiltAxes(TiltAxes axes)
|
||||
{
|
||||
return (TiltAxesSimple)Enum.Parse(typeof(TiltAxesSimple), axes.ToString().Replace("I", ""));
|
||||
}
|
||||
|
||||
public enum TiltAxesPolarity
|
||||
{
|
||||
PPP = 0,
|
||||
PPN = 1,
|
||||
PNP = 2,
|
||||
PNN = 3,
|
||||
NPP = 4,
|
||||
NPN = 5,
|
||||
NNP = 6,
|
||||
NNN = 7
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Convert a string representation of a recording mode enumeration into its corresponding
|
||||
/// enumeration value.
|
||||
/// </summary>
|
||||
///
|
||||
/// <param name="tiltAxes">
|
||||
/// The <see cref="string"/> representation to be converted.
|
||||
/// </param>
|
||||
///
|
||||
/// <returns>
|
||||
/// The <see cref="Test.Module.TiltAxes"/> value corresponding to the
|
||||
/// specified string, if any. If not, an exception will be thrown.
|
||||
/// </returns>
|
||||
///
|
||||
public static TiltAxes GetTiltAxesFromString(string tiltAxes)
|
||||
{
|
||||
try
|
||||
{
|
||||
return (TiltAxes)Enum.Parse(typeof(TiltAxes), tiltAxes);
|
||||
}
|
||||
|
||||
catch (Exception ex)
|
||||
{
|
||||
throw new Exception("encountered problem getting tilt axis from string representation " + (null != tiltAxes ? "\"" + tiltAxes + "\"" : "<<NULL>>"), ex);
|
||||
}
|
||||
}
|
||||
|
||||
public static int[] GetPolaritiesFromTiltAxes(TiltAxes ta)
|
||||
{
|
||||
var rv = new List<int>();
|
||||
var rvBool = GetBoolPolaritiesFromTiltAxes(ta);
|
||||
foreach (var b in rvBool)
|
||||
{
|
||||
rv.Add(b ? -1 : 1);
|
||||
}
|
||||
return rv.ToArray();
|
||||
}
|
||||
|
||||
public static bool[] GetBoolPolaritiesFromTiltAxes(TiltAxes tiltAxes)
|
||||
{
|
||||
var rv = new bool[3];
|
||||
var ax = 0;
|
||||
|
||||
var tiltAxesString = tiltAxes.ToString();
|
||||
|
||||
// Parse enum for I's, invert corresponding array member;
|
||||
for (var i = 0; i < tiltAxesString.Length; i++)
|
||||
{
|
||||
if (tiltAxesString[i] == 'I')
|
||||
{
|
||||
rv[ax] = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
ax++;
|
||||
}
|
||||
}
|
||||
return rv;
|
||||
}
|
||||
public enum TiltSensorCalAttributes
|
||||
{
|
||||
TILTSENSOR_CAL_1_GainAxis1 = 0,
|
||||
TILTSENSOR_CAL_2_ZeroAxis1 = 1,
|
||||
TILTSENSOR_CAL_3_ZeroDomAxis2PosAxis1 = 2,
|
||||
TILTSENSOR_CAL_4_ZeroDomAxis2NegAxis1 = 3,
|
||||
TILTSENSOR_CAL_5_ZeroDomAxis3PosAxis1 = 4,
|
||||
TILTSENSOR_CAL_6_ZeroDomAxis3NegAxis1 = 5,
|
||||
TILTSENSOR_CAL_7_GainAxis2 = 6,
|
||||
TILTSENSOR_CAL_8_ZeroAxis2 = 7,
|
||||
TILTSENSOR_CAL_9_ZeroDomAxis1PosAxis2 = 8,
|
||||
TILTSENSOR_CAL_10_ZeroDomAxis1NegAxis2 = 9,
|
||||
TILTSENSOR_CAL_11_ZeroDomAxis3PosAxis2 = 10,
|
||||
TILTSENSOR_CAL_12_ZeroDomAxis3NegAxis2 = 11,
|
||||
TILTSENSOR_CAL_13_GainAxis3 = 12,
|
||||
TILTSENSOR_CAL_14_ZeroAxis3 = 13,
|
||||
TILTSENSOR_CAL_15_ZeroDomAxis1PosAxis3 = 14,
|
||||
TILTSENSOR_CAL_16_ZeroDomAxis1NegAxis3 = 15,
|
||||
TILTSENSOR_CAL_17_ZeroDomAxis2PosAxis3 = 16,
|
||||
TILTSENSOR_CAL_18_ZeroDomAxis2NegAxis3 = 17,
|
||||
}
|
||||
/// <summary>
|
||||
/// 3 digits of precision by default
|
||||
/// http://manuscript.dts.local/f/cases/34373/Tilt-Sensor-4-0-reported-value-significant-figure-in-DataPRO
|
||||
/// </summary>
|
||||
private const int SIGNIFICANT_DIGITS = 1000;
|
||||
public static double[] GetTiltDegreesEU(short[] tiltSensorADC, double[] tiltSensorCals)
|
||||
{
|
||||
return GetTiltDegreesEU(tiltSensorADC, tiltSensorCals, TiltAxes.IXYZ, 3, new float[2] { 0F, 0F });
|
||||
}
|
||||
|
||||
public static double[] GetTiltDegreesEU(short[] tiltSensorADC, double[] tiltSensorCals, TiltAxes axes, int ignoredAxis, float[] mountOffsetAxis)
|
||||
{
|
||||
var mountOffsetAxisOne = mountOffsetAxis[0];
|
||||
var mountOffsetAxisTwo = mountOffsetAxis[1];
|
||||
double tiltSensorAxisXDegreesPre = 0D;
|
||||
double tiltSensorAxisYDegreesPre = 0D;
|
||||
double tiltSensorAxisZDegreesPre = 0D;
|
||||
|
||||
if (ignoredAxis == 0) { ignoredAxis = 3; }
|
||||
|
||||
int dominantAxis = GetDominantTiltAxis(tiltSensorADC);
|
||||
double[] gains = GetTiltGains(tiltSensorCals);
|
||||
double[] zeroData = GetTiltZeroData(tiltSensorADC, tiltSensorCals, dominantAxis);
|
||||
|
||||
var xFactor = 1;
|
||||
var yFactor = 1;
|
||||
var zFactor = 1;
|
||||
switch (axes)
|
||||
{
|
||||
case TiltAxes.IXIYIZ:
|
||||
case TiltAxes.IXIYZ:
|
||||
case TiltAxes.IXIZIY:
|
||||
case TiltAxes.IXIZY:
|
||||
case TiltAxes.IXYIZ:
|
||||
case TiltAxes.IXYZ:
|
||||
case TiltAxes.IXZIY:
|
||||
case TiltAxes.IXZY:
|
||||
case TiltAxes.IYIXIZ:
|
||||
case TiltAxes.IYIXZ:
|
||||
case TiltAxes.IYIZIX:
|
||||
case TiltAxes.IYZIX:
|
||||
case TiltAxes.IZIXIY:
|
||||
case TiltAxes.IZIXY:
|
||||
case TiltAxes.IZIYIX:
|
||||
case TiltAxes.IZYIX:
|
||||
case TiltAxes.YIXIZ:
|
||||
case TiltAxes.YIXZ:
|
||||
case TiltAxes.YIZIX:
|
||||
case TiltAxes.YZIX:
|
||||
case TiltAxes.ZIXIY:
|
||||
case TiltAxes.ZIXY:
|
||||
case TiltAxes.ZIYIX:
|
||||
case TiltAxes.ZYIX:
|
||||
xFactor = -1;
|
||||
break;
|
||||
}
|
||||
switch (axes)
|
||||
{
|
||||
case TiltAxes.IXIYIZ:
|
||||
case TiltAxes.IXIYZ:
|
||||
case TiltAxes.IXIZIY:
|
||||
case TiltAxes.IXZIY:
|
||||
case TiltAxes.IYIXIZ:
|
||||
case TiltAxes.IYIXZ:
|
||||
case TiltAxes.IYIZIX:
|
||||
case TiltAxes.IYIZX:
|
||||
case TiltAxes.IYXIZ:
|
||||
case TiltAxes.IYXZ:
|
||||
case TiltAxes.IYZIX:
|
||||
case TiltAxes.IYZX:
|
||||
case TiltAxes.IZIXIY:
|
||||
case TiltAxes.IZIYIX:
|
||||
case TiltAxes.IZIYX:
|
||||
case TiltAxes.IZXIY:
|
||||
case TiltAxes.XIYIZ:
|
||||
case TiltAxes.XIYZ:
|
||||
case TiltAxes.XIZIY:
|
||||
case TiltAxes.XZIY:
|
||||
case TiltAxes.ZIXIY:
|
||||
case TiltAxes.ZIYIX:
|
||||
case TiltAxes.ZIYX:
|
||||
case TiltAxes.ZXIY:
|
||||
yFactor = -1;
|
||||
break;
|
||||
}
|
||||
switch (axes)
|
||||
{
|
||||
case TiltAxes.IXIYIZ:
|
||||
case TiltAxes.IXIZIY:
|
||||
case TiltAxes.IXIZY:
|
||||
case TiltAxes.IXYIZ:
|
||||
case TiltAxes.IYIXIZ:
|
||||
case TiltAxes.IYIZIX:
|
||||
case TiltAxes.IYIZX:
|
||||
case TiltAxes.IYXIZ:
|
||||
case TiltAxes.IZIXIY:
|
||||
case TiltAxes.IZIXY:
|
||||
case TiltAxes.IZIYIX:
|
||||
case TiltAxes.IZIYX:
|
||||
case TiltAxes.IZXIY:
|
||||
case TiltAxes.IZXY:
|
||||
case TiltAxes.IZYIX:
|
||||
case TiltAxes.IZYX:
|
||||
case TiltAxes.XIYIZ:
|
||||
case TiltAxes.XIZIY:
|
||||
case TiltAxes.XIZY:
|
||||
case TiltAxes.XYIZ:
|
||||
case TiltAxes.YIXIZ:
|
||||
case TiltAxes.YIZIX:
|
||||
case TiltAxes.YIZX:
|
||||
case TiltAxes.YXIZ:
|
||||
zFactor = -1;
|
||||
break;
|
||||
}
|
||||
var simplifiedAxes = SimplifyTiltAxes(axes);
|
||||
switch (simplifiedAxes)
|
||||
{
|
||||
case TiltAxesSimple.XYZ:
|
||||
double Sx = xFactor * ((tiltSensorADC[0] - zeroData[0]) / gains[0]);
|
||||
double Sy = yFactor * ((tiltSensorADC[1] - zeroData[1]) / gains[1]);
|
||||
double Sz = zFactor * ((tiltSensorADC[2] - zeroData[2]) / gains[2]);
|
||||
double SG = Math.Sqrt(Math.Pow(Sx, 2) + Math.Pow(Sy, 2) + Math.Pow(Sz, 2));
|
||||
switch (ignoredAxis)
|
||||
{
|
||||
case 1:
|
||||
tiltSensorAxisXDegreesPre = double.NaN;
|
||||
tiltSensorAxisYDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sy, SG) + mountOffsetAxisTwo) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisZDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sz, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
break;
|
||||
case 2:
|
||||
tiltSensorAxisXDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sx, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisYDegreesPre = double.NaN;
|
||||
tiltSensorAxisZDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sz, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
break;
|
||||
case 3:
|
||||
tiltSensorAxisXDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sx, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisYDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sy, SG) + mountOffsetAxisTwo) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisZDegreesPre = double.NaN;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case TiltAxesSimple.XZY:
|
||||
Sx = xFactor * ((tiltSensorADC[0] - zeroData[0]) / gains[0]);
|
||||
Sz = zFactor * ((tiltSensorADC[1] - zeroData[1]) / gains[1]);
|
||||
Sy = yFactor * ((tiltSensorADC[2] - zeroData[2]) / gains[2]);
|
||||
SG = Math.Sqrt(Math.Pow(Sx, 2) + Math.Pow(Sy, 2) + Math.Pow(Sz, 2));
|
||||
switch (ignoredAxis)
|
||||
{
|
||||
case 1:
|
||||
tiltSensorAxisXDegreesPre = double.NaN;
|
||||
tiltSensorAxisZDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sz, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisYDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sy, SG) + mountOffsetAxisTwo) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
break;
|
||||
case 2:
|
||||
tiltSensorAxisXDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sx, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisZDegreesPre = double.NaN;
|
||||
tiltSensorAxisYDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sy, SG) + mountOffsetAxisTwo) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
break;
|
||||
case 3:
|
||||
tiltSensorAxisXDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sx, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisZDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sz, SG) + mountOffsetAxisTwo) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisYDegreesPre = double.NaN;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case TiltAxesSimple.YXZ:
|
||||
Sy = yFactor * ((tiltSensorADC[0] - zeroData[0]) / gains[0]);
|
||||
Sx = xFactor * ((tiltSensorADC[1] - zeroData[1]) / gains[1]);
|
||||
Sz = zFactor * ((tiltSensorADC[2] - zeroData[2]) / gains[2]);
|
||||
SG = Math.Sqrt(Math.Pow(Sx, 2) + Math.Pow(Sy, 2) + Math.Pow(Sz, 2));
|
||||
switch (ignoredAxis)
|
||||
{
|
||||
case 1:
|
||||
tiltSensorAxisYDegreesPre = double.NaN;
|
||||
tiltSensorAxisXDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sx, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisZDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sz, SG) + mountOffsetAxisTwo) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
break;
|
||||
case 2:
|
||||
tiltSensorAxisYDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sy, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisXDegreesPre = double.NaN;
|
||||
tiltSensorAxisZDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sz, SG) + mountOffsetAxisTwo) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
break;
|
||||
case 3:
|
||||
tiltSensorAxisYDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sy, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisXDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sx, SG) + mountOffsetAxisTwo) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisZDegreesPre = double.NaN;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case TiltAxesSimple.YZX:
|
||||
Sy = yFactor * ((tiltSensorADC[0] - zeroData[0]) / gains[0]);
|
||||
Sz = zFactor * ((tiltSensorADC[1] - zeroData[1]) / gains[1]);
|
||||
Sx = xFactor * ((tiltSensorADC[2] - zeroData[2]) / gains[2]);
|
||||
SG = Math.Sqrt(Math.Pow(Sx, 2) + Math.Pow(Sy, 2) + Math.Pow(Sz, 2));
|
||||
switch (ignoredAxis)
|
||||
{
|
||||
case 1:
|
||||
tiltSensorAxisYDegreesPre = double.NaN;
|
||||
tiltSensorAxisZDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sz, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisXDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sx, SG) + mountOffsetAxisTwo) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
break;
|
||||
case 2:
|
||||
tiltSensorAxisYDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sy, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisZDegreesPre = double.NaN;
|
||||
tiltSensorAxisXDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sx, SG) + mountOffsetAxisTwo) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
break;
|
||||
case 3:
|
||||
tiltSensorAxisYDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sy, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisZDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sz, SG) + mountOffsetAxisTwo) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisXDegreesPre = double.NaN;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case TiltAxesSimple.ZXY:
|
||||
Sz = zFactor * ((tiltSensorADC[0] - zeroData[0]) / gains[0]);
|
||||
Sx = xFactor * ((tiltSensorADC[1] - zeroData[1]) / gains[1]);
|
||||
Sy = yFactor * ((tiltSensorADC[2] - zeroData[2]) / gains[2]);
|
||||
SG = Math.Sqrt(Math.Pow(Sx, 2) + Math.Pow(Sy, 2) + Math.Pow(Sz, 2));
|
||||
switch (ignoredAxis)
|
||||
{
|
||||
case 1:
|
||||
tiltSensorAxisZDegreesPre = double.NaN;
|
||||
tiltSensorAxisXDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sx, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisYDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sy, SG) + mountOffsetAxisTwo) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
break;
|
||||
case 2:
|
||||
tiltSensorAxisZDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sz, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisXDegreesPre = double.NaN;
|
||||
tiltSensorAxisYDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sy, SG) + mountOffsetAxisTwo) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
break;
|
||||
case 3:
|
||||
tiltSensorAxisZDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sz, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisXDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sx, SG) + mountOffsetAxisTwo) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisYDegreesPre = double.NaN;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case TiltAxesSimple.ZYX:
|
||||
Sz = zFactor * ((tiltSensorADC[0] - zeroData[0]) / gains[0]);
|
||||
Sy = yFactor * ((tiltSensorADC[1] - zeroData[1]) / gains[1]);
|
||||
Sx = xFactor * ((tiltSensorADC[2] - zeroData[2]) / gains[2]);
|
||||
SG = Math.Sqrt(Math.Pow(Sx, 2) + Math.Pow(Sy, 2) + Math.Pow(Sz, 2));
|
||||
switch (ignoredAxis)
|
||||
{
|
||||
case 1:
|
||||
tiltSensorAxisZDegreesPre = double.NaN;
|
||||
tiltSensorAxisYDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sy, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisXDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sx, SG) + mountOffsetAxisTwo) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
break;
|
||||
case 2:
|
||||
tiltSensorAxisZDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sz, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisYDegreesPre = double.NaN;
|
||||
tiltSensorAxisXDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sx, SG) + mountOffsetAxisTwo) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
break;
|
||||
case 3:
|
||||
tiltSensorAxisZDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sz, SG) + mountOffsetAxisOne) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisYDegreesPre = Math.Round((DegreesFromADC.GetDegrees(Sy, SG) + mountOffsetAxisTwo) * SIGNIFICANT_DIGITS) / SIGNIFICANT_DIGITS; //1 decimal place
|
||||
tiltSensorAxisXDegreesPre = double.NaN;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
return new double[] { tiltSensorAxisXDegreesPre, tiltSensorAxisYDegreesPre, tiltSensorAxisZDegreesPre };
|
||||
}
|
||||
|
||||
public static double[] GetTiltZeroData(short[] tiltSensorADC, double[] tiltSensorCals, int dominantAxis)
|
||||
{
|
||||
double[] zeroData = new double[]
|
||||
{
|
||||
tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_2_ZeroAxis1],
|
||||
tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_8_ZeroAxis2],
|
||||
tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_14_ZeroAxis3]
|
||||
};
|
||||
|
||||
bool positivePolarity = (tiltSensorADC[dominantAxis] > 0);
|
||||
switch (dominantAxis)
|
||||
{
|
||||
case 0:
|
||||
zeroData[1] = (positivePolarity) ? tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_9_ZeroDomAxis1PosAxis2] : tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_10_ZeroDomAxis1NegAxis2];
|
||||
zeroData[2] = (positivePolarity) ? tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_15_ZeroDomAxis1PosAxis3] : tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_16_ZeroDomAxis1NegAxis3];
|
||||
break;
|
||||
case 1:
|
||||
zeroData[0] = (positivePolarity) ? tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_3_ZeroDomAxis2PosAxis1] : tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_4_ZeroDomAxis2NegAxis1];
|
||||
zeroData[2] = (positivePolarity) ? tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_17_ZeroDomAxis2PosAxis3] : tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_18_ZeroDomAxis2NegAxis3];
|
||||
break;
|
||||
case 2:
|
||||
zeroData[0] = (positivePolarity) ? tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_5_ZeroDomAxis3PosAxis1] : tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_6_ZeroDomAxis3NegAxis1];
|
||||
zeroData[1] = (positivePolarity) ? tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_11_ZeroDomAxis3PosAxis2] : tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_12_ZeroDomAxis3NegAxis2];
|
||||
break;
|
||||
}
|
||||
|
||||
return zeroData;
|
||||
}
|
||||
|
||||
public static int GetDominantTiltAxis(short[] tiltSensorADC)
|
||||
{
|
||||
return (tiltSensorADC.Max() < Math.Abs(tiltSensorADC.Min())) ? Array.IndexOf(tiltSensorADC, tiltSensorADC.Min()) : Array.IndexOf(tiltSensorADC, tiltSensorADC.Max());
|
||||
}
|
||||
|
||||
public static double[] GetTiltGains(double[] tiltSensorCals)
|
||||
{
|
||||
double[] gains = new double[3];
|
||||
gains[0] = tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_1_GainAxis1] == 0D ? 1D : tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_1_GainAxis1];
|
||||
gains[1] = tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_7_GainAxis2] == 0D ? 1D : tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_7_GainAxis2];
|
||||
gains[2] = tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_13_GainAxis3] == 0D ? 1D : tiltSensorCals[(int)TiltSensorCalAttributes.TILTSENSOR_CAL_13_GainAxis3];
|
||||
return gains;
|
||||
}
|
||||
|
||||
public enum AxisLabel
|
||||
{
|
||||
X = 0,
|
||||
Y = 1,
|
||||
Z = 2,
|
||||
}
|
||||
|
||||
public static AxisLabel[] GetAxisLabelFromTiltAxes(TiltAxes tiltAxes)
|
||||
{
|
||||
var simplifiedTiltAxes = SimplifyTiltAxes(tiltAxes);
|
||||
var axesLabelChars = simplifiedTiltAxes.ToString().ToCharArray();
|
||||
var rv = new AxisLabel[axesLabelChars.Length];
|
||||
for (int i = 0; i < rv.Length; i++)
|
||||
{
|
||||
rv[i] = (AxisLabel)Enum.Parse(typeof(AxisLabel), axesLabelChars[i].ToString());
|
||||
}
|
||||
return rv;
|
||||
}
|
||||
|
||||
public static AxisLabel[] GetAxisLabelFromTiltAxes(string tiltAxes)
|
||||
{
|
||||
return GetAxisLabelFromTiltAxes(GetTiltAxesFromString(tiltAxes));
|
||||
}
|
||||
|
||||
public static string ConvertBoolToInvertString(bool isInverted)
|
||||
{
|
||||
return isInverted ? "I" : "";
|
||||
}
|
||||
|
||||
public static string GetOrientationLabelFromAxisInfo(string[] axisLabels, bool[] invertAxis)
|
||||
{
|
||||
var sb = new StringBuilder();
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
sb.Append($"{ConvertBoolToInvertString(invertAxis[i])}{axisLabels[i]}");
|
||||
}
|
||||
|
||||
return sb.ToString();
|
||||
}
|
||||
|
||||
public static TiltAxes GetTiltAxesFromAxisInfo(string[] axisLabels, bool[] invertAxis)
|
||||
{
|
||||
return GetTiltAxesFromString(GetOrientationLabelFromAxisInfo(axisLabels, invertAxis));
|
||||
}
|
||||
|
||||
public static float[] GetMountOffsetsOrTargetsFromAxisInfo(float[] perAxisValue, int axisToIgnore)
|
||||
{
|
||||
var rv = new float[2];
|
||||
switch (axisToIgnore)
|
||||
{
|
||||
case 1:
|
||||
rv[0] = perAxisValue[1];
|
||||
rv[1] = perAxisValue[2];
|
||||
break;
|
||||
case 2:
|
||||
rv[0] = perAxisValue[0];
|
||||
rv[1] = perAxisValue[2];
|
||||
break;
|
||||
case 3:
|
||||
rv[0] = perAxisValue[0];
|
||||
rv[1] = perAxisValue[1];
|
||||
break;
|
||||
}
|
||||
return rv;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user