//accelerometer tests
//private void updateAccelerometer(IData data) {
// var accel = data.Value<Acceleration>();
// setText(accel.ToString(), 0);
//}
/// <summary>
/// Routine used for collection of Quaternions data of Sensors. In the end, calls a new routine to plot processed Data.
/// </summary>
/// <param name="data"> Quaternion data collected by the sensor</param>
/// <param name="sensorNumber"> number of the sensor used in project, starting by 0 for 1, 1 for 2 and etc </param>
private async Task quaternionStreamRoutine(IData data, int sensorNumber)
{
if (isRunning)
{
var quat = data.Value <Quaternion>();
var eulerAngles = calculateEulerAngles(quat);
var time = data.FormattedTimestamp.ToString();
var year = time.Substring(0, 4); var month = time.Substring(5, 2); var day = time.Substring(8, 2);
var hour = time.Substring(11, 2); var minute = time.Substring(14, 2); var second = time.Substring(17, 2);
var milli = time.Substring(20, 3);
// Store data point
if (record)
{
String newLine = string.Format("{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11}{12}", samples[0], year, month, day, hour, minute, second, milli, quat.W, quat.X, quat.Y, quat.Z, Environment.NewLine);
addPoint(newLine, sensorNumber);
newLine = string.Format("{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11}", samples[0], year, month, day, hour, minute, second, milli, eulerAngles.roll, eulerAngles.pitch, eulerAngles.yaw, Environment.NewLine);
addPointEuler(newLine, sensorNumber);
}
// Update counters
samples[sensorNumber]++;
freq[sensorNumber]++;
// Save reference quaternion
if (shouldCenter[sensorNumber])
{
refEuler[sensorNumber] = eulerAngles;
refQuats[sensorNumber] = quat;
shouldCenter[sensorNumber] = false;
centered[sensorNumber] = true;
}
double angle = 0;
double denom = 1;
if (centered[sensorNumber])
{
WindowsQuaternion a = convertToWindowsQuaternion(refQuats[sensorNumber]);
WindowsQuaternion b = convertToWindowsQuaternion(quat);
quat = centerData(refQuats[sensorNumber], quat);
eulerAngles = centerData(refEuler[sensorNumber], eulerAngles);
angle = (angleMode) ? 2 * Math.Acos(WindowsQuaternion.Dot(a, b) / (a.Length() * b.Length())) * (180 / Math.PI) : 0;
}
else if (angleMode)
{
angle = 2 * Math.Acos(quat.W) * (180 / Math.PI);
denom = Math.Sqrt(1 - Math.Pow(quat.W, 2));
denom = (denom < 0.001) ? 1 : denom; // avoid divide by zero type errors
}
angle = (angle > 180) ? 360 - angle : angle;
await CoreApplication.MainView.CoreWindow.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
plotValues(angle, quat, eulerAngles, denom, sensorNumber);
});
}
}
public static NQuaternion CustomQuatSlerp(NQuaternion v0, NQuaternion v1, float t)
{
v0 = NQuaternion.Normalize(v0);
v1 = NQuaternion.Normalize(v1);
float dot = NQuaternion.Dot(v0, v1);
if (dot < 0)
{
v1 = -v1;
dot = -dot;
}
const double DOT_THRESH = 0.9995;
if (dot > DOT_THRESH)
{
var diff = (v1 - v0);
diff.X *= t;
diff.Y *= t;
diff.Z *= t;
diff.W *= t;
return(NQuaternion.Normalize(v0 + diff));
}
// Since dot is in range [0, DOT_THRESHOLD], acos is safe
float theta_0 = (float)Math.Acos(dot); // theta_0 = angle between input vectors
float theta = theta_0 * t; // theta = angle between v0 and result
float sin_theta = (float)Math.Sin(theta); // compute this value only once
float sin_theta_0 = (float)Math.Sin(theta_0); // compute this value only once
float s0 = (float)Math.Cos(theta) - dot * sin_theta / sin_theta_0; // == sin(theta_0 - theta) / sin(theta_0)
float s1 = sin_theta / sin_theta_0;
var a = v0;
a.X *= s0;
a.Y *= s0;
a.Z *= s0;
a.W *= s0;
var b = v1;
b.X *= s1;
b.Y *= s1;
b.Z *= s1;
b.W *= s1;
return(a + b);
}
private async void streamSwitch_Toggled(object sender, RoutedEventArgs e)
{
double angle1 = 0;
double angle2 = 0;
if (streamSwitch.IsOn)
{
myStopWatch.Start();
isRunning = true;
Clear_Click(null, null);
samples[0] = 0;
samples[1] = 0;
sensorFusions = new ISensorFusionBosch[2];
sensorFusion = metawears[0].GetModule <ISensorFusionBosch>();
sensorFusion.Configure(); // default settings is NDoF mode with +/-16g acc range and 2000dps gyro range
await sensorFusion.Quaternion.AddRouteAsync(source => source.Stream(async data =>
{
if (isRunning)
{
var quat = data.Value <Quaternion>();
var time = data.FormattedTimestamp.ToString();
var year = time.Substring(0, 4);
var month = time.Substring(5, 2);
var day = time.Substring(8, 2);
var hour = time.Substring(11, 2);
var minute = time.Substring(14, 2);
var second = time.Substring(17, 2);
var milli = time.Substring(20, 3);
//print(time.ToString());
//print(time[0].ToString());
// Store data point
if (record)
{
String newLine = string.Format("{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11}{12}", samples[0], year, month, day, hour, minute, second, milli, quat.W, quat.X, quat.Y, quat.Z, Environment.NewLine);
addPoint(newLine, 0);
}
//var secs = myStopWatch.ElapsedMilliseconds * 0.001;
// Update counters
samples[0]++;
freq[0]++;
// Save reference quaternion
if (shouldCenter[0])
{
centerQuats[0] = quat;
shouldCenter[0] = false;
centered[0] = true;
}
double angle = 0;
double denom = 1;
if (centered[0])
{
WindowsQuaternion a = convertToWindowsQuaternion(centerQuats[0]);
WindowsQuaternion b = convertToWindowsQuaternion(quat);
quat = centerData(centerQuats[0], quat);
angle = (angleMode) ? 2 * Math.Acos(WindowsQuaternion.Dot(a, b) / (a.Length() * b.Length())) * (180 / Math.PI) : 0;
}
else if (angleMode)
{
angle = 2 * Math.Acos(quat.W) * (180 / Math.PI);
denom = Math.Sqrt(1 - Math.Pow(quat.W, 2));
denom = (denom < 0.001) ? 1 : denom; // avoid divide by zero type errors
}
angle = (angle > 250) ? 360 - angle : angle;
angle1 = angle;
await CoreApplication.MainView.CoreWindow.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
// Add values to plot
if ((bool)wCheckbox.IsChecked)
{
(model.Series[0] as LineSeries).Points.Add(new DataPoint(samples[0], (angleMode) ? angle : quat.W));
}
if ((bool)xyzCheckbox.IsChecked)
{
(model.Series[1] as LineSeries).Points.Add(new DataPoint(samples[0], quat.X / denom));
(model.Series[2] as LineSeries).Points.Add(new DataPoint(samples[0], quat.Y / denom));
(model.Series[3] as LineSeries).Points.Add(new DataPoint(samples[0], quat.Z / denom));
}
// Display values numerically
double[] values = { angleMode?angle : quat.W, (quat.X / denom), (quat.Y / denom), (quat.Z / denom) };
String[] labels = { angleMode ? "Angle: " : "W: ", "\nX: ", "\nY: ", "\nY: " };
String s = createOrientationText(labels, values);
setText(s, 0);
// Reset axes as needed
if ((bool)wCheckbox.IsChecked || (bool)xyzCheckbox.IsChecked)
{
model.InvalidatePlot(true);
//if (secs > MainViewModel.MAX_SECONDS)
if (samples.Max() > MainViewModel.MAX_DATA_SAMPLES)
{
model.Axes[1].Reset();
//model.Axes[1].Maximum = secs;
//model.Axes[1].Minimum = secs - MainViewModel.MAX_SECONDS;
model.Axes[1].Maximum = samples.Max();
model.Axes[1].Minimum = (samples.Max() - MainViewModel.MAX_DATA_SAMPLES);
model.Axes[1].Zoom(model.Axes[1].Minimum, model.Axes[1].Maximum);
}
}
});
}
}));
sensorFusion.Quaternion.Start();
sensorFusion.Start();
if (numBoards == 2)
{
sensorFusion2 = metawears[1].GetModule <ISensorFusionBosch>();
sensorFusion2.Configure(); // default settings is NDoF mode with +/-16g acc range and 2000dps gyro range
await sensorFusion2.Quaternion.AddRouteAsync(source => source.Stream(async data =>
{
var quat = data.Value <Quaternion>();
// Store data point
if (record)
{
String newLine = string.Format("{0},{1},{2},{3},{4}{5}", samples[0], quat.W, quat.X, quat.Y, quat.Z, Environment.NewLine);
addPoint(newLine, 1);
}
if (isRunning)
{
// Update counters
samples[1]++;
freq[1]++;
// Center quaternion if needed
if (shouldCenter[1])
{
centerQuats[1] = quat;
shouldCenter[1] = false;
centered[1] = true;
}
double angle = 0;
double denom = 1;
if (centered[1])
{
WindowsQuaternion a = convertToWindowsQuaternion(centerQuats[1]);
WindowsQuaternion b = convertToWindowsQuaternion(quat);
angle = 2 * Math.Acos(WindowsQuaternion.Dot(a, b) / (a.Length() * b.Length())) * (180 / Math.PI);;
quat = centerData(centerQuats[1], quat);
//System.Diagnostics.Debug.WriteLine(finalQuat.w.ToString() + " " + finalQuat.x.ToString() + " " + finalQuat.y.ToString() + " " + finalQuat.z.ToString());
}
else if (angleMode)
{
angle = 2 * Math.Acos(quat.W) * (180 / Math.PI);
denom = Math.Sqrt(1 - Math.Pow(quat.W, 2));
denom = (denom < 0.001) ? 1 : denom; // avoid divide by zero type errors
}
if (denom < 0.001)
{
denom = 1;
}
angle = (angle > 250) ? 360 - angle : angle;
angle2 = angle;
//var secs = myStopWatch.ElapsedMilliseconds * 0.001;
await CoreApplication.MainView.CoreWindow.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
// Display quaternion
String s;
// return;
if (angleSwitch.IsOn)
{
if ((bool)wCheckbox.IsChecked)
{
(model.Series[4] as LineSeries).Points.Add(new DataPoint(samples[0], angle));
}
if ((bool)xyzCheckbox.IsChecked)
{
(model.Series[5] as LineSeries).Points.Add(new DataPoint(samples[0], quat.X / denom));
(model.Series[6] as LineSeries).Points.Add(new DataPoint(samples[0], quat.Y / denom));
(model.Series[7] as LineSeries).Points.Add(new DataPoint(samples[0], quat.Z / denom));
}
s = "Angle: " + angle.ToString() + "\nX: " + (quat.X / denom).ToString() + "\nY: " + (quat.Y / denom).ToString() + "\nZ: " + (quat.Z / denom).ToString();
}
else
{
if ((bool)wCheckbox.IsChecked)
{
(model.Series[4] as LineSeries).Points.Add(new DataPoint(samples[0], quat.W));
}
if ((bool)xyzCheckbox.IsChecked)
{
(model.Series[5] as LineSeries).Points.Add(new DataPoint(samples[0], quat.X));
(model.Series[6] as LineSeries).Points.Add(new DataPoint(samples[0], quat.Y));
(model.Series[7] as LineSeries).Points.Add(new DataPoint(samples[0], quat.Z));
}
s = "W: " + quat.W.ToString() + "\nX: " + quat.X.ToString() + "\nY: " + quat.Y.ToString() + "\nZ: " + quat.Z.ToString();
}
setText(s, 1);
difference.Text = Math.Abs(angle1 - angle2).ToString();
});
}
}));
sensorFusion2.Quaternion.Start();
sensorFusion2.Start();
print("Sensor fusion should be running!");
}
InitFreqTimer();
Clear.Background = new SolidColorBrush(Windows.UI.Colors.Red);
}
else
{
isRunning = false;
sensorFusions[0] = sensorFusion;
sensorFusions[1] = sensorFusion2;
for (var i = 0; i < numBoards; i++)
{
sensorFusions[i].Stop();
sensorFusions[i].Quaternion.Stop();
metawears[i].TearDown();
freq[i] = 0;
}
timer1.Dispose();
myStopWatch.Stop();
myStopWatch.Reset();
}
}
