private void OnGetInclinometer(object sender, RoutedEventArgs e)
{
Inclinometer inclinometer = Inclinometer.GetDefault();
InclinometerReading reading = inclinometer.GetCurrentReading();
this.DefaultViewModel["InclinometerResult"] = GetInclinometerResult(reading);
}
/// <summary>
/// This is the dispatcher callback.
/// </summary>
/// <param name="sender"></param>
/// <param name="args"></param>
private void DisplayCurrentReading(object sender, object args)
{
InclinometerReading reading = _inclinometer.GetCurrentReading();
if (reading != null)
{
ScenarioOutput_X.Text = String.Format("{0,5:0.00}", reading.PitchDegrees);
ScenarioOutput_Y.Text = String.Format("{0,5:0.00}", reading.RollDegrees);
ScenarioOutput_Z.Text = String.Format("{0,5:0.00}", reading.YawDegrees);
switch (reading.YawAccuracy)
{
case MagnetometerAccuracy.Unknown:
ScenarioOutput_YawAccuracy.Text = "Unknown";
break;
case MagnetometerAccuracy.Unreliable:
ScenarioOutput_YawAccuracy.Text = "Unreliable";
break;
case MagnetometerAccuracy.Approximate:
ScenarioOutput_YawAccuracy.Text = "Approximate";
break;
case MagnetometerAccuracy.High:
ScenarioOutput_YawAccuracy.Text = "High";
break;
default:
ScenarioOutput_YawAccuracy.Text = "No data";
break;
}
}
}
/// <summary>
/// This is the event handler for ReadingChanged events.
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
async private void ReadingChanged(object sender, InclinometerReadingChangedEventArgs e)
{
await Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
InclinometerReading reading = e.Reading;
ScenarioOutput_X.Text = String.Format("{0,5:0.00}", reading.PitchDegrees);
ScenarioOutput_Y.Text = String.Format("{0,5:0.00}", reading.RollDegrees);
ScenarioOutput_Z.Text = String.Format("{0,5:0.00}", reading.YawDegrees);
switch (reading.YawAccuracy)
{
case MagnetometerAccuracy.Unknown:
ScenarioOutput_YawAccuracy.Text = "Unknown";
break;
case MagnetometerAccuracy.Unreliable:
ScenarioOutput_YawAccuracy.Text = "Unreliable";
break;
case MagnetometerAccuracy.Approximate:
ScenarioOutput_YawAccuracy.Text = "Approximate";
break;
case MagnetometerAccuracy.High:
ScenarioOutput_YawAccuracy.Text = "High";
break;
default:
ScenarioOutput_YawAccuracy.Text = "No data";
break;
}
});
}
private void activateSensorData()
{
Inclinometer inclinometer = Inclinometer.GetDefault();
if (inclinometer != null)
{
sensorRead = inclinometer.GetCurrentReading();
}
}
public static Matrix3D ToMatrix3D(this InclinometerReading i)
{
var m = new Matrix3D();
m.Rotate(new Quaternion(new Vector3D(0, 1, 0), i.RollDegrees));
m.Rotate(new Quaternion(new Vector3D(1, 0, 0), i.PitchDegrees));
m.Rotate(new Quaternion(new Vector3D(0, 0, 1), i.YawDegrees));
return(m);
}
static Inclination ConvertToInclination(InclinometerReading reading)
{
return(new Inclination
{
Roll = reading.RollDegrees,
Pitch = reading.PitchDegrees,
Yaw = reading.YawDegrees
});
}
private string GetInclinometerResult(InclinometerReading reading)
{
var sb = new StringBuilder();
sb.AppendFormat("pitch {0} degrees\n", reading.PitchDegrees);
sb.AppendFormat("roll {0} degrees\n", reading.RollDegrees);
sb.AppendFormat("yaw accuracy {0}\n", reading.YawAccuracy);
sb.AppendFormat("yaw {0} degrees\n", reading.YawDegrees);
return(sb.ToString());
}
/// <summary>
/// This is the event handler for ReadingChanged events.
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
async private void ReadingChanged(object sender, InclinometerReadingChangedEventArgs e)
{
await Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
InclinometerReading reading = e.Reading;
ScenarioOutput_X.Text = String.Format("{0,5:0.00}", reading.PitchDegrees);
ScenarioOutput_Y.Text = String.Format("{0,5:0.00}", reading.RollDegrees);
ScenarioOutput_Z.Text = String.Format("{0,5:0.00}", reading.YawDegrees);
});
}
void SetRotation(InclinometerReading inclinometerReading)
{
if (inclinometerReading == null)
{
return;
}
imageCanvasRotate.Angle = inclinometerReading.YawDegrees;
northArrowRotate.Angle = inclinometerReading.YawDegrees;
}
/// <summary>
/// This is the dispatcher callback.
/// </summary>
/// <param name="sender"></param>
/// <param name="args"></param>
private void DisplayCurrentReading(object sender, object args)
{
InclinometerReading reading = _inclinometer.GetCurrentReading();
if (reading != null)
{
ScenarioOutput_X.Text = String.Format("{0,5:0.00}", reading.PitchDegrees);
ScenarioOutput_Y.Text = String.Format("{0,5:0.00}", reading.RollDegrees);
ScenarioOutput_Z.Text = String.Format("{0,5:0.00}", reading.YawDegrees);
}
}
private async void inclinometerSensor_ReadingChanged(Inclinometer sender, InclinometerReadingChangedEventArgs args)
{
await Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
InclinometerReading reading = args.Reading;
yawDegrees.Text = String.Format("{0,2:0.00}", reading.YawDegrees.ToString());
pitchDegrees.Text = String.Format("{0,2:0.00}", reading.PitchDegrees.ToString());
rollDegrees.Text = String.Format("{0,2:0.00}", reading.RollDegrees.ToString());
});
}
void ShowYawPitchRoll(InclinometerReading inclinometerReading)
{
if (inclinometerReading == null)
{
return;
}
yawText.Text = inclinometerReading.YawDegrees.ToString("F0") + "°";
pitchText.Text = inclinometerReading.PitchDegrees.ToString("F0") + "°";
rollText.Text = inclinometerReading.RollDegrees.ToString("F0") + "°";
}
private void UpdateElementsOnScreen(InclinometerReading reading)
{
var roll = reading.RollDegrees * Math.PI / 180.0;
var pitch = reading.PitchDegrees * Math.PI / 180.0;
var yaw = reading.YawDegrees * Math.PI / 180.0;
foreach (var child in LayoutRoot.Children)
{
var fe = (child as FrameworkElement);
if (fe == null || fe.ActualHeight == 0 || fe.ActualWidth == 0)
{
continue;
}
var element = fe.DataContext as IWorldElement <Event>;
if (element == null)
{
continue;
}
var offset = ViewModel.CalculateScreenOffset(element, fe.ActualWidth, fe.ActualHeight, roll, pitch, yaw);
if (offset.TranslateX < -this.ActualWidth)
{
offset.TranslateX = -this.ActualWidth;
}
if (offset.TranslateX > this.ActualWidth * 2)
{
offset.TranslateX = this.ActualWidth * 2;
}
if (offset.TranslateY < -this.ActualHeight)
{
offset.TranslateY = -this.ActualHeight;
}
if (offset.TranslateY > this.ActualHeight * 2)
{
offset.TranslateY = this.ActualHeight * 2;
}
if (offset.Scale < 0)
{
offset.Scale = 0.0001;
}
if (offset.Scale > 2)
{
offset.Scale = 2;
}
fe.RenderTransform = new CompositeTransform
{
TranslateX = offset.TranslateX,
TranslateY = offset.TranslateY,
ScaleX = offset.Scale,
ScaleY = offset.Scale
};
}
}
void ReadInclinometerData()
{
if (inclineSensor != null)
{
InclinometerReading reading = inclineSensor.GetCurrentReading();
if (reading != null)
{
inclineX.Value = reading.PitchDegrees;
inclineY.Value = reading.RollDegrees;
inclineZ.Value = reading.YawDegrees;
}
}
}
public void OnGetInclinometer()
{
Inclinometer sensor = Inclinometer.GetDefault();
if (sensor != null)
{
InclinometerReading reading = sensor.GetCurrentReading();
InclinometerInfo = $"pitch degrees: {reading.PitchDegrees} roll degrees: {reading.RollDegrees} yaw accuracy: {reading.YawAccuracy} yaw degrees: {reading.YawDegrees}";
}
else
{
InclinometerInfo = "Inclinometer not found";
}
}
public double GetInclinationZ()
{
if (_inclinometer != null)
{
InclinometerReading reading = _inclinometer.GetCurrentReading();
if (reading != null)
{
return(reading.YawDegrees);
}
}
return(0.0);
}
public void OnGetInclinometerReport()
{
Inclinometer sensor = Inclinometer.GetDefault();
if (sensor != null)
{
sensor.ReportInterval = Math.Max(sensor.MinimumReportInterval, 1000);
sensor.ReadingChanged += async(s, e) =>
{
InclinometerReading reading = e.Reading;
await CoreApplication.MainView.Dispatcher.RunAsync(CoreDispatcherPriority.Low, () =>
{
InclinometerInfoReport = $"pitch degrees: {reading.PitchDegrees} roll degrees: {reading.RollDegrees} yaw accuracy: {reading.YawAccuracy} yaw degrees: {reading.YawDegrees} {reading.Timestamp:T}";
});
};
}
}
public override void GetRotation(ref Quaternion quaternion)
{
if (_hasSensor)
{
#if WINDOWS_PHONE
_iReading = _sensor.GetCurrentReading();
var alpha = _iReading.YawDegrees;
var beta = _iReading.RollDegrees;
var gamma = _iReading.PitchDegrees;
if (beta > 0)
{
alpha = alpha - 180.0f;
beta = -180.0f + beta;
gamma = gamma - 180.0f;
}
var z = alpha * Deg2Rad / 2.0f;
var x = beta * Deg2Rad / 2.0f;
var y = gamma * Deg2Rad / 2.0f;
var cX = Mathf.Cos(x);
var cY = Mathf.Cos(y);
var cZ = Mathf.Cos(z);
var sX = Mathf.Sin(x);
var sY = Mathf.Sin(y);
var sZ = Mathf.Sin(z);
// ZXY quaternion construction.
var w = cX * cY * cZ - sX * sY * sZ;
x = sX * cY * cZ - cX * sY * sZ;
y = cX * sY * cZ + sX * cY * sZ;
z = cX * cY * sZ + sX * sY * cZ;
quaternion.Set(x, y, z, w);
#else
quaternion = Quaternion.identity;
#endif
}
else
{
quaternion = Quaternion.identity;
}
}
private void ReadingChanged(object sender, InclinometerReadingChangedEventArgs e)
{
if (state)
{
InclinometerReading reading = e.Reading;
pitch_raw = reading.PitchDegrees;
//pitch_kalman.filter(reading.PitchDegrees);
/*
* Console.WriteLine("\n\n\n\nPitch: {0,5:0.00}", reading.PitchDegrees);
* Console.WriteLine("Roll: {0,5:0.00}", reading.RollDegrees);
* Console.WriteLine("Yaw: {0,5:0.00}", reading.YawDegrees);
*
* Console.WriteLine("Kalman Pitch: {0,5:0.00}", pitch_kalman.filter(reading.PitchDegrees));
* Console.WriteLine("Kalman Roll: {0,5:0.00}", roll_kalman.filter(reading.RollDegrees));
* Console.WriteLine("Kalman Yaw: {0,5:0.00}\n\n\n\n", yaw_kalman.filter(reading.YawDegrees));
*/
}
}
void ShowYawPitchRoll(InclinometerReading inclinometerReading)
{
if (inclinometerReading == null)
{
return;
}
double yaw = inclinometerReading.YawDegrees;
double pitch = inclinometerReading.PitchDegrees;
double roll = inclinometerReading.RollDegrees;
yawValue.Text = yaw.ToString("F0") + "°";
pitchValue.Text = pitch.ToString("F0") + "°";
rollValue.Text = roll.ToString("F0") + "°";
yawRotate.Angle = yaw;
if (pitch <= 90 && pitch >= -90)
{
pitchPath.Fill = pitchPath.Stroke;
pitchEllipse.Center = new Point(this.ActualWidth / 2,
this.ActualHeight * (pitch + 90) / 180);
}
else
{
pitchPath.Fill = null;
if (pitch > 90)
{
pitchEllipse.Center = new Point(this.ActualWidth / 2,
this.ActualHeight * (270 - pitch) / 180);
}
else // pitch < -90
{
pitchEllipse.Center = new Point(this.ActualWidth / 2,
this.ActualHeight * (-90 - pitch) / 180);
}
}
rollEllipse.Center = new Point(this.ActualWidth * (roll + 90) / 180,
this.ActualHeight / 2);
}
// Uncomment for Inclinometre
// Create the change event
private async void ReadingChanged(Inclinometer sender, InclinometerReadingChangedEventArgs e)
{
await Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
InclinometerReading reading = e.Reading;
roll = reading.RollDegrees;
pitch = reading.PitchDegrees;
yaw = reading.YawDegrees;
// Move right
if (pitch > 0 && WeaponPosX < 1100 * scaleWidth)
{
WeaponPosX = WeaponPosX + pitch;
}
// Else move left
else if (pitch < 0 && WeaponPosX > 100 * scaleWidth)
{
WeaponPosX = WeaponPosX + pitch;
}
});
}
public String GetInclinometerReadingDisplayText(InclinometerReading reading)
{
if (reading == null)
{
return("No Reading Available.");
}
var axisAdjustment = SensorExtensions.AxisOffset.Default;
if (CompensateForDisplayOrientation)
{
axisAdjustment = DisplayOrientation.AxisAdjustmentFactor();
}
var adjustedPitchDegrees = reading.PitchDegrees * axisAdjustment.X;
var adjustedRollDegrees = reading.RollDegrees * axisAdjustment.Y;
var adjustedYawDegrees = reading.YawDegrees * axisAdjustment.Z;
return(String.Format("Pitch={0} Roll={1} Yaw={2}",
adjustedPitchDegrees,
adjustedRollDegrees,
adjustedYawDegrees));
}
void _inclinometer_ReadingChanged(Inclinometer sender, InclinometerReadingChangedEventArgs args)
{
try
{
InclinometerReading reading = args.Reading;
if (reading != null)
{
SensorEventArgs sensorArgs = new SensorEventArgs(reading.RollDegrees);
if (SensorReadingChanged != null)
{
SensorReadingChanged(sender, sensorArgs);
}
}
}
catch (Exception)
{
_inclinometerEnabled = false;
}
_inclinometerChecked = true;
}
private void SIncline_ReadingChanged(Inclinometer sender, InclinometerReadingChangedEventArgs args)
{
LastIncline = args.Reading;
}
