public SixPointsCalibrationData()
{
CurrentResult = new SensorCalibrationData();
for (var i = 0; i < 10; i++)
{
_subSteps[i] = SubSteps.NotStarted;
_data[i] = new ConcurrentBag<RawSensorData>();
}
}
public static SensorCalibrationData Calibrate(IEnumerable<RawSensorData> measurepoints)
{
try
{
var count = measurepoints.Count();
double[,] accdata = new double[count, 3];
double[,] magdata = new double[count, 3];
int i = 0;
foreach (RawSensorData rsd in measurepoints)
{
accdata[i, 0] = Convert.ToSingle(rsd.Accelerometer.X);
accdata[i, 1] = Convert.ToSingle(rsd.Accelerometer.Y);
accdata[i, 2] = Convert.ToSingle(rsd.Accelerometer.Z);
magdata[i, 0] = Convert.ToSingle(rsd.Magnometer.X);
magdata[i, 1] = Convert.ToSingle(rsd.Magnometer.Y);
magdata[i, 2] = Convert.ToSingle(rsd.Magnometer.Z);
i++;
}
var accres = Calibrate(accdata, 0);
var magres = Calibrate(magdata, 0);
SensorCalibrationData sc = new SensorCalibrationData();
sc.AccelerometerBias.X = Convert.ToSingle(accres[0, 0]);
sc.AccelerometerBias.Y = Convert.ToSingle(accres[1, 0]);
sc.AccelerometerBias.Z = Convert.ToSingle(accres[2, 0]);
sc.AccelerometerGain.X = 1/Convert.ToSingle(accres[3, 0]);
sc.AccelerometerGain.Y = 1/Convert.ToSingle(accres[4, 0]);
sc.AccelerometerGain.Z = 1/Convert.ToSingle(accres[5, 0]);
sc.MagnometerBias.X = Convert.ToSingle(magres[0, 0]);
sc.MagnometerBias.Y = Convert.ToSingle(magres[1, 0]);
sc.MagnometerBias.Z = Convert.ToSingle(magres[2, 0]);
sc.MagnometerGain.X = 1/Convert.ToSingle(magres[3, 0]);
sc.MagnometerGain.Y = 1/Convert.ToSingle(magres[4, 0]);
sc.MagnometerGain.Z = 1/Convert.ToSingle(magres[5, 0]);
return sc;
}
catch
{
return null;
}
}
public override void ParseData(List<byte> data)
{
Data = SensorCalibrationData.FromBytes(data);
}
public SetSensorCalibration(SensorCalibrationData data)
: base(KFlyCommandType.SetSensorCalibration)
{
Data = data;
}
public SetSensorCalibration()
: base(KFlyCommandType.SetSensorCalibration)
{
Data = new SensorCalibrationData();
}
public static SensorCalibrationData FromBytes(List<byte> bytes)
{
var c = new SensorCalibrationData();
c.SetBytes(bytes);
return c;
}