private void SetUltrasonicSensorData()
{
if (App.isRPi)
{
var bytes = MultiWii.MSP_ULTRASONIC_SENSOR((int)MultiWii.usDistance);
Socket.SendData(bytes);
}
else
{
Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
// UI update
var distance = MultiWii.usDistance;
distanceLabel.Text = $"D:{distance}cm";
});
}
}
private void SetSignalStrengthData()
{
if (App.isRPi)
{
var signal = Telemetry.GetSignalStrength();
MultiWii.signalStrength = signal;
var bytes = MultiWii.MSP_SIGNAL_STRENGTH(signal);
Socket.SendData(bytes);
}
else
{
Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
// UI update
signalStrengthLabel.Text = $"SS:{MultiWii.signalStrength}";
});
}
}
public static void Land()
{
if (App.isRPi)
{
cts = new CancellationTokenSource();
var ct = cts.Token;
Task.Run(async() =>
{
if (!IsInControl)
{
TakeControl();
}
if (MultiWii.usDistance <= 200)
{
IsLanding = true;
while (MultiWii.usDistance > 15)
{
if (ct.IsCancellationRequested)
{
ReleaseControl();
return;
}
if (MultiWii.throttle < 1000)
{
break;
}
var throttle = MultiWii.throttle - 1;
MultiWii.MSP_SET_RAW_RC((ushort)throttle, (ushort)MultiWii.roll, (ushort)MultiWii.pitch, (ushort)MultiWii.yaw,
(ushort)MultiWii.aux1, 1000, (ushort)MultiWii.aux3, (ushort)MultiWii.aux4); //Cleanflight - TAER1234
await Task.Delay(100);
}
var cmd = Encoding.ASCII.GetBytes("#land");
Socket.SendData(cmd);
IsLanding = false;
}
ReleaseControl();
}, ct);
}
else
{
MainPage.throttle = 0;
MainPage.throttleTakeOffRange = 0;
MainPage.isTakeOffCompleted = false;
MainPage.isLandCompleted = true;
CoreApplication.MainView.CoreWindow.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
var currentPage = ((ContentControl)Window.Current.Content).Content as Page;
var takeOffLandToggle = currentPage.FindName("takeOffLandToggle") as Image;
var armToggle = currentPage.FindName("armToggle") as ToggleButton;
if (armToggle.IsChecked == true)
{
takeOffLandToggle.Opacity = 1;
}
takeOffLandToggle.Source = new Windows.UI.Xaml.Media.Imaging.BitmapImage(new Uri("ms-appx:///Assets/Icons/takeoff.png"));
});
}
}
private async Task InitNaze32()
{
await MultiWii.Init();
}
private void OnTimer()
{
Task.Run(() =>
{
long currentTimeMilliseconds = -1, previousTimeMilliseconds = -1;
long currentTelemetryMs = -1, previousTelemetryMs = -1;
while (true)
{
currentTimeMilliseconds = currentTelemetryMs = stopwatch.ElapsedMilliseconds;
// Check for new cmd every 20 ms
if ((currentTimeMilliseconds - previousTimeMilliseconds) > 20 || previousTimeMilliseconds == -1)
{
var socketIsConnected = Socket.socketIsConnected;
if (!App.isRPi && isGamepadConnected && socketIsConnected)
{
// Read data from controller
var reading = controller.GetCurrentReading();
// Set RC data
SetThrottle(reading.RightTrigger, reading.LeftTrigger, reading.LeftThumbstickY);
SetYaw(reading.LeftThumbstickX);
SetPitch(reading.RightThumbstickY);
SetRoll(reading.RightThumbstickX);
Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
pitchLabel.Text = $"P:{(ushort)pitch}";
rollLabel.Text = $"R:{(ushort)roll}";
yawLabel.Text = $"Y:{(ushort)yaw}";
throttleLabel.Text = $"T:{(ushort)throttle}";
takeOffRangeLabel.Text = $"TOR:{throttleTakeOffRange}";
});
//Send data to RPI
var bytes = MultiWii.Calculate_MSP_SET_RAW_RC((ushort)throttle, (ushort)roll, (ushort)pitch, (ushort)yaw, aux1, aux2, aux3, aux4); //Cleanflight - TAER1234
Socket.SendData(bytes);
lastCmdSent = stopwatch.ElapsedMilliseconds;
}
previousTimeMilliseconds = currentTimeMilliseconds;
}
if ((currentTelemetryMs - previousTelemetryMs) > 150 || previousTelemetryMs == -1)
{
if (App.isRPi)
{
MultiWii.GetGPSData();
MultiWii.GetAttitude();
MultiWii.GetBatteryState();
ultrasonicSensor.GetDistance();
}
SetGpsData();
SetAttitudeData();
SetBatteryData();
SetSignalStrengthData();
SetUltrasonicSensorData();
SendTelemetryData();
previousTelemetryMs = currentTelemetryMs;
}
}
});
}
