private void Joystick_InputReceived(object sender, JoystickEventArgs e)
{
if (e.Button == 0 && e.IsPressed) // Front button
{
drone.Hover();
}
else if (e.Button == 1 && e.IsPressed) // Pad-2 button
{
drone.Emergency();
}
else if (e.Button == 2 && e.IsPressed) // Pad-3 button
{
drone.Takeoff();
}
else if (e.Button == 4 && e.IsPressed) // Pad-5 button
{
drone.Land();
}
else if (e.Button == 9 && e.IsPressed) // Throttle-10 button
{
heightMaintainer.TargetHeight += 0.25f; // meter
}
else if (e.Button == 10 && e.IsPressed) // Throttle-11 button
{
heightMaintainer.TargetHeight -= 0.25f; // meter
}
}
private void ReceiverOnCommandReceived(object sender, CommandEventArgs eventArgs)
{
Log(eventArgs.Command);
if (!_executeCommands)
{
return;
}
switch (eventArgs.Command)
{
case "takeoff":
_client.Takeoff();
break;
case "land":
_client.Land();
break;
case "hover":
_client.Hover();
break;
case "up":
_client.Progress(FlightMode.Progressive, gaz: 0.25f);
break;
case "turnleft":
_client.Progress(FlightMode.Progressive, yaw: 0.25f);
break;
case "forward":
_client.Progress(FlightMode.Progressive, pitch: -0.5f);
break;
case "turnright":
_client.Progress(FlightMode.Progressive, yaw: -0.25f);
break;
case "down":
_client.Progress(FlightMode.Progressive, gaz: -0.25f);
break;
case "left":
_client.Progress(FlightMode.Progressive, yaw: 0.25f);
break;
case "right":
_client.Progress(FlightMode.Progressive, roll: 0.05f);
break;
case "back":
_client.Progress(FlightMode.Progressive, pitch: 0.5f);
break;
default:
Debug.WriteLine("Unknown Command: " + eventArgs.Command);
break;
}
}
private void DroneControls_KeyDown(object sender, KeyEventArgs e)
{
System.Diagnostics.Debug.WriteLine(e.Key);
switch (e.Key)
{
case Key.Left:
droneClient.Progress(FlightMode.Progressive, roll: -0.05f);
break;
case Key.Up:
droneClient.Progress(FlightMode.Progressive, gaz: 0.25f);
break;
case Key.Right:
droneClient.Progress(FlightMode.Progressive, roll: 0.05f);
break;
case Key.Down:
droneClient.Progress(FlightMode.Progressive, gaz: -0.25f);
break;
case Key.A:
droneClient.Progress(FlightMode.Progressive, yaw: 0.25f);
break;
case Key.D:
droneClient.Progress(FlightMode.Progressive, yaw: -0.25f);
break;
case Key.S:
droneClient.Progress(FlightMode.Progressive, pitch: 0.05f);
break;
case Key.W:
droneClient.Progress(FlightMode.Progressive, pitch: -0.05f);
break;
case Key.I:
droneClient.Takeoff();
break;
case Key.O:
droneClient.Land();
break;
default:
break;
}
}
public bool Land()
{
if (!enabled)
{
return(false);
}
Logger.WriteLine("Drone.Land: IsActive = " + droneClient.IsActive + ", IsFlying = " + droneClient.IsFlying);
if (droneClient.IsActive && droneClient.IsFlying)
{
if (detectFlyStatusTimer != null)
{
detectFlyStatusTimer.Stop();
}
detectFlyStatusTimer = new DispatcherTimer();
detectFlyStatusTimer.Interval = TimeSpan.FromMilliseconds(200);
detectFlyStatusTimer.Tick += (object sender, EventArgs e) =>
{
if (!droneClient.IsFlying)
{
if (OnFlyStopStart != null)
{
OnFlyStopStart(false);
}
detectFlyStatusTimer.Stop();
}
};
detectFlyStatusTimer.Start();
droneClient.Land();
// commandTimer.Stop();
return(true);
}
return(false);
}
protected override void Update(GameTime gameTime)
{
var keyState = Keyboard.GetState(PlayerIndex.One);
if (screenType == ScreenType.Splash)
{
splashTimeElapsed += gameTime.ElapsedGameTime.Milliseconds;
if (splashTimeElapsed >= 3000)
{
screenType = ScreenType.Drive;
}
}
else
{
if (keyState.IsKeyDown(Keys.Enter))
{
DroneTakeOff();
}
if (keyState.IsKeyDown(Keys.Escape))
{
droneClient.Land();
}
if (keyState.IsKeyDown(Keys.E))
{
droneClient.Emergency();
}
UpdateFrame();
}
kinectStatusMessage = (kinect != null) ? kinect.StatusMessage : string.Empty;
base.Update(gameTime);
}
static void Main(string[] args)
{
var droneClient = new DroneClient("192.168.1.1");
SleepAndPaint(droneClient);
droneClient.Start();
SleepAndPaint(droneClient);
droneClient.Takeoff();
SleepAndPaint(droneClient);
droneClient.Progress(FlightMode.Progressive, yaw: -0.05f);
System.Console.WriteLine("Yaw -0.05");
SleepAndPaint(droneClient);
droneClient.Hover();
SleepAndPaint(droneClient);
droneClient.Land();
SleepAndPaint(droneClient);
droneClient.Stop();
SleepAndPaint(droneClient);
System.Console.ReadLine();
droneClient.Dispose();
}
/// <summary>
/// Called if the gameobject is destroyed
/// </summary>
void OnDestroy()
{
droneClient.Land();
droneClient.Stop();
droneClient.Dispose();
videoPacketDecoderWorker.Stop();
videoPacketDecoderWorker.Dispose();
}
private void TakeOffLandButton_Click(object sender, RoutedEventArgs e)
{
if (_droneClient.IsFlying)
{
_droneClient.Land();
}
else
{
_droneClient.TakeOff();
}
UpdateDisplay();
}
/// <summary>
/// Lands the drone
/// </summary>
/// <returns>Boolean Value whether the command works properly</returns>
public bool Land()
{
try
{
_client.Land();
return(true);
}
catch (Exception)
{
return(false);
}
}
private void TakeOff_Click(object sender, RoutedEventArgs e)
{
if (_droneClient.IsFlying())
{
_droneClient.Land();
TakeOffButton.Content = "Take off";
}
else
{
_droneClient.TakeOff();
TakeOffButton.Content = "Land";
}
}
/// <summary>
/// Sends the command with its parameters to a provided DroneClient
/// </summary>
/// <param name="aDroneClient">Drone client to the the command to</param>
public void Send(DroneClient aDroneClient)
{
switch (Command)
{
case Type.Progress: aDroneClient.Progress(ProgressMode, roll: Roll, pitch: Pitch, yaw: Yaw, gaz: Gaz); break;
case Type.Takeoff: aDroneClient.Takeoff(); break;
case Type.Hover: aDroneClient.Hover(); break;
case Type.Land: aDroneClient.Land(); break;
case Type.Emergency: aDroneClient.Emergency(); break;
case Type.ResetEmergency: aDroneClient.ResetEmergency(); break;
case Type.FlatTrim: aDroneClient.FlatTrim(); break;
}
}
void GestureDetection_LeftHandUpDownChanged(object sender, HandPositionChangedArgs args)
{
switch (args.Position)
{
case HandPosition.Up:
_client.Takeoff();
DroneCommandChanged(_client, new DroneCommandChangedEventArgs {
CommandText = "Taking Off"
});
break;
case HandPosition.Center:
break;
case HandPosition.Down:
_client.Land();
DroneCommandChanged(_client, new DroneCommandChangedEventArgs {
CommandText = "Landing"
});
break;
}
}
// Update is called once per frame
void Update()
{
convertCameraData();
updateGamepadState();
moveStick();
// Start or land the drone
if (state.Buttons.Start.Equals(ButtonState.Pressed) && !startButtonPressed)
{
if (isLanded)
{
droneClient.Takeoff();
}
else
{
droneClient.Land();
}
isLanded = !isLanded;
startButtonPressed = true;
}
if (!state.Buttons.Start.Equals(ButtonState.Pressed))
{
startButtonPressed = false;
}
// exit application
if (Input.GetKey("escape") || state.Buttons.Back.Equals(ButtonState.Pressed))
{
Application.Quit();
}
// Move the drone
var pitch = -state.ThumbSticks.Left.Y;
var roll = state.ThumbSticks.Left.X;
var gaz = state.Triggers.Right - state.Triggers.Left;
var yaw = state.ThumbSticks.Right.X;
droneClient.Progress(AR.Drone.Client.Command.FlightMode.Progressive, pitch: pitch, roll: roll, gaz: gaz, yaw: yaw);
// Switch drone camera
if (CameraForSwitchCheck.rotation.x >= SwitchRotation)
{
if (SecondaryRenderer.material.mainTexture != cameraTexture)
{
MainRenderer.material.mainTexture = blackTexture;
SecondaryRenderer.material.mainTexture = cameraTexture;
switchDroneCamera(AR.Drone.Client.Configuration.VideoChannelType.Vertical);
}
}
else
{
if (MainRenderer.material.mainTexture != cameraTexture)
{
SecondaryRenderer.material.mainTexture = blackTexture;
MainRenderer.material.mainTexture = cameraTexture;
switchDroneCamera(AR.Drone.Client.Configuration.VideoChannelType.Horizontal);
}
}
// set status text
if (navigationData != null)
{
StatusText.text = string.Format("Battery: {0} % \nYaw: {1:f} \nPitch: {2:f} \nRoll: {3:f} \nAltitude: {4} m",
navigationData.Battery.Percentage, navigationData.Yaw, navigationData.Pitch,
navigationData.Roll, navigationData.Altitude);
}
// determine wifi strength
determineWifiStrength();
}
void DroneControl(object sender)
{
int expiration = Properties.Settings.Default.AutoPilotExpiration;
bool useQ = Properties.Settings.Default.UseAutoPilot;
_autopilot.Active = useQ;
float _lrfb = Properties.Settings.Default.DefaultLRFBSpeed;
float _yaw = Properties.Settings.Default.DefaultYawSpeed;
float _gaz = Properties.Settings.Default.DefaultGazSpeed;
#region // engine
if (sender == "v")
{
_droneClient.FlatTrim();
_autopilot.ClearObjectives();
_droneClient.Takeoff();
}
;
if (sender == "n")
{
_autopilot.ClearObjectives();
_droneClient.Emergency();
}
;
if (sender == "m")
{
_droneClient.ResetEmergency();
}
;
if (sender == "b")
{
_autopilot.ClearObjectives();
_droneClient.Land();
}
;
#endregion
_autopilot.Active = true;
// navigation
#region left right up down and hover
//if (sender == btnLeft || sender == btnRight)
//{
// if (useQ)
// {
// _autopilot.EnqueueObjective(Objective.Create(expiration, new SetRoll(
// (sender == btnLeft ? -_lrfb : _lrfb)
// )));
// }
// else
// {
// _droneClient.Progress(FlightMode.Progressive,
// roll: (sender == btnLeft ? -_lrfb : _lrfb)
// );
// }
//}
if (sender == "a")
{
if (useQ)
{
_autopilot.EnqueueObjective(Objective.Create(expiration, new SetRoll(-_lrfb)));
}
else
{
_droneClient.Progress(FlightMode.Progressive, roll: -_lrfb);
}
}
;
if (sender == "d")
{
if (useQ)
{
_autopilot.EnqueueObjective(Objective.Create(expiration, new SetRoll(_lrfb)));
}
else
{
_droneClient.Progress(FlightMode.Progressive, roll: _lrfb);
}
}
;
if (sender == "w")
{
if (useQ)
{
_autopilot.EnqueueObjective(Objective.Create(expiration, new SetPitch(-_lrfb)));
}
else
{
_droneClient.Progress(FlightMode.Progressive, pitch: -_lrfb);
}
}
;
if (sender == "s")
{
if (useQ)
{
_autopilot.EnqueueObjective(Objective.Create(expiration, new SetPitch(_lrfb)));
}
else
{
_droneClient.Progress(FlightMode.Progressive, pitch: _lrfb);
}
}
;
if (sender == "x")
{
if (useQ)
{
_autopilot.EnqueueObjective(
Objective.Create(expiration,
new VelocityX(0.0f),
new VelocityY(0.0f),
new Altitude(1.0f)
));
}
else
{
_droneClient.Hover();
}
}
#endregion
#region yaw left and right
if (sender == "j")
{
if (useQ)
{
_autopilot.EnqueueObjective(Objective.Create(expiration, new SetYaw(-_yaw)));
}
else
{
_droneClient.Progress(FlightMode.Progressive, yaw: -_yaw);
}
}
;
if (sender == "l")
{
if (useQ)
{
_autopilot.EnqueueObjective(Objective.Create(expiration, new SetYaw(_yaw)));
}
else
{
_droneClient.Progress(FlightMode.Progressive, yaw: _yaw);
}
}
;
#endregion
#region gaz up and down
if (sender == "i")
{
if (useQ)
{
_autopilot.EnqueueObjective(Objective.Create(expiration, new SetGaz(_gaz)));
}
else
{
_droneClient.Progress(FlightMode.Progressive, gaz: _gaz);
}
//_droneClient.Hover();
}
;
if (sender == "k")
{
if (useQ)
{
_autopilot.EnqueueObjective(Objective.Create(expiration, new SetGaz(-_gaz)));
}
else
{
_droneClient.Progress(FlightMode.Progressive, gaz: -_gaz);
}
}
;
#endregion
}
private void button3_Click(object sender, EventArgs e)
{
_droneClient.Land();
}
protected async override void OnNavigatedTo(NavigationEventArgs e)
{
base.OnNavigatedTo(e);
if (await _droneClient.ConnectAsync())
{
TakeOffButton.IsEnabled = true;
await Task.Run(async() =>
{
try
{
if (_speechRecognizer == null)
{
_speechRecognizer = new SpeechRecognizer();
_speechRecognizer.Grammars.AddGrammarFromList("DroneCommands", new List <string>
{
"Take off", "Take off the drone", "Take the drone off",
"Land", "Land the drone"
});
}
try
{
while (true)
{
Dispatcher.BeginInvoke(() =>
{
if (TextRecognizer.Text != "Voice: listening...")
{
TextRecognizer.Foreground = new SolidColorBrush(Colors.Green);
TextRecognizer.Text = "Voice: listening...";
}
});
var result = await _speechRecognizer.RecognizeAsync();
//if (result.TextConfidence == SpeechRecognitionConfidence.High)
if (result.Text.Length > 0)
{
Dispatcher.BeginInvoke(() =>
{
switch (result.Text)
{
case "Take off":
case "Take off the drone":
case "Take the drone off":
_droneClient.TakeOff();
break;
case "Land":
case "Land the drone":
_droneClient.Land();
break;
}
});
await Task.Delay(1000);
}
else
{
await Task.Delay(500);
}
}
}
catch (Exception ex)
{
Debug.WriteLine(ex);
}
Dispatcher.BeginInvoke(() =>
{
TextRecognizer.Foreground = new SolidColorBrush(Colors.Red);
TextRecognizer.Text = "Voice: unknown";
});
}
catch (Exception ex)
{
Debug.WriteLine(ex);
}
});
}
}
public void Stop()
{
droneClient.Land();
droneClient.Stop();
}
/// <summary>
/// This method will let the drone land.
/// The given time is the threading wait time
/// </summary>
/// <param name="time"></param>
public void Land(int time = 3000)
{
Console.WriteLine("Land");
_droneClient.Land();
System.Threading.Thread.Sleep(time);
}
public void Land()
{
_client.Land();
}
public async void Update()
{
float pitch = 0, roll = 0, yaw = 0, gaz = 0;
if (_DroneClient == null || _Controller == null)
{
return;
}
if (!_Controller.IsConnected || !_Controller.GetState(out _ControllerState))
{
_FailCounter++;
if (_FailCounter > _FailCounterMax)
{
DroneClient.InputState.Update(0, 0, 0, 0);
//Avoid overflow
_FailCounter = _FailCounterMax;
}
return;
}
if (_ControllerState.PacketNumber <= _ControllerPreviousState.PacketNumber)
{
return;
}
//Thumbs
var leftThumb = NormalizeInput(_ControllerState.Gamepad.LeftThumbX, _ControllerState.Gamepad.LeftThumbY, Convert.ToInt16(SharpDX.XInput.Gamepad.LeftThumbDeadZone * 1.1), _JoystickRange);
if (leftThumb.NormalizedMagnitude > 0)
{
roll = (float)_ControllerState.Gamepad.LeftThumbX * _RollThrottle / _JoystickRange;
pitch = (float)_ControllerState.Gamepad.LeftThumbY * _PitchThrottle / _JoystickRange;
}
var rightThumb = NormalizeInput(_ControllerState.Gamepad.RightThumbX, _ControllerState.Gamepad.RightThumbY, Convert.ToInt16(SharpDX.XInput.Gamepad.RightThumbDeadZone * 1.1), _JoystickRange);
if (rightThumb.NormalizedMagnitude > 0)
{
yaw = (float)_ControllerState.Gamepad.RightThumbX * _YawThrottle / _JoystickRange;
Debug.WriteLine(yaw);
gaz = (float)_ControllerState.Gamepad.RightThumbY * _GazThrottle / _JoystickRange;
}
_FailCounter = 0;
DroneClient.InputState.Update(roll, -pitch, yaw, gaz);
//Debug.WriteLine("InputState=" + DroneClient.InputState.ToString());
//Buttons
var buttons = _ControllerState.Gamepad.Buttons;
if (buttons.HasFlag(GamepadButtonFlags.Start))
{
if (await DroneClient.ConnectAsync())
{
if (DroneClient.NavigationData.State.HasFlag(NavigationState.Landed))
{
DroneClient.TakeOff();
}
else
{
DroneClient.Land();
}
}
}
if (buttons.HasFlag(GamepadButtonFlags.Back))
{
DroneClient.Emergency();
}
if (buttons.HasFlag(GamepadButtonFlags.Y))
{
DroneClient.ResetEmergency();
}
if (buttons.HasFlag(GamepadButtonFlags.X))
{
DroneClient.ExecuteFlatTrim();
}
if (buttons.HasFlag(GamepadButtonFlags.A))
{
DroneClient.TakePicture();
}
if (buttons.HasFlag(GamepadButtonFlags.B))
{
if (DroneClient.IsRecording())
{
DroneClient.StopRecordingVideo();
}
else
{
DroneClient.StartRecordingVideo();
}
}
if (buttons.HasFlag(GamepadButtonFlags.DPadLeft))
{
DroneClient.PlayAnimation(FlightAnimationType.FlipLeft);
}
if (buttons.HasFlag(GamepadButtonFlags.DPadUp))
{
DroneClient.PlayAnimation(FlightAnimationType.FlipAhead);
}
if (buttons.HasFlag(GamepadButtonFlags.DPadRight))
{
DroneClient.PlayAnimation(FlightAnimationType.FlipRight);
}
if (buttons.HasFlag(GamepadButtonFlags.DPadDown))
{
DroneClient.PlayAnimation(FlightAnimationType.FlipBehind);
}
_ControllerPreviousState = _ControllerState;
}
void Loop(CancellationToken token)
{
Active = true;
step = 0;
Stopwatch sw1 = null;
bool pic = false;
try
{
while (!token.IsCancellationRequested)
{
switch (currentMission)
{
case Missions.Objective:
case Missions.Home:
switch (step)
{
case 0:
flyToObjective();
break;
case 1:
if (sw1 == null)
{
sw1 = Stopwatch.StartNew();
Logger.Log("Objective Reached, Taking picture", LogLevel.Event);
}
if (sw1.ElapsedMilliseconds < 4000)
{
if (sw1.ElapsedMilliseconds > 2000 && !pic)
{
pic = true;
SendPicture();
}
hover();
}
else
{
Logger.LogInfo("Step 2");
step = 2;
droneClient.Land();
Stop();
}
break;
default:
hover();
break;
}
break;
case Missions.AttendeesPicture:
default:
{
CurrentCommand = "Hover";
hover();
break;
}
}
Task.Delay(10).Wait();
}
}
finally
{
Active = false;
}
}
static void FlyDroneBuffer(string heightEvtType)
{
ConnectBufferBCI();
bool flying = false;
bool running = true;
float targetHeight = 1, targetVelocity = 2;
float maxHeight = 3, minHeight = 0.1f;
float gaz, roll, pitch, yaw;
gaz = roll = pitch = yaw = 0;
// Thread to read events from the buffer.
new Thread(
() =>
{
while (running)
{
var sec = bci_client.WaitForEvents(lastEvent, 5000);
if (sec.NumEvents > lastEvent)
{
var events = bci_client.GetEvents(lastEvent, sec.NumEvents - 1);
lastEvent = sec.NumEvents;
foreach (var evt in events)
{
string evttype = evt.Type.ToString();
Console.WriteLine("{0}: {1}", evttype, evt.Value);
// BCI event type.
if (evttype == heightEvtType)
{
var val = double.Parse(evt.Value.ToString());
if (val > 0 && targetHeight < maxHeight)
{
targetHeight += 0.1f;
}
else if (val < 0 && targetHeight < minHeight)
{
targetHeight -= 0.1f;
}
}
// Joystick
else if (evttype == "joystick")
{
var raw_val = evt.Value.ToString();
if (raw_val == "Button0")
{
if (flying)
{
drone.Land();
}
else
{
drone.Takeoff();
}
flying = !flying;
}
}
// exit
else if (evttype == "exit")
{
running = false;
}
}
}
}
}
).Start();
// Event handler (probably called from another thread) to receive navigation data from the drone.
drone.NavigationDataAcquired +=
navData =>
{
// Correction for height.
if (navData.Altitude < targetHeight - 0.1)
{
gaz = Math.Min(targetHeight - drone.NavigationData.Altitude, 1.0f);
}
else if (navData.Altitude > targetHeight + 0.1)
{
gaz = -Math.Min(drone.NavigationData.Altitude - targetHeight, 1.0f);
}
else
{
gaz = 0;
}
// Correction for velocity.
if (navData.Velocity.X > targetVelocity + 0.5)
{
pitch = navData.Pitch - 0.025f;
}
else if (navData.Velocity.X < targetHeight - 0.5)
{
pitch = navData.Pitch + 0.025f;
}
else
{
pitch = navData.Pitch;
}
};
// Infinite loop to send data to the drone.
while (running)
{
if (flying)
{
drone.Progress(FlightMode.Hover, roll, pitch, yaw, gaz);
}
Thread.Sleep(250);
}
// Land when done, if required.
if (flying)
{
drone.Land();
}
}
public void Land()
{
_droneClient.Land();
}