private double[,] ExtractData(int range, int sensType, int sensPos)
{
// sampwin - smooto -for
int size = samplewin.Count;
double[,] data = new double[size, 3];
List <Packet> smoothed = DataAnalysis.SmoothData(samplewin, range);
for (int i = 0; i < size; i++)
{
for (int j = 0; j < 3; j++)
{
switch (sensType)
{
case 0:
data[i, j] = smoothed[i].Sensors[sensPos].acc[j]; // dipende dal tipo sens
break;
case 1:
data[i, j] = smoothed[i].Sensors[sensPos].gyr[j]; // dipende dal tipo sens
break;
case 2:
data[i, j] = smoothed[i].Sensors[sensPos].mag[j]; // dipende dal tipo sens
break;
case 3:
data[i, j] = smoothed[i].Sensors[sensPos].q[j]; // dipende dal tipo sens
break;
}
}
}
return(data);
}
void DataProviderClientDisconnectedWriter(DataProvider dataProvider, TcpClient aClient)
{
if (this.InvokeRequired)
{
Invoke(new DataProviderClientDisconnected(DataProviderClientDisconnectedWriter), new object[] { dataProvider, aClient });
}
else
{
// azione di scrivere che il client e'connesso, non funziona
printToServerConsole("Client disconnected!\n");
// chiamo le chiamate per analizzare le azioni e scrivere
// moto stazionamento: acc x y z
double[,] acc = ExtractData(10, 0, 0);
String[] motoStaz = DataAnalysis.MotoStazionamento(acc, window);
//string path = Environment.GetFolderPath(Environment.SpecialFolder.MyDocuments);
//System.IO.File.WriteAllText(csvPath + @"\output.txt", "Moto stazionamento:");
//System.IO.File.WriteAllLines(csvPath + @"\output1.txt", motoStaz);
// do a Luca ...
// lat/sit/stand
acc = ExtractData(20, 0, 0);
int size = acc.GetLength(0);
double[] accX = new double[size];
for (int i = 0; i < size; i++)
{
accX[i] = acc[i, 0];
}
String[] layStandSit = DataAnalysis.LayStandSit(accX);
//System.IO.File.WriteAllText(csvPath + @"\output.txt", "lay Stand Sit:");
//System.IO.File.WriteAllLines(csvPath + @"\output2.txt", layStandSit);
// passo a Luca ...
// girata
double[,] mag = ExtractData(10, 2, 0);
double[] girate = DataAnalysis.Girata(mag);
//System.IO.File.WriteAllText(csvPath + @"\output3.txt", "Girate:");
String[] test = new String[girate.Count()];
for (int i = 0; i < girate.Count(); ++i)
{
test[i] = girate[i].ToString();
}
//System.IO.File.WriteAllLines(csvPath + @"\output3.txt", test);
// passo a Luca ...
DataWriter.DataWrite(motoStaz, layStandSit, girate, frequence, csvPath);
if (samplewin != null)
{
DataWriter.PrintPacketsToFile(samplewin, csvPath);
}
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//Dobbiamo prendere gia l' array con i dati del singolo sensore
//dobbiamo prendere in ingresso un parametro per la standard deviation che si sceglie dall' interfaccia
//Funzione MotoStazionamento, ritorna un array di Stringhe contentente lo stato di moto di ogni singola rilevazione
//"Fermo" oppure "In-Piedi"
public static String[] MotoStazionamento(double[,] providedData, int window)
{
double[] module = DataAnalysis.ComputeModules(providedData);
double[] sd = DataAnalysis.ComputeStandardDeviations(module, window);
double[] squared = DataAnalysis.ComputeSquare(sd, 50, 0.53, 0.4);
int sd_size = sd.Count();
String[] state = new String[sd_size];
for (int i = 0; i < sd_size; i++)
{
if (squared[i] <= 0.54)
{
state[i] = "Fermo";
}
else
{
state[i] = "Non-Fermo";
}
}
return(state);
}
private void EulerGraph()
{
int size = selected.Count;
double[,] data = new double[size, 4];
myPane.YAxis.Title.Text = "rad";
for (int i = 0; i < size; ++i)
{
for (int j = 0; j < 4; ++j)
{
data[i, j] = selected[i].Sensors[selectedSensor].q[j];
}
}
double[,] euler = DataAnalysis.ComputeEulerAngles(data);
double[,] eucont = DataAnalysis.RemoveDiscontinuities(euler);
double[] e0 = ExtractAngles(eucont, 0);
double[] e1 = ExtractAngles(eucont, 1);
double[] e2 = ExtractAngles(eucont, 2);
smooth = smoothing_cb.Checked;
if (!smooth)
{
DisplayEuler(e0, "Roll", true, Color.Blue);
DisplayEuler(e1, "Pitch", false, Color.Green);
DisplayEuler(e2, "Yaw", false, Color.Red);
}
else
{
DisplayEuler(e0, "Roll Smoothed", true, Color.Blue);
DisplayEuler(e1, "Pitch Smoothed", false, Color.Green);
DisplayEuler(e2, "Yaw Smoothed", false, Color.Red);
}
zedGraphControl1.AxisChange();
zedGraphControl1.Refresh();
}
// non deve prendere un array di double, ma meglio una lista di packet. altrimenti
// devo richiareextract data ogni volta che premo un qualsiasi bottone, meglio
// fare la chiamata allínterno di display data
// ricevo un solo parametro, list<Packet>
// all'interno del metodo richiamare la funzione extractData,
// e passare i volire della funzione richiesta
// pulire il grafico nel se non si devono visualizzare gli angoli di eulero
private void DisplayData(List <Packet> sampwin)
{
// controllo se e' attivo lo smoothing!!
if (smoothRange < 10)
{
checkBox3.Enabled = false;
cutOff_value.Enabled = false;
}
samplewin = sampwin;
smoothed = DataAnalysis.SmoothData(samplewin, smoothRange);
filtered = smoothing_cb.Checked ? smoothed : samplewin;
if (checkBox1.Checked)
{
selected = DataAnalysis.ComputeHighPass(filtered);
}
else
{
if (checkBox2.Checked)
{
selected = DataAnalysis.ComputeLowPass(filtered);
}
else
{
selected = filtered;
}
}
smooth = smoothing_cb.Checked;
double[,] data;
myPane.CurveList.Clear();
zedGraphControl1.Invalidate();
//if (willClearPane) { myPane.CurveList.Clear(); }
myPane.XAxis.Title.Text = "time (seconds)";
switch (selectedSensorType)
{
case 0:
//acc
//printToServerConsole("acc");
myPane.YAxis.Title.Text = "m/s²";
break;
case 1:
//gyr
//printToServerConsole("gry");
myPane.YAxis.Title.Text = "Rad/s²";
break;
case 2:
//mag
//printToServerConsole("mag");
myPane.YAxis.Title.Text = "Tesla";
break;
case 3:
//qua
//printToServerConsole("qua");
myPane.YAxis.Title.Text = "y";
break;
default:
//bohh
myPane.YAxis.Title.Text = "none";
break;
}
// ottengo i dati in base al sensore selezionato e posizione
data = ExtractData(selectedSensorType, selectedSensor);
double[] modules = DataAnalysis.ComputeModules(data);
PointPairList plist = new PointPairList();
switch (selectedGraph)
{
case 0:
// module
//printToServerConsole("mod");
plist.Clear();
double t = 0;
for (int i = 0; i < modules.Length; i++)
{
plist.Add(new PointPair(t, modules[i]));
t += 1.0 / frequence;
}
// problemi con i titoli
myPane.Title.Text = "Module";
if (!smooth)
{
myPane.AddCurve("Module", plist, Color.Blue, SymbolType.None);
}
else
{
myPane.AddCurve("Module Smoothed", plist, Color.Blue, SymbolType.None);
}
zedGraphControl1.AxisChange();
zedGraphControl1.Refresh();
break;
case 1:
// der
//printToServerConsole("dev");
plist.Clear();
t = 0;
double[] drv = DataAnalysis.ComputeDerivatives(modules, Int32.Parse(frequence_box.Text));
for (int i = 0; i < drv.Length; i++)
{
plist.Add(new PointPair(t, drv[i]));
t += 1.0 / frequence;
}
myPane.Title.Text = "Derivated";
if (!smooth)
{
myPane.AddCurve("Derivated", plist, Color.Blue, SymbolType.None);
}
else
{
myPane.AddCurve("Derivated Smoothed", plist, Color.Blue, SymbolType.None);
}
zedGraphControl1.AxisChange();
zedGraphControl1.Refresh();
break;
case 2:
// std
//printToServerConsole("std");
plist.Clear();
t = 0;
double[] dst0 = DataAnalysis.ComputeStandardDeviations(modules, smoothRange);
double epsilon = 0.4;
// sotto i 10 non squadra nulla di corretto perche' e' troppo instabile, sopra il 25 tentenna. tra 10 e 25 ok
/*if (smoothing_cb.Checked) {
* if (smoothRange >= 10 && smoothRange < 20) {
* cutOff = 0.53;
* }
* else if (smoothRange >= 20 && smoothRange < 30) {
* cutOff = 0.33;
* }
* else {
* cutOff = 0.21;
* }
* }
* else {
* cutOff = 1.5;
* }*/
double[] dst = checkBox3.Checked ? DataAnalysis.ComputeSquare(dst0, frequence, cutOff, epsilon) : dst0;
for (int i = 0; i < dst.Length; i++)
{
plist.Add(new PointPair(t, dst[i]));
t += 1.0 / frequence;
}
myPane.Title.Text = "Module Standard Deviation";
if (!smooth)
{
myPane.AddCurve("Module Standard Deviation", plist, Color.Blue, SymbolType.None);
}
else
{
myPane.AddCurve("Smoothed Module Standard Deviation", plist, Color.Blue, SymbolType.None);
}
zedGraphControl1.AxisChange();
zedGraphControl1.Refresh();
break;
case 3:
// euler
EulerGraph();
break;
case 4:
// dead
int size = samplewin.Count;
double[,] q0 = new double[size, 4];
for (int p = 0; p < size; p++)
{
for (int i = 0; i < 4; i++)
{
q0[p, i] = samplewin[p].Sensors[0].q[i];
}
}
double[,] acc = new double[size, 3];
for (int p = 0; p < size; p++)
{
for (int i = 0; i < 3; i++)
{
acc[p, i] = samplewin[p].Sensors[0].acc[i];
}
}
double[,] mag = new double[size, 3];
for (int p = 0; p < size; p++)
{
for (int i = 0; i < 3; i++)
{
mag[p, i] = samplewin[p].Sensors[0].mag[i];
}
}
PointPairList dead = DataAnalysis.ComputeDeadReckoning(q0, acc, mag, frequence, window);
PointPairList start = new PointPairList();
start.Add(dead.First());
PointPairList end = new PointPairList();
end.Add(dead.Last());
dead.RemoveAt(0);
dead.RemoveAt(dead.Count - 1);
GraphPane pane = zedGraphControl1.GraphPane;
pane.CurveList.Clear();
myPane.Title.Text = "Dead Reckoning";
myPane.YAxis.Title.Text = "m";
myPane.XAxis.Title.Text = "m";
pane.AddCurve("Start point", start, Color.Green, SymbolType.Square);
pane.AddCurve("Path", dead, Color.Black, SymbolType.None);
pane.AddCurve("End point", end, Color.Red, SymbolType.Circle);
zedGraphControl1.AxisChange();
zedGraphControl1.Refresh();
break;
case 5:
// arc tan
plist.Clear();
t = 0;
mag = ExtractData(2, selectedSensor);
size = mag.GetLength(0);
double[] magY = new double[size];
double[] magZ = new double[size];
for (int i = 0; i < size; i++)
{
magY[i] = mag[i, 1];
magZ[i] = mag[i, 2];
}
double[,] tan = DataAnalysis.FunzioneOrientamento(magY, magZ);
double[,] smtan = DataAnalysis.RemoveDiscontinuities(tan);
for (int i = 0; i < size; i++)
{
plist.Add(new PointPair(t, smtan[i, 0]));
t += 1.0 / frequence;
}
myPane.Title.Text = "Arc Tan(MagY/MagZ)";
if (!smooth)
{
myPane.AddCurve("Arc Tan(MagY/MagZ)", plist, Color.Blue, SymbolType.None);
}
else
{
myPane.AddCurve("Arc Tan(MagY/MagZ) Smoothed", plist, Color.Blue, SymbolType.None);
}
zedGraphControl1.AxisChange();
zedGraphControl1.Refresh();
break;
case 6:
// acc x
plist.Clear();
t = 0;
acc = ExtractData(0, selectedSensor);
size = acc.GetLength(0);
double[] accX = new double[size];
for (int i = 0; i < size; i++)
{
accX[i] = acc[i, 0];
}
double[] accX_opt = checkBox3.Checked ? DataAnalysis.ComputeSquare(accX, frequence, 7, 0.4) : accX;
for (int i = 0; i < size; i++)
{
plist.Add(new PointPair(t, accX_opt[i]));
t += 1.0 / frequence;
}
myPane.YAxis.Title.Text = "N/kg";
myPane.Title.Text = "Acc X";
if (!smooth)
{
myPane.AddCurve("Acc X", plist, Color.Blue, SymbolType.None);
}
else
{
myPane.AddCurve("Acc X Smoothed", plist, Color.Blue, SymbolType.None);
}
zedGraphControl1.AxisChange();
zedGraphControl1.Refresh();
// modulo acc x
// point pair list
break;
}
}
