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();
}
