private static void ResetPositionEstimator(CrazyflieCopter crazyflie) { crazyflie.ParamConfigurator.SetValue("kalman.resetEstimation", (byte)1).Wait(); Thread.Sleep(100); crazyflie.ParamConfigurator.SetValue("kalman.resetEstimation", (byte)0).Wait(); Thread.Sleep(2000); }
static void Main(string[] args) { var logRepository = LogManager.GetRepository(Assembly.GetEntryAssembly()); XmlConfigurator.Configure(logRepository, new FileInfo("log4net.config")); try { var crazyflie = new CrazyflieCopter(); crazyflie.Connect().Wait(); try { Fly(crazyflie); } catch (Exception ex) { Log.Error("Error flying crazyfly.", ex); } crazyflie?.Disconnect(); } catch (Exception ex) { Log.Error("Error setting up radio.", ex); } Console.WriteLine("ended."); Console.ReadLine(); }
private static bool LandAndPause(CrazyflieCopter crazyflie) { for (int i = 0; i < 10; i++) { crazyflie.Commander.SendSetPoint(0, 0, 0, (ushort)(15000 - (10 * i))); Thread.Sleep(200); } crazyflie.Commander.SendStopSetPoint(); Log.InfoFormat("Paused...Hit SPACE to resume, ESC to quit."); var pauseLoop = true; while (pauseLoop) { if (Console.KeyAvailable) { switch (Console.ReadKey().Key) { // resume case ConsoleKey.Spacebar: pauseLoop = false; break; // end case ConsoleKey.Escape: pauseLoop = false; return(false); } } } ResetPositionEstimator(crazyflie); return(true); }
private static void HighLevelCommandExample(CrazyflieCopter crazyflie) { if (crazyflie.ParamConfigurator.IsParameterKnown("commander.enHighLevel")) { crazyflie.HighLevelCommander.Enable().Wait(); // To enable the mellinger controller: //crazyflie.ParamConfigurator.SetValue("stabilizer.controller", (byte)2).Wait(); // To enable the default controller: //crazyflie.ParamConfigurator.SetValue("stabilizer.controller", (byte)1).Wait(); crazyflie.ParamConfigurator.SetValue("kalman.resetEstimation", (byte)1).Wait(); Thread.Sleep(10); crazyflie.ParamConfigurator.SetValue("kalman.resetEstimation", (byte)0).Wait(); Thread.Sleep(1000); try { crazyflie.HighLevelCommander.Takeoff(0.3f, 2f); Thread.Sleep(2000); crazyflie.HighLevelCommander.Land(0, 2f); Thread.Sleep(2100); } finally { crazyflie.HighLevelCommander.Stop(); crazyflie.HighLevelCommander.Disable().Wait(); } } else { Log.Error("Highlevel commander not available. Update Crazyflie firmware."); } }
private static void ParameterExample(CrazyflieCopter crazyflie) { crazyflie.ParamConfigurator.RequestUpdateOfAllParams().Wait(); // alternatively you can also use event AllParametersUpdated var result = crazyflie.ParamConfigurator.GetLoadedParameterValue("system.selftestPassed"); Log.Info($"self test passed: {Convert.ToBoolean(result)}"); }
private static void CommanderExample(CrazyflieCopter crazyflie) { // use a velocity of 0.2m/sec for 2 seconds in z direction to start. for (int i = 0; i < 10; i++) { crazyflie.Commander.SendVelocityWorldSetpoint(0, 0, 0.2f, 0); Thread.Sleep(200); } // use a velocity of -0.2m/sec for 2 seconds in z direction to land. for (int i = 0; i < 10; i++) { crazyflie.Commander.SendVelocityWorldSetpoint(0, 0, -0.2f, 0); Thread.Sleep(200); } }
static void Main(string[] args) { var logRepository = LogManager.GetRepository(Assembly.GetEntryAssembly()); XmlConfigurator.Configure(logRepository, new FileInfo("log4net.config")); try { var crazyflie = new CrazyflieCopter(); crazyflie.Connect().Wait(); try { LoggingExample(crazyflie); ParameterExample(crazyflie); Console.WriteLine("Sleepy time...Wait for takeoff demo Press ENTER."); WaitForKey(ConsoleKey.Enter); CommanderExample(crazyflie); Console.WriteLine("Sleepy time...Wait for high level demo Press ENTER."); WaitForKey(ConsoleKey.Enter); HighLevelCommandExample(crazyflie); Console.WriteLine("Sleepy time...Wait for high level navigator demo Press ENTER."); WaitForKey(ConsoleKey.Enter); NavigationExample(crazyflie); Console.WriteLine("Sleepy time...Hit ESC to quit."); WaitForKey(ConsoleKey.Escape); } catch (Exception ex) { Log.Error("Error testing crazyfly.", ex); } crazyflie?.Disconnect(); } catch (Exception ex) { Log.Error("Error setting up radio.", ex); } Console.WriteLine("ended."); Console.ReadLine(); }
private static void NavigationExample(CrazyflieCopter crazyflie) { var task = crazyflie.ParamConfigurator.RefreshParameterValue("flightmode.posSet"); task.Wait(); Log.Info("flightmode.posSet before: " + task.Result); task = crazyflie.ParamConfigurator.RefreshParameterValue("stabilizer.controller"); task.Wait(); Log.Info("stabilizer.controller before: " + task.Result); //crazyflie.ParamConfigurator.SetValue("stabilizer.controller", (byte)2).Wait(); //crazyflie.ParamConfigurator.SetValue("flightmode.posSet", (byte)1).Wait(); //crazyflie.ParamConfigurator.SetValue("stabilizer.controller", (byte)1); var navigator = new Navigator(crazyflie); try { navigator.PositionUpdate += Navigator_PositionUpdate; navigator.Start(100); try { navigator.WaitForCalibratedPosition(TimeSpan.FromSeconds(15)).Wait(); } catch (Exception ex) { Log.Error("didn't found a calibration; abort demo."); return; } navigator.Takeoff(1f).Wait(); Log.Warn("Takeoff now done"); navigator.NavigateTo(0.4f, 0.4f, 1f).Wait(); navigator.NavigateTo(1.6f, 0.4f, 1f).Wait(); navigator.NavigateTo(1.6f, 1.1f, 1f).Wait(); navigator.NavigateTo(0.4f, 1.1f, 1f).Wait(); navigator.NavigateTo(0.4f, 0.4f, 1f).Wait(); navigator.Land(-0.3f).Wait(); } finally { navigator.Stop().Wait(); } }
private static void LoggingExample(CrazyflieCopter crazyflie) { if (!crazyflie.Logger.IsLogVariableKnown("stabilizer.roll")) { Log.Warn("stabilizer.roll not a known log variable"); } var config = crazyflie.Logger.CreateEmptyLogConfigEntry("Stabilizer", 100); config.AddVariable("stabilizer.roll", "float"); config.AddVariable("stabilizer.pitch", "float"); config.AddVariable("stabilizer.yaw", "float"); config.LogDataReceived += Config_LogDataReceived; crazyflie.Logger.AddConfig(config); crazyflie.Logger.StartConfig(config); Thread.Sleep(1000); crazyflie.Logger.StopConfig(config); crazyflie.Logger.DeleteConfig(config); Thread.Sleep(1000); }
private static void Fly(CrazyflieCopter crazyflie) { ResetPositionEstimator(crazyflie); crazyflie.ParamConfigurator.SetValue("flightmode.posSet", (byte)0); try { for (int i = 0; i < 10; i++) { crazyflie.Commander.SendSetPoint(0, 0, 0, 0); } Thread.Sleep(200); crazyflie.Commander.SendSetPoint(0, 0, 0, 15000); // Init float roll = 0; float pitch = 0; float yaw = 0; ushort thrust = 0; // Max/min values float rollRange = 50; float pitchRange = 50; float yawRange = 100; ushort thrustRange = 50000; // Stick ranges int stickRange = 1000; // Get first attached game controller found var directInput = new DirectInput(); var attahcedGameControllerDevices = directInput.GetDevices(DeviceClass.GameController, DeviceEnumerationFlags.AttachedOnly); if (!attahcedGameControllerDevices.Any()) { throw new ApplicationException("No available game controllers found."); } var attachedDeviceInstance = attahcedGameControllerDevices.First(); var joystick = new Joystick(directInput, attachedDeviceInstance.InstanceGuid); foreach (DeviceObjectInstance doi in joystick.GetObjects(ObjectDeviceType.Axis)) { joystick.GetObjectPropertiesById((int)doi.ObjectType).SetRange(-1 * stickRange, stickRange); } joystick.Properties.AxisMode = DeviceAxisMode.Absolute; joystick.Acquire(); var joystickState = new JoystickState(); var loop = true; while (loop) { if (Console.KeyAvailable) { switch (Console.ReadKey().Key) { // end case ConsoleKey.Escape: loop = false; break; // pause case ConsoleKey.Spacebar: loop = LandAndPause(crazyflie); continue; default: Log.InfoFormat("Invalid key for action."); break; } } // Poll the device and get state joystick.Poll(); joystick.GetCurrentState(ref joystickState); // Get buttons pressed info var stringWriter = new StringWriter(); var buttons = joystickState.GetButtons(); var anyButtonsPressed = buttons.Any(b => b == true); if (anyButtonsPressed) { for (int buttonNumber = 0; buttonNumber < buttons.Length; buttonNumber++) { if (buttons[buttonNumber] == true) { stringWriter.Write(string.Format("{0}", buttonNumber)); } } } var buttonsPressedString = stringWriter.ToString().Trim(); // Joystick info var leftStickX = joystickState.X; var leftStickY = joystickState.Y; var rightStickX = joystickState.RotationX; var rightStickY = joystickState.RotationY; roll = rollRange * rightStickX / stickRange; pitch = pitchRange * rightStickY / stickRange; yaw = yawRange * leftStickX / stickRange; thrust = (ushort)(leftStickY > 0 ? 0 : thrustRange * -1 * leftStickY / stickRange); var infoString = String.Format("LX:{0,7}, LY:{1,7}, RX:{2,7}, RY:{3,7}, Buttons:{4,7}.\tRoll:{5, 7}, Pitch:{6, 7}, Yaw:{7, 7}, Thrust:{8, 7}.", leftStickX, leftStickY, rightStickX, rightStickY, buttonsPressedString, roll, pitch, yaw, thrust); Console.WriteLine(infoString); Thread.Sleep(20); crazyflie.Commander.SendSetPoint(roll, pitch, yaw, thrust); } } catch (Exception) { try { crazyflie.Commander.SendStopSetPoint(); } catch (Exception) { } throw; } crazyflie.Commander.SendStopSetPoint(); }
public Navigator(CrazyflieCopter copter) { _copter = copter; }
private static void Fly(CrazyflieCopter crazyflie) { ResetPositionEstimator(crazyflie); crazyflie.ParamConfigurator.SetValue("flightmode.posSet", (byte)0); try { ushort thrustIncrements = 1000; float pitchIncrements = 5; float yawIncrements = 2; float rollIncrements = 5; ushort thrust = 15000; float pitch = 0; float yaw = 0; float roll = 0; for (int i = 0; i < 10; i++) { crazyflie.Commander.SendSetPoint(0, 0, 0, 0); } Thread.Sleep(200); crazyflie.Commander.SendSetPoint(0, 0, 0, 15000); var loop = true; while (loop) { Log.InfoFormat("Thrust: {0}, Pitch: {1}, Roll: {2}, Yaw: {3}.", thrust, pitch, roll, yaw); if (Console.KeyAvailable) { switch (Console.ReadKey().Key) { // end case ConsoleKey.Escape: loop = false; break; // pause case ConsoleKey.Spacebar: loop = LandAndPause(crazyflie); continue; // thrust up case ConsoleKey.UpArrow: thrust += thrustIncrements; break; // thrust down case ConsoleKey.DownArrow: thrust -= thrustIncrements; break; // yaw right case ConsoleKey.RightArrow: yaw += yawIncrements; break; // yaw left case ConsoleKey.LeftArrow: yaw -= yawIncrements; break; // pitch backward case ConsoleKey.S: pitch += pitchIncrements; break; // pitch forward case ConsoleKey.W: pitch -= pitchIncrements; break; // roll right case ConsoleKey.D: roll += rollIncrements; break; // roll left case ConsoleKey.A: roll -= rollIncrements; break; default: Log.InfoFormat("Invalid key for action."); break; } } Thread.Sleep(20); crazyflie.Commander.SendSetPoint(roll, pitch, yaw, thrust); } } catch (Exception) { try { crazyflie.Commander.SendStopSetPoint(); } catch (Exception) { } throw; } crazyflie.Commander.SendStopSetPoint(); }
private static void Fly(CrazyflieCopter crazyflie) { ResetPositionEstimator(crazyflie); crazyflie.ParamConfigurator.SetValue("flightmode.posSet", (byte)1); try { float vIncrement = 0.05f; float yawIncrement = 5; float vx = 0; float vy = 0; float vz = -0.4f; // ensure that it doesn't takeoff directly. float yaw = 0; var loop = true; while (loop) { Log.InfoFormat($"Vx: {vx}, Vy: {vy}, vz: {vz}, Yaw: {yaw}."); if (Console.KeyAvailable) { switch (Console.ReadKey().Key) { // end case ConsoleKey.Escape: loop = false; break; // pause case ConsoleKey.Spacebar: loop = LandAndPause(crazyflie); continue; // move up case ConsoleKey.UpArrow: vz += vIncrement; break; // move down case ConsoleKey.DownArrow: vz -= vIncrement; break; // yaw right case ConsoleKey.RightArrow: yaw += yawIncrement; break; // yaw left case ConsoleKey.LeftArrow: yaw -= yawIncrement; break; // move in negative x direction case ConsoleKey.S: vx -= vIncrement; break; // move in postive x direction case ConsoleKey.W: vx += vIncrement; break; // move in positive y direction case ConsoleKey.D: vy += vIncrement; break; // move in negative y direction case ConsoleKey.A: vy -= vIncrement; break; default: Log.InfoFormat("Invalid key for action."); break; } } crazyflie.Commander.SendVelocityWorldSetpoint(vx, vy, vz, yaw); Thread.Sleep(50); } } catch (Exception) { try { crazyflie.Commander.SendStopSetPoint(); } catch (Exception) { } throw; } crazyflie.Commander.SendStopSetPoint(); }