595 lines
31 KiB
C#
595 lines
31 KiB
C#
|
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;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|