void AccelerometerMeasurementHandler(AccelerometerData measurement)
{
//Tracer.Trace("TrackRoamerBrickProximityBoardService::AccelerometerMeasurementHandler()");
try
{
_state.LastSampleTimestamp = new DateTime(measurement.TimeStamp);
_state.MostRecentAccelerometer = new AccelerometerDataDssSerializable(measurement);
_state.LinkState = "receiving Accelerometer Data";
//
// Inform subscribed services that the state has changed.
//
_submgrPort.Post(new submgr.Submit(_state, DsspActions.ReplaceRequest));
UpdateAccelerometerData usd = new UpdateAccelerometerData();
usd.Body = _state.MostRecentAccelerometer;
base.SendNotification <UpdateAccelerometerData>(_submgrPort, usd);
}
catch (Exception e)
{
_state.LinkState = "Error while receiving Accelerometer Data";
LogError(e);
}
}
static void DataUpdated(object sender, AccelerometerReadingChangedEventArgs e)
{
var reading = e.Reading;
var data = new AccelerometerData(reading.AccelerationX, reading.AccelerationY, reading.AccelerationZ);
OnChanged(data);
}
public async Task <IHttpActionResult> Post(AccelerometerData accdata)
{
await Initilization;
var response = await _repo.CreateAccData(accdata);
return(Ok(response.Resource));
}
public void Accelerometer_Comparison(
float x1,
float y1,
float z1,
float x2,
float y2,
float z2,
bool equals)
{
var data1 = new AccelerometerData(x1, y1, z1);
var data2 = new AccelerometerData(x2, y2, z2);
if (equals)
{
Assert.True(data1.Equals(data2));
Assert.True(data1 == data2);
Assert.False(data1 != data2);
Assert.Equal(data1, data2);
Assert.Equal(data1.GetHashCode(), data2.GetHashCode());
}
else
{
Assert.False(data1.Equals(data2));
Assert.False(data1 == data2);
Assert.True(data1 != data2);
Assert.NotEqual(data1, data2);
Assert.NotEqual(data1.GetHashCode(), data2.GetHashCode());
}
}
void Accelerometer_ReadingChanged(object sender, AccelerometerChangedEventArgs e)
{
AccelerometerData data = e.Reading;
valX = data.Acceleration.X;
valY = data.Acceleration.Y;
valZ = data.Acceleration.Z;
}
public AccelerometerDataDssSerializable(AccelerometerData accelerometerData)
{
TimeStamp = new DateTime(accelerometerData.TimeStamp);
accX = accelerometerData.accX;
accY = accelerometerData.accY;
accZ = accelerometerData.accZ;
}
void ISensorEventListener.OnSensorChanged(SensorEvent e)
{
if ((e?.Values?.Count ?? 0) < 3)
{
return;
}
var data = new AccelerometerData(e.Values[0] / gravity, e.Values[1] / gravity, e.Values[2] / gravity);
Accelerometer.OnChanged(data);
}
static void DataUpdated(CMAccelerometerData data, NSError error)
{
if (data == null)
{
return;
}
var field = data.Acceleration;
var accelData = new AccelerometerData(field.X, field.Y, field.Z);
OnChanged(accelData);
}
void DataUpdated(CMAccelerometerData data, NSError error)
{
if (data == null)
{
return;
}
#pragma warning disable CA1416 // https://github.com/xamarin/xamarin-macios/issues/14619
var field = data.Acceleration;
#pragma warning restore CA1416
var accelData = new AccelerometerData(field.X * -1, field.Y * -1, field.Z * -1);
OnChanged(accelData);
}
void ISensorEventListener.OnSensorChanged(SensorEvent?e)
{
var values = e?.Values ?? Array.Empty <float>();
if (values.Count < 3)
{
return;
}
var data = new AccelerometerData(values[0] / gravity, values[1] / gravity, values[2] / gravity);
ChangeHandler?.Invoke(data);
}
// ------------------------------------------------------------------------------------------
// Extract X,Y,Z accelerometer values, convert to a 3-member int array.
//
// The values are being converted to int * 100 (Z = 0.98 = 98) for
// easier processing.
// X & Y readings are converted to absolute (taking '-' out).
// ------------------------------------------------------------------------------------------
private void read_XYZ(AccelerometerData accelData)
{
//
aX = (Convert.ToInt16(Math.Abs(accelData.Acceleration.X) * 100));
aY = (Convert.ToInt16(Math.Abs(accelData.Acceleration.Y) * 100));
aZ = (Convert.ToInt16(accelData.Acceleration.Z * 100));
//
// if Z -, in recoil, skip ++ Only downward Z's are used ( < 98)
if ((aZ > 0) & (aZ <= 98))
{
// get the averages lined up
aTotalZ = aTotalZ + aZ;
// get total used read count
aTotalZ_cnt++;
}
}
private void SaveDeviceMotion()
{
NSOperationQueue queue = new NSOperationQueue();
cmManager.StartDeviceMotionUpdates(queue, async(CMDeviceMotion motion, NSError error) =>
{
//Repository<AccelerometerData> accRepo = new Repository<AccelerometerData>(SQLiteHelper.Instance.Connection);
Debug.WriteLine("DeviceMotionUpdate" + motion.Timestamp);
// motion.Timestamp, set to accelerometerdata. Time since boot. Convert to time? How?
DateTimeOffset timestamp = Utils.TimeSinceBootToDateTimeOffset(motion.Timestamp);
Debug.WriteLine("Compare timestamps vs now: {0} vs {1}", DateTimeOffset.Now, timestamp);
AccelerometerData userAcc = new AccelerometerData()
{
Type = AccelerometerType.User,
Timestamp = timestamp,
X = motion.UserAcceleration.X,
Y = motion.UserAcceleration.Y,
Z = motion.UserAcceleration.Z
};
AccelerometerData gravAcc = new AccelerometerData()
{
Type = AccelerometerType.Gravity,
Timestamp = timestamp,
X = motion.Gravity.X,
Y = motion.Gravity.Y,
Z = motion.Gravity.Z
};
Debug.WriteLine("Useracc " + userAcc);
Debug.WriteLine("gravacc " + gravAcc);
//await accRepo.Insert(userAcc);
//await accRepo.Insert(gravAcc);
});
}
/**
* @return <code>true</code> when the sensor was successfully enabled, <code>false</code> otherwise.
*/
public bool EnableAccelerometerSensor(/* final */ Context pContext, /* final */ IAccelerometerListener pAccelerometerListener, /* final */ AccelerometerSensorOptions pAccelerometerSensorOptions)
{
/* final */
SensorManager sensorManager = (SensorManager)pContext.GetSystemService(Context.SensorService);
//if (this.isSensorSupported(sensorManager, SensorType.Accelerometer))
if (this.IsSensorSupported(sensorManager, SensorType.Accelerometer))
{
this.mAccelerometerListener = pAccelerometerListener;
if (this.mAccelerometerData == null)
{
this.mAccelerometerData = new AccelerometerData();
}
this.RegisterSelfAsSensorListener(sensorManager, SensorType.Accelerometer, pAccelerometerSensorOptions.getSensorDelay());
return(true);
}
else
{
return(false);
}
}
internal static void OnChanged(AccelerometerData reading) => OnChanged(new AccelerometerChangedEventArgs(reading));
public AccelerometerChangedEventArgs(AccelerometerData reading) => Reading = reading;
private void OnValuesUpdated(AccelerometerData data)
{
ValuesUpdated?.Invoke(this, new AccelerometerEventArgs(data));
}
public SensorData(Vector3 v_accelerometer, Vector3 v_gyroscope, Vector3 v_magnetometer)
{
accelerometer = new AccelerometerData(v_accelerometer);
gyroscope = v_gyroscope;
magnetometer = new MagnetometerData(v_magnetometer, 0);
}
void AccelerometerDataHandler(AccelerometerData accelerometerData)
{
//Tracer.Trace("ProximityBoardCcrServiceCommander::AccelerometerDataHandler()");
_pbCommanderDataEventsPort.Post(accelerometerData);
}
private void button1_Click(object sender, EventArgs e)
{
if (bgwPlayingPresenter != null && bgwPlayingPresenter.IsBusy)
{
bgwPlayingPresenter.CancelAsync();
return;
}
if (accDeviationMagnitudeRenderer.verticalMarkersIndexesUsingXSpace.Count == 0)
{
return;
}
int currentDateTimeIndex = accDeviationMagnitudeRenderer.verticalMarkersIndexesUsingXSpace[0];
ThreadSafeOperations.ToggleButtonState(btnPlayStopAccAnimation, true, "STOP", true);
DoWorkEventHandler currDoWorkHandler = delegate(object currBGWsender, DoWorkEventArgs args)
{
BackgroundWorker selfworker = currBGWsender as BackgroundWorker;
SimpleVectorGraphics2D frontViewRenderer = new SimpleVectorGraphics2D(pbAccSideVisualization.Size);
frontViewRenderer.presentAxes = false;
frontViewRenderer.ptLeftTopSpaceCorner = new PointD(-5.0d, 0.0d);
frontViewRenderer.ptRightBottomSpaceCorner = new PointD(5.0d, -10.0d);
//frontViewRenderer.listVectorsToDraw.Add(DenseVector.OfEnumerable(new double[] {1.0d, -9.0d}));
//frontViewRenderer.listVectorsShift.Add(DenseVector.OfEnumerable(new double[] {0.0d, 0.0d}));
//frontViewRenderer.listVectColors.Add(new Bgr(Color.Blue));
//frontViewRenderer.Represent();
//ThreadSafeOperations.UpdatePictureBox(pbAccSideVisualization, frontViewRenderer.TheImage.Bitmap, false);
for (int i = currentDateTimeIndex; i < accDeviationMagnitudeRenderer.dvScatterXSpace[0].Count - 1; i++)
{
if (selfworker.CancellationPending)
{
break;
}
DateTime dtStartCalculate = DateTime.Now;
accDeviationMagnitudeRenderer.verticalMarkersList.Clear();
accDeviationMagnitudeRenderer.verticalMarkersList.Add(accDeviationMagnitudeRenderer.dvScatterXSpace[0][i]);
accDeviationMagnitudeRenderer.verticalMarkersIndexesUsingXSpace.Clear();
accDeviationMagnitudeRenderer.verticalMarkersIndexesUsingXSpace.Add(i);
accDeviationMagnitudeRenderer.RepresentMarkers();
ThreadSafeOperations.UpdatePictureBox(pbRepresentingDevMagnitude,
accDeviationMagnitudeRenderer.TheImage.Bitmap, false);
accDeviationAngleRenderer.verticalMarkersList.Clear();
accDeviationAngleRenderer.verticalMarkersList.Add(accDeviationAngleRenderer.dvScatterXSpace[0][i]);
accDeviationAngleRenderer.verticalMarkersIndexesUsingXSpace.Clear();
accDeviationAngleRenderer.verticalMarkersIndexesUsingXSpace.Add(i);
accDeviationAngleRenderer.RepresentMarkers();
ThreadSafeOperations.UpdatePictureBox(pbRepresentingDevAngle, accDeviationAngleRenderer.TheImage.Bitmap,
false);
accDeviationDirectionRenderer.verticalMarkersList.Clear();
accDeviationDirectionRenderer.verticalMarkersList.Add(accDeviationDirectionRenderer.dvScatterXSpace[0][i]);
accDeviationDirectionRenderer.verticalMarkersIndexesUsingXSpace.Clear();
accDeviationDirectionRenderer.verticalMarkersIndexesUsingXSpace.Add(i);
accDeviationDirectionRenderer.RepresentMarkers();
ThreadSafeOperations.UpdatePictureBox(pbRepresentingDevDirection, accDeviationDirectionRenderer.TheImage.Bitmap,
false);
frontViewRenderer.listVectorsToDraw.Clear();
frontViewRenderer.listVectorsShift.Clear();
frontViewRenderer.listVectColors.Clear();
//int globalArrayIndex = sensorsHistoryRepresentingScale + i-1;
DenseVector zeroValuesVect = DenseVector.OfEnumerable(new double[] { 0.0d, 0.0d });
DenseVector dvAccCalibrated = DenseVector.Create(3, idx => dmAccData[i, idx + 3]);
DenseVector dvAccCurrent = DenseVector.Create(3, idx => dmAccData[i, idx]);
AccelerometerData accCalibrated = new AccelerometerData(dvAccCalibrated);
accCalibrated.AccDoubleZ = -accCalibrated.AccDoubleZ;
AccelerometerData currAcc = new AccelerometerData(dvAccCurrent);
currAcc.AccDoubleZ = -currAcc.AccDoubleZ;
double koeffToRealGravity = 9.81d / accCalibrated.AccMagnitude;
accCalibrated = accCalibrated * koeffToRealGravity;
currAcc = currAcc * koeffToRealGravity;
AccelerometerData dAccCalibration = accCalibrated - (new AccelerometerData(0.0d, 0.0d, -9.81d));
currAcc = currAcc - dAccCalibration;
accCalibrated = accCalibrated - dAccCalibration;
frontViewRenderer.listVectorsToDraw.Add(DenseVector.OfEnumerable(new double[] { accCalibrated.xyProjectionMagnitude(), accCalibrated.AccDoubleZ }));
frontViewRenderer.listVectorsShift.Add(zeroValuesVect);
frontViewRenderer.listVectColors.Add(new Bgr(Color.Blue));
frontViewRenderer.listVectorsToDraw.Add(DenseVector.OfEnumerable(new double[] { currAcc.xyProjectionMagnitude(), currAcc.AccDoubleZ }));
frontViewRenderer.listVectorsShift.Add(zeroValuesVect);
frontViewRenderer.listVectColors.Add(new Bgr(Color.Red));
frontViewRenderer.Represent();
ThreadSafeOperations.UpdatePictureBox(pbAccSideVisualization, frontViewRenderer.TheImage.Bitmap, false);
TimeSpan dtCalculationTime = DateTime.Now - dtStartCalculate;
double timeToWait = (accDeviationDirectionRenderer.dvScatterXSpace[0][i + 1] -
accDeviationDirectionRenderer.dvScatterXSpace[0][i]) * 1000.0d
- (double)(dtCalculationTime.TotalMilliseconds);
if (timeToWait > 0)
{
//System.Windows.Forms.Application.DoEvents();
Thread.Sleep(Convert.ToInt32(timeToWait));
}
}
};
RunWorkerCompletedEventHandler currWorkCompletedHandler =
delegate(object currBGWCompletedSender, RunWorkerCompletedEventArgs args)
{
ThreadSafeOperations.ToggleButtonState(btnPlayStopAccAnimation, true, "PLAY", true);
};
bgwPlayingPresenter = new BackgroundWorker();
bgwPlayingPresenter.WorkerSupportsCancellation = true;
bgwPlayingPresenter.DoWork += currDoWorkHandler;
bgwPlayingPresenter.RunWorkerCompleted += currWorkCompletedHandler;
object[] BGWargs = new object[] { "", "" };
bgwPlayingPresenter.RunWorkerAsync(BGWargs);
}
private void btnReadData_Click(object sender, EventArgs e)
{
strLogFilesDirectory = tbLogFilesPath.Text;
if (bgwDataReader != null && bgwDataReader.IsBusy)
{
bgwDataReader.CancelAsync();
return;
}
ThreadSafeOperations.ToggleButtonState(btnReadData, true, "CANCEL", true);
DoWorkEventHandler currDoWorkHandler = delegate(object currBGWsender, DoWorkEventArgs args)
{
BackgroundWorker selfworker = currBGWsender as BackgroundWorker;
List <double> lTotalDataToAdd = new List <double>();
List <DateTime> lDateTimeList = new List <DateTime>();
DirectoryInfo dInfo = new DirectoryInfo(strLogFilesDirectory);
FileInfo[] fInfoArr = dInfo.GetFiles("*AccelerometerDataLog*.nc");
int fInfoCounter = 0;
foreach (FileInfo fInfo in fInfoArr)
{
if (selfworker.CancellationPending)
{
break;
}
fInfoCounter++;
selfworker.ReportProgress(Convert.ToInt32((double)(fInfoCounter - 1) / (double)fInfoArr.Length));
ThreadSafeOperations.SetText(lblStatusString, "reading " + fInfo.FullName, false);
Dictionary <string, object> dictDataLoaded = NetCDFoperations.ReadDataFromFile(fInfo.FullName);
string varNameDateTime = "DateTime";
if (dictDataLoaded.Keys.Contains("DateTime"))
{
varNameDateTime = "DateTime";
}
else if (dictDataLoaded.Keys.Contains("Datetime"))
{
varNameDateTime = "Datetime";
}
List <long> currFileDateTimeLongTicksList = new List <long>((dictDataLoaded[varNameDateTime] as long[]));
List <DateTime> currFileDateTimeList = currFileDateTimeLongTicksList.ConvertAll(longVal => new DateTime(longVal));
string varNameAccData = "AccelerometerData";
List <AccelerometerData> lAccData = AccelerometerData.OfDenseMatrix(dictDataLoaded[varNameAccData] as DenseMatrix);
List <double> lAccDataToAdd = lAccData.ConvertAll <double>(acc => acc.AccMagnitude);
lTotalDataToAdd.AddRange(lAccDataToAdd);
lDateTimeList.AddRange(currFileDateTimeList);
selfworker.ReportProgress(Convert.ToInt32((double)(fInfoCounter) / (double)fInfoArr.Length));
}
accSeriaData.AddSubseriaData(lTotalDataToAdd, lDateTimeList);
//теперь обработаем считанные данные
ThreadSafeOperations.SetText(lblStatusString, "basic acceleration data processing...", false);
accSubseries = accSeriaData.SplitWithTimeSpanCondition(dt => dt.TotalMilliseconds >= 1200);
accSubseries.RemoveAll(theSeria => theSeria.TotalSeriaDuration.TotalSeconds < 100);
List <double> listSeriesStats =
accSubseries.ConvertAll(timeseria => timeseria.TotalSeriaDuration.TotalSeconds);
DescriptiveStatistics stats = new DescriptiveStatistics(listSeriesStats);
string strToShow = "Acceleration data start time: " + accSubseries[0].StartTime.ToString("s") + Environment.NewLine;
strToShow += "Acceleration data end time: " + accSubseries[accSubseries.Count - 1].EndTime.ToString("s") + Environment.NewLine;
strToShow += "total chunks count: " + accSubseries.Count + Environment.NewLine;
strToShow += "mean chunk duration: " + stats.Mean.ToString("0.##e-00") + " s" + Environment.NewLine;
strToShow += "min chunk duration: " + stats.Minimum.ToString("0.##e-00") + " s" + Environment.NewLine;
strToShow += "max chunk duration: " + stats.Maximum.ToString("0.##e-00") + " s" + Environment.NewLine;
strToShow += "StdDev of chunk duration: " + stats.StandardDeviation.ToString("0.##e-00") + " s" + Environment.NewLine;
ThreadSafeOperations.SetTextTB(tbReportLog, strToShow, true);
List <GPSdata> lTotalGPSDataToAdd = new List <GPSdata>();
List <DateTime> lGPSDateTimeList = new List <DateTime>();
dInfo = new DirectoryInfo(strLogFilesDirectory);
fInfoArr = dInfo.GetFiles("*GPSDataLog*.nc");
fInfoCounter = 0;
foreach (FileInfo fInfo in fInfoArr)
{
if (selfworker.CancellationPending)
{
break;
}
fInfoCounter++;
selfworker.ReportProgress(Convert.ToInt32((double)(fInfoCounter - 1) / (double)fInfoArr.Length));
ThreadSafeOperations.SetText(lblStatusString, "reading " + fInfo.FullName, false);
Dictionary <string, object> dictDataLoaded = NetCDFoperations.ReadDataFromFile(fInfo.FullName);
string varNameDateTime = "DateTime";
if (dictDataLoaded.Keys.Contains("DateTime"))
{
varNameDateTime = "DateTime";
}
else if (dictDataLoaded.Keys.Contains("Datetime"))
{
varNameDateTime = "Datetime";
}
List <long> currFileDateTimeLongTicksList = new List <long>((dictDataLoaded[varNameDateTime] as long[]));
List <DateTime> currFileDateTimeList = currFileDateTimeLongTicksList.ConvertAll(longVal => new DateTime(longVal));
string varNameGPSData = "GPSdata";
List <GPSdata> lGPSData = GPSdata.OfDenseMatrix(dictDataLoaded[varNameGPSData] as DenseMatrix);
//List<double> lGPSDataToAdd = lGPSData.ConvertAll<double>(acc => acc.AccMagnitude);
lTotalGPSDataToAdd.AddRange(lGPSData);
lGPSDateTimeList.AddRange(currFileDateTimeList);
selfworker.ReportProgress(Convert.ToInt32((double)(fInfoCounter) / (double)fInfoArr.Length));
}
gpsSeriaData.AddSubseriaData(lTotalGPSDataToAdd, lGPSDateTimeList);
//теперь обработаем считанные данные
ThreadSafeOperations.SetText(lblStatusString, "basic GPS data processing...", false);
gpsSeriaData.RemoveValues(gpsDatum => ((gpsDatum.lat == 0.0d) && (gpsDatum.lon == 0.0d)));
gpsSeriaData.RemoveDuplicatedTimeStamps();
strToShow = Environment.NewLine + "GPS data start time: " + gpsSeriaData.StartTime.ToString("s") + Environment.NewLine;
strToShow += "GPS data end time: " + gpsSeriaData.EndTime.ToString("s") + Environment.NewLine;
ThreadSafeOperations.SetTextTB(tbReportLog, strToShow, true);
};
RunWorkerCompletedEventHandler currWorkCompletedHandler = delegate(object currBGWCompletedSender, RunWorkerCompletedEventArgs args)
{
ThreadSafeOperations.ToggleButtonState(btnReadData, true, "Read data", true);
};
ProgressChangedEventHandler bgwDataReader_ProgressChanged = delegate(object bgwDataReaderSender, ProgressChangedEventArgs args)
{
ThreadSafeOperations.UpdateProgressBar(prbReadingProcessingData, args.ProgressPercentage);
};
bgwDataReader = new BackgroundWorker();
bgwDataReader.WorkerSupportsCancellation = true;
bgwDataReader.WorkerReportsProgress = true;
bgwDataReader.DoWork += currDoWorkHandler;
bgwDataReader.RunWorkerCompleted += currWorkCompletedHandler;
bgwDataReader.ProgressChanged += bgwDataReader_ProgressChanged;
object[] BGWargs = new object[] { "", "" };
bgwDataReader.RunWorkerAsync(BGWargs);
}
// =================================================================================================================
#region DataReadyPort handlers
/// <summary>
/// This handler is activated by empty Recv messages coming to internal DataReadyPort.
/// The data comes here from pmFrameCompleteHandler
/// This is our opportunity to purge stale data from the queues.
/// The data posted to Responses port is then picked up by ProximityBoardCcrServiceCommander:SonarDataHandler() and others,
/// to be later passed to TrackRoamerBrickProximityBoard.
/// </summary>
/// <param name="recv"></param>
private void DataHandler(Recv recv)
{
/*
* if(_sonarDatas.Count > 1 || _directionDatas.Count > 1 || _accelDatas.Count > 1 || _proxDatas.Count > 1 || _parkingSensorsDatas.Count > 1)
* {
* // for debugging, log if queues get more than 1 piece of each data type:
* Tracer.Trace("ProximityBoardManager: DataHandler() sonars: " + _sonarDatas.Count + " compass: "******" accels: " + _accelDatas.Count + " proxs: " + _proxDatas.Count + " parking: " + _parkingSensorsDatas.Count);
* }
*/
if (_sonarDatas.Count > 0)
{
SonarData sonarData = null;
lock (_sonarDatas)
{
while (_sonarDatas.Count > 0)
{
sonarData = _sonarDatas.Last();
_sonarDatas.Clear();
}
}
if (sonarData != null)
{
MeasurementsPort.Post(sonarData);
}
}
if (_directionDatas.Count > 0)
{
DirectionData directionData = null;
lock (_directionDatas)
{
while (_directionDatas.Count > 0)
{
directionData = _directionDatas.Last();
_directionDatas.Clear();
}
}
if (directionData != null)
{
MeasurementsPort.Post(directionData);
}
}
if (_accelDatas.Count > 0)
{
AccelerometerData accelerometerData = null;
lock (_accelDatas)
{
while (_accelDatas.Count > 0)
{
accelerometerData = _accelDatas.Last();
_accelDatas.Clear();
}
}
if (accelerometerData != null)
{
MeasurementsPort.Post(accelerometerData);
}
}
if (_proxiDatas.Count > 0)
{
ProximityData proximityData = null;
lock (_proxiDatas)
{
while (_proxiDatas.Count > 0)
{
proximityData = _proxiDatas.Last();
_proxiDatas.Clear();
}
}
if (proximityData != null)
{
MeasurementsPort.Post(proximityData);
}
}
if (_parkingSensorsDatas.Count > 0)
{
ParkingSensorData psiData = null;
lock (_parkingSensorsDatas)
{
while (_parkingSensorsDatas.Count > 0)
{
psiData = _parkingSensorsDatas.Last();
_parkingSensorsDatas.Clear();
}
}
if (psiData != null)
{
MeasurementsPort.Post(psiData);
}
}
//Tracer.Trace("AnalogData: current=" + AnalogValue1 + " prev=" + AnalogValue1Prev);
// and last but not least - post AnalogData - only if any of the values changed:
if (AnalogValue1 != AnalogValue1Prev)
{
AnalogValue1Prev = AnalogValue1;
AnalogData analogData = new AnalogData()
{
analogValue1 = AnalogValue1,
TimeStamp = DateTime.Now.Ticks
};
MeasurementsPort.Post(analogData);
}
}
// =================================================================================================================
#region Processing Data Frame from LibPicSensors.ProximityModule - pmFrameCompleteHandler()
// This will be called whenever the data is read from the board:
private void pmFrameCompleteHandler(object sender, AsyncInputFrameArgs aira)
{
/*
* Tracer.Trace("Async frame arrived. " + now);
*
* StringBuilder frameValue = new StringBuilder();
*
* frameValue.AppendFormat("ProximityBoardManager: frame arrived: {0} ", aira.dPos1Mks);
* frameValue.AppendFormat("{0} ", aira.dPos2Mks);
* frameValue.AppendFormat("{0} ", aira.dPing1DistanceMm);
* frameValue.AppendFormat("{0}", aira.dPing2DistanceMm);
*
* Tracer.Trace(frameValue.ToString());
*/
bool haveData = false;
// IR Proximity sensors data:
ProximityData proxData = new ProximityData()
{
TimeStamp = aira.timestamp
};
proxData.setProximityData(aira.sensorsState.irbE1, aira.sensorsState.irbE2, aira.sensorsState.irbE3, aira.sensorsState.irbE4,
aira.sensorsState.irbO1, aira.sensorsState.irbO2, aira.sensorsState.irbO3, aira.sensorsState.irbO4);
lock (_proxiDatas)
{
_proxiDatas.Enqueue(proxData);
}
haveData = true; // allow DataPort.Post(new Recv()) to happen, with data already enqueued in the ...Datas
// accelerometer data:
AccelerometerData accData = new AccelerometerData()
{
TimeStamp = aira.timestamp
};
accData.setAccelerometerData(aira.sensorsState.accelX, aira.sensorsState.accelY, aira.sensorsState.accelZ);
lock (_accelDatas)
{
_accelDatas.Enqueue(accData);
}
haveData = true; // allow DataPort.Post(new Recv()) to happen, with data already enqueued in the ...Datas
// compass data:
DirectionData dirData = null;
if (aira.sensorsState.compassHeading >= 0.0d && aira.sensorsState.compassHeading <= 359.999999d)
{
dirData = new DirectionData()
{
heading = aira.sensorsState.compassHeading, TimeStamp = aira.timestamp
};
lock (_directionDatas)
{
_directionDatas.Enqueue(dirData);
}
haveData = true; // allow DataPort.Post(new Recv()) to happen, with data already enqueued in the ...Datas
}
ParkingSensorData psiData = null;
if (aira.sensorsState.parkingSensorsValid)
{
psiData = new ParkingSensorData()
{
TimeStamp = aira.timestamp
};
psiData.setParkingSensorData(aira.sensorsState);
lock (_parkingSensorsDatas)
{
_parkingSensorsDatas.Enqueue(psiData);
}
haveData = true; // allow DataPort.Post(new Recv()) to happen, with data already enqueued in the ...Datas
}
// frames that are marked "fromPingScanStop" feed the sonar sweep view controls:
if (aira.fromPingScanStop)
{
int angleRaw1 = (int)aira.dPos1Mks;
double distM1 = aira.dPing1DistanceM;
//Tracer.Trace(String.Format("angleRaw1 = {0} us", angleRaw1));
//Tracer.Trace(String.Format("distMm1 = {0} mm", distMm1));
int angleRaw2 = (int)aira.dPos2Mks;
double distM2 = aira.dPing2DistanceM;
//Tracer.Trace(String.Format("angleRaw2 = {0} us", angleRaw2));
//Tracer.Trace(String.Format("distMm2 = {0} mm", distMm2));
haveData = haveData && setSonarRayReading(angleRaw1, distM1, angleRaw2, distM2, aira.timestamp);
}
if (haveData)
{
AnalogValue1 = aira.sensorsState.analogValue1;
// just post internally an empty signal message, the receiver - DataHandler(Recv recv) should examine the ...Datas and forward data to Responses port
DataReadyPort.Post(new Recv());
}
}
public void OnSensorChanged(SensorEvent e)
{
var data = new AccelerometerData(e.Values[0], e.Values[1], e.Values[2]);
Accelerometer.OnChanged(data);
}
private void Represent()
{
Dictionary <string, object> dictDataToShow = NetCDFoperations.ReadDataFromFile(strLogFilename);
long[] arrDateTimeTicksValues = new long[] { 0 };
if (dictDataToShow.Keys.Contains("DateTime"))
{
arrDateTimeTicksValues = (long[])(dictDataToShow["DateTime"]);
}
else if (dictDataToShow.Keys.Contains("Datetime"))
{
arrDateTimeTicksValues = (long[])(dictDataToShow["Datetime"]);
}
else
{
MessageBox.Show("Couldn`t acces the DateTime field of the file " + strLogFilename, "",
MessageBoxButtons.OK, MessageBoxIcon.Warning);
return;
}
DenseVector dvSecondsVector = DenseVector.Create(arrDateTimeTicksValues.Length, i =>
{
DateTime dateTime1 = new DateTime(arrDateTimeTicksValues[i]);
TimeSpan dt = dateTime1 - dateTime1.Date;
return(dt.TotalSeconds);
});
dmAccData = (DenseMatrix)dictDataToShow["AccelerometerData"];
if (sensorsHistoryRepresentingScale < 86400)
{
double lastDateTimeSecondsValue = dvSecondsVector[dvSecondsVector.Count - 1];
int searchingIndex = dvSecondsVector.Count - 1;
for (int idx = dvSecondsVector.Count - 1; idx >= 0; idx--)
{
if (lastDateTimeSecondsValue - dvSecondsVector[idx] >= sensorsHistoryRepresentingScale)
{
searchingIndex = idx;
break;
}
}
dvSecondsVector =
(DenseVector)dvSecondsVector.SubVector(searchingIndex, dvSecondsVector.Count - searchingIndex);
dmAccData =
(DenseMatrix)dmAccData.SubMatrix(searchingIndex, dvSecondsVector.Count, 0, dmAccData.ColumnCount);
}
#region filter input data noise
List <DenseVector> dmAccDataFiltered = new List <DenseVector>();
Dictionary <string, DenseVector> dictCurrColumnFilteredData = new Dictionary <string, DenseVector>();
for (int col = 0; col < dmAccData.ColumnCount; col++)
{
DenseVector dvVectToFilter = (DenseVector)dmAccData.Column(col);
dictCurrColumnFilteredData = DataAnalysis.SavGolFilter(dvVectToFilter, dvSecondsVector, 6, 6, 0, 6);
dmAccDataFiltered.Add(dictCurrColumnFilteredData["values"]);
//dmAccData.SetColumn(col, dictCurrColumnFilteredData["values"]);
//dvSecondsVector = dictCurrColumnFilteredData["time"];
}
dvSecondsVector = dictCurrColumnFilteredData["time"];
dmAccData = (DenseMatrix)DenseMatrix.OfColumns(dvSecondsVector.Count, dmAccData.ColumnCount, dmAccDataFiltered);
#endregion filter input data noise
// надо рассчитать углы отклонения отдельно - по сглаженным данным и по данным гироскопа
DenseVector dvDataToShowDevAngleValue = (DenseVector)dmAccData.Column(6);
if (cbFilterData.Checked)
{
List <double> ac = new List <double>();
}
if (accDeviationAngleRenderer == null)
{
accDeviationAngleRenderer = new MultipleScatterAndFunctionsRepresentation(pbRepresentingDevAngle.Size);
accDeviationAngleRenderer.dvScatterXSpace.Add(dvSecondsVector);
accDeviationAngleRenderer.dvScatterFuncValues.Add(dvDataToShowDevAngleValue);
accDeviationAngleRenderer.scatterLineColors.Add(new Bgr(Color.Blue));
accDeviationAngleRenderer.scatterDrawingVariants.Add(SequencesDrawingVariants.polyline);
}
else
{
if (accDeviationAngleRenderer.xSpaceMin != dvSecondsVector.Max() - sensorsHistoryRepresentingScale)
{
accDeviationAngleRenderer.xSpaceMin = dvSecondsVector.Max() - sensorsHistoryRepresentingScale;
accDeviationAngleRenderer.dvScatterXSpace.Clear();
accDeviationAngleRenderer.dvScatterFuncValues.Clear();
accDeviationAngleRenderer.scatterLineColors.Clear();
accDeviationAngleRenderer.scatterDrawingVariants.Clear();
accDeviationAngleRenderer.dvScatterXSpace.Add(dvSecondsVector);
accDeviationAngleRenderer.dvScatterFuncValues.Add(dvDataToShowDevAngleValue);
accDeviationAngleRenderer.scatterLineColors.Add(new Bgr(Color.Blue));
accDeviationAngleRenderer.scatterDrawingVariants.Add(SequencesDrawingVariants.polyline);
}
}
accDeviationAngleRenderer.xSpaceMin = dvSecondsVector.Max() - sensorsHistoryRepresentingScale;
accDeviationAngleRenderer.Represent();
ThreadSafeOperations.UpdatePictureBox(pbRepresentingDevAngle, accDeviationAngleRenderer.TheImage.Bitmap);
DenseVector dvDataToShowAccMagnitudeDev = DenseVector.Create(dmAccData.RowCount, i =>
{
AccelerometerData currAccData = new AccelerometerData(dmAccData[i, 0], dmAccData[i, 1], dmAccData[i, 2]);
AccelerometerData calibrationAccData = new AccelerometerData(dmAccData[i, 3], dmAccData[i, 4], dmAccData[i, 5]);
return(currAccData.AccMagnitude - calibrationAccData.AccMagnitude);
});
if (accDeviationMagnitudeRenderer == null)
{
accDeviationMagnitudeRenderer = new MultipleScatterAndFunctionsRepresentation(pbRepresentingDevMagnitude.Size);
accDeviationMagnitudeRenderer.dvScatterXSpace.Add(dvSecondsVector);
accDeviationMagnitudeRenderer.dvScatterFuncValues.Add(dvDataToShowAccMagnitudeDev);
accDeviationMagnitudeRenderer.scatterLineColors.Add(new Bgr(Color.Blue));
accDeviationMagnitudeRenderer.scatterDrawingVariants.Add(SequencesDrawingVariants.polyline);
}
else
{
if (accDeviationMagnitudeRenderer.xSpaceMin != dvSecondsVector.Max() - sensorsHistoryRepresentingScale)
{
accDeviationMagnitudeRenderer.xSpaceMin = dvSecondsVector.Max() - sensorsHistoryRepresentingScale;
accDeviationMagnitudeRenderer.dvScatterXSpace.Clear();
accDeviationMagnitudeRenderer.dvScatterFuncValues.Clear();
accDeviationMagnitudeRenderer.scatterLineColors.Clear();
accDeviationMagnitudeRenderer.scatterDrawingVariants.Clear();
accDeviationMagnitudeRenderer.dvScatterXSpace.Add(dvSecondsVector);
accDeviationMagnitudeRenderer.dvScatterFuncValues.Add(dvDataToShowAccMagnitudeDev);
accDeviationMagnitudeRenderer.scatterLineColors.Add(new Bgr(Color.Blue));
accDeviationMagnitudeRenderer.scatterDrawingVariants.Add(SequencesDrawingVariants.polyline);
}
}
accDeviationMagnitudeRenderer.xSpaceMin = dvSecondsVector.Max() - sensorsHistoryRepresentingScale;
accDeviationMagnitudeRenderer.Represent();
ThreadSafeOperations.UpdatePictureBox(pbRepresentingDevMagnitude, accDeviationMagnitudeRenderer.TheImage.Bitmap);
AccelerometerData accCalibratedData = new AccelerometerData(dmAccData[0, 3], dmAccData[0, 4],
dmAccData[0, 5]);
double phiAngle = Math.Asin(accCalibratedData.xyProjectionMagnitude() / accCalibratedData.AccMagnitude);
double tmpL = accCalibratedData.AccMagnitude * Math.Sin(phiAngle) * Math.Cos(phiAngle);
double tmpLz = tmpL * Math.Sin(phiAngle);
double tmpLx = tmpL * Math.Cos(phiAngle) *
Math.Sqrt(1 +
accCalibratedData.AccY * accCalibratedData.AccY /
(accCalibratedData.AccX * accCalibratedData.AccX));
double tmpLy = tmpLx * accCalibratedData.AccY / accCalibratedData.AccX;
AccelerometerData unitaryAccVectorZeroAngle = new AccelerometerData(tmpLx, tmpLy, tmpLz);
unitaryAccVectorZeroAngle = unitaryAccVectorZeroAngle / unitaryAccVectorZeroAngle.AccMagnitude;
AccelerometerData unitaryAccVectorCalibratedAcceleration = accCalibratedData / (accCalibratedData.AccMagnitude);
DenseVector dvDataToShowAccDevDirection = DenseVector.Create(dmAccData.RowCount, i =>
{
AccelerometerData currAccData = new AccelerometerData(dmAccData[i, 0], dmAccData[i, 1], dmAccData[i, 2]);
AccelerometerData currAccDataProjectionPerpendicularToCalibratedAcc = currAccData -
unitaryAccVectorCalibratedAcceleration *
(currAccData * accCalibratedData / accCalibratedData.AccMagnitude);
double retAngle =
Math.Acos(currAccDataProjectionPerpendicularToCalibratedAcc * unitaryAccVectorZeroAngle /
currAccDataProjectionPerpendicularToCalibratedAcc.AccMagnitude);
AccelerometerData vectProduct = unitaryAccVectorZeroAngle ^
currAccDataProjectionPerpendicularToCalibratedAcc;
if (vectProduct * unitaryAccVectorCalibratedAcceleration > 0)
{
//значит угол лежит в пределах от 0 до PI - ничего не делаем
retAngle = retAngle + 0.0d;
}
else
{
//векторное произведение противоположно по направлению g0 - значит, угол лежит в диапазоне от Pi до 2Pi или от -PI до PI
retAngle = -retAngle;
}
return(retAngle);
});
if (accDeviationDirectionRenderer == null)
{
accDeviationDirectionRenderer = new MultipleScatterAndFunctionsRepresentation(pbRepresentingDevDirection.Size);
accDeviationDirectionRenderer.dvScatterXSpace.Add(dvSecondsVector);
accDeviationDirectionRenderer.dvScatterFuncValues.Add(dvDataToShowAccDevDirection);
accDeviationDirectionRenderer.scatterLineColors.Add(new Bgr(Color.Blue));
accDeviationDirectionRenderer.scatterDrawingVariants.Add(SequencesDrawingVariants.polyline);
}
else
{
if (accDeviationDirectionRenderer.xSpaceMin != dvSecondsVector.Max() - sensorsHistoryRepresentingScale)
{
accDeviationDirectionRenderer.xSpaceMin = dvSecondsVector.Max() - sensorsHistoryRepresentingScale;
accDeviationDirectionRenderer.dvScatterXSpace.Clear();
accDeviationDirectionRenderer.dvScatterFuncValues.Clear();
accDeviationDirectionRenderer.scatterLineColors.Clear();
accDeviationDirectionRenderer.scatterDrawingVariants.Clear();
accDeviationDirectionRenderer.dvScatterXSpace.Add(dvSecondsVector);
accDeviationDirectionRenderer.dvScatterFuncValues.Add(dvDataToShowAccDevDirection);
accDeviationDirectionRenderer.scatterLineColors.Add(new Bgr(Color.Blue));
accDeviationDirectionRenderer.scatterDrawingVariants.Add(SequencesDrawingVariants.polyline);
}
}
accDeviationDirectionRenderer.xSpaceMin = dvSecondsVector.Max() - sensorsHistoryRepresentingScale;
accDeviationDirectionRenderer.Represent();
ThreadSafeOperations.UpdatePictureBox(pbRepresentingDevDirection, accDeviationDirectionRenderer.TheImage.Bitmap);
}
internal AccelerometerChangedEventArgs(AccelerometerData reading) => Reading = reading;
void ISensorEventListener.OnSensorChanged(SensorEvent e)
{
var data = new AccelerometerData(e.Values[0] / gravity, e.Values[1] / gravity, e.Values[2] / gravity);
Accelerometer.OnChanged(data);
}
private void Accelerometer_ReadingChanged(object sender, AccelerometerChangedEventArgs e)
{
this.AccelerometerData = e.Reading;
}
