void tcpMsgRecEvent(string[] receivedMessage)
{
String message = receivedMessage[0];
double[] sensorValues = new double[3];
int sensor_joint_ID = Int32.Parse(receivedMessage[1]);
if (globalDataSet.DebugMode)
{
Debug.WriteLine("receivedMessage: " + receivedMessage[0]);
}
aliveBit = true;
// remove x, y, z character in message string
message = message.Replace("x", "");
message = message.Replace("y", "");
message = message.Replace("z", "");
//message = message.Replace(".", ",");
// Split message to x, y, z and timestamp value
String[] messageData = message.Split(':');
Decimal[] messageDataAsDecimal = new Decimal[messageData.Length];
for (int i = 0; i < 3; i++)
{
sensorValues[i] = double.Parse(messageData[i], CultureInfo.InvariantCulture.NumberFormat);
//Debug.WriteLine("sensorValues: " + sensorValues[i]);
}
#region Calibration
if (sensor_joint_ID <= SENSOR_AMOUNT)
{
if ((!sensorCalibrationSet[sensor_joint_ID]) & (!buttonCalibrateSensors.Enabled))
{
double alpha = -9999;
// Check quadrant
if ((sensorValues[1] >= 0) & (sensorValues[2] < 0))
{
alpha = -(Math.PI / 2) + Math.Acos((sensorValues[1] * GRAVITATION_EARTH) / GRAVITATION_EARTH); // Quadrant 1
}
else if ((sensorValues[1] >= 0) & (sensorValues[2] > 0))
{
alpha = (Math.PI / 2) - Math.Acos((sensorValues[1] * GRAVITATION_EARTH) / GRAVITATION_EARTH); // Quadrant 2
}
if (alpha != -9999)
{
// Create rotation matrix around x axis
Rx_x[0] = 1;
Rx_x[1] = 0;
Rx_x[2] = 0;
Rx_y[0] = 0;
Rx_y[1] = Math.Cos(alpha);
Rx_y[2] = -Math.Sin(alpha);
Rx_z[0] = 0;
Rx_z[1] = Math.Sin(alpha);
Rx_z[2] = Math.Cos(alpha);
//for (int i = 0; i < 3; i++) Debug.WriteLine("Rx: " + Rx_x[i]);
//for (int i = 0; i < 3; i++) Debug.WriteLine("Rx: " + Rx_y[i]);
//for (int i = 0; i < 3; i++) Debug.WriteLine("Rx: " + Rx_z[i]);
// Set sensor calibration state to "calibration successfull"
sensorCalibrationSet[sensor_joint_ID] = true;
//Debug.WriteLine("Calibration finished - " + sensor_joint_ID + " -");
for (int i = 0; i < SENSOR_AMOUNT; i++)
{
if (!sensorCalibrationSet[i])
{
break;
}
else if (i == SENSOR_AMOUNT - 1)
{
helperFunctions.changeElementEnable(buttonCalibrateSensors, true);
}
}
}
else
{
helperFunctions.changeElementText(textBox_Info, "Calibration failed!", true);
helperFunctions.changeElementEnable(buttonCalibrateSensors, true);
}
}
// Modify sensor values with calibration data
gs_x[sensor_joint_ID] = ((Rx_x[0] * sensorValues[0]) + (Rx_x[1] * sensorValues[1]) + (Rx_x[2] * sensorValues[2])) * 90;
gs_y[sensor_joint_ID] = ((Rx_y[0] * sensorValues[0]) + (Rx_y[1] * sensorValues[1]) + (Rx_y[2] * sensorValues[2])) * 90;
gs_z[sensor_joint_ID] = ((Rx_z[0] * sensorValues[0]) + (Rx_z[1] * sensorValues[1]) + (Rx_z[2] * sensorValues[2])) * 90;
// Set calibration data only when checkbox is checked
if (checkBox_showCalibData.Checked)
{
// Convert double to decimal
messageDataAsDecimal[0] = Convert.ToDecimal(gs_x[sensor_joint_ID], CultureInfo.InvariantCulture.NumberFormat);
messageDataAsDecimal[1] = Convert.ToDecimal(gs_y[sensor_joint_ID], CultureInfo.InvariantCulture.NumberFormat);
messageDataAsDecimal[2] = Convert.ToDecimal(gs_z[sensor_joint_ID], CultureInfo.InvariantCulture.NumberFormat);
}
else
{
for (int i = 0; i < messageDataAsDecimal.Length - 1; i++)
{
messageDataAsDecimal[i] = Decimal.Parse(messageData[i], CultureInfo.InvariantCulture.NumberFormat) * 90;
}
}
// Get timestamp
messageDataAsDecimal[3] = Decimal.Parse(messageData[3], CultureInfo.InvariantCulture.NumberFormat);
}
else
{
messageDataAsDecimal[0] = 0;
messageDataAsDecimal[1] = 0;
messageDataAsDecimal[2] = 0;
helperFunctions.changeElementText(textBox_Info, "Failure in sensor id!", true);
}
#endregion
// Save to db
if (recordIsActive)
{
// Save write cycles to stop if max write cycle is reached
if (sampleStep == sampleTimeFactor)
{
if ((writeCycle < MAX_WRITE_CYCLE))
{
if (!notExecuted)
{
writeToDb(messageDataAsDecimal, sensor_joint_ID, dataSet_Db1);
}
if (sensor_joint_ID == firstSensorId)
{
sampleStep = DEFAULT_SAMPLE_TIME_FACTOR;
writeCycle++;
helperFunctions.changeElementText(labelSavedRows, writeCycle.ToString(), false);
}
}
else
{
writeCycle = 0;
recordIsActive = false;
savedRowCounter = 0;
notExecuted = true;
firstSensorId = -1;
button_recordToDb.BeginInvoke((MethodInvoker) delegate() { button_recordToDb.PerformClick(); });
helperFunctions.changeElementEnable(textBox_maxSamples, true);
helperFunctions.changeElementText(button_recordToDb, "Record to db", false);
MessageBox.Show("Measurement finished.");
}
}
else if (sensor_joint_ID == firstSensorId)
{
sampleStep++;
}
// Save first sensor id to calculate correct sample time
// Next sample is when the first sensor id comes again
if (notExecuted)
{
firstSensorId = sensor_joint_ID;
notExecuted = false;
}
}
// Show sensor values in graph
if ((checkBox_showGraphs.Checked) && (formCharts != null))
{
formCharts.setNewChartData(messageDataAsDecimal, sensor_joint_ID);
}
aliveBit = false;
if (globalDataSet.ShowProgramDuration)
{
Debug.WriteLine(globalDataSet.Timer_programExecution.ElapsedMilliseconds - globalDataSet.TimerValue);
}
}
