public SimplePIDController(Drone drone, float mass, float gravity, AbstractDroneCommand droneCommand, Parameter[] parameters)
{
_drone = drone;
_droneMass = mass;
_gravity = gravity;
_droneCommand = droneCommand;
_gravity = gravity;
_altParameter = new Parameter(ParameterType.Altitude, new Coefficients(0, 0, 0));
_yawParameter = new Parameter(ParameterType.Yaw, new Coefficients(0, 0, 0));
_pitchParameter = new Parameter(ParameterType.Pitch, new Coefficients(0, 0, 0));
_rollParameter = new Parameter(ParameterType.Roll, new Coefficients(0, 0, 0));
foreach (Parameter param in parameters)
{
switch (param.Type)
{
case ParameterType.Altitude:
_altParameter = param;
break;
case ParameterType.Pitch:
_pitchParameter = param;
break;
case ParameterType.Roll:
_rollParameter = param;
break;
case ParameterType.Yaw:
_yawParameter = param;
break;
}
}
}
public MainForm(Drone drone)
{
if (drone == null)
throw new ArgumentNullException(nameof(drone));
InitializeComponent();
this.drone = drone;
timer.Interval = 250;
timer.Tick += Timer_Tick;
timer.Start();
drone.OnInfoChange += Drone_OnInfoChange;
drone.OnPingChange += Drone_OnPingChange;
drone.OnDataChange += Drone_OnDataChange;
drone.OnSettingsChange += Drone_OnSettingsChange;
motorControl1.Init(drone);
flightControl1.Init(drone);
sensorControl1.Init(drone);
ipInfoLabel.Text = string.Format("Connecting to \"{0}\"", drone.Address);
UpdatePing(drone.IsConnected, drone.Ping);
UpdateInfo(drone.Info);
UpdateData(drone.Data);
UpdateSettings(drone.Settings);
}
public DroneOTA(Drone drone)
{
this.Drone = drone;
if (IsRunning)
Abort();
}
public CheckpointPIDController(Drone drone, float mass, float gravity, AbstractDroneCommand droneCommand, Parameter[] parameters, List<Checkpoint> checkpoints)
: base(drone, mass, gravity, droneCommand, parameters)
{
_targetCheckpoints = checkpoints;
_isStarted = false;
_rotorsEnabled = false;
}
public RecordForm(Drone drone, InputManager inputManager)
{
this.drone = drone;
this.inputManager = inputManager;
InitializeComponent();
}
public void Init(Drone drone)
{
if (drone == null)
throw new ArgumentNullException(nameof(drone));
this.Drone = drone;
this.Drone.OnDataChange += Drone_OnDataChange;
}
public PlayerController(PlayerInteraction player, Drone drone, float mass, float gravity, AbstractDroneCommand droneCommand, Parameter[] parameters)
: base(drone, mass, gravity, droneCommand, parameters)
{
_command = droneCommand;
_player = player;
_deltaZ = 0;
_drone = drone;
_isStarted = false;
return;
}
public SettingsForm(Drone drone)
{
InitializeComponent();
this.drone = drone;
this.info = drone.Info;
this.data = drone.Settings;
Bind(nameTextBox, "data.DroneName");
Bind(saveConfigCheckBox, "data.SaveConfig");
Bind(firmwareVersionTextBox, "info.BuildVersion");
Bind(buildDateTextBox, "info.BuildName");
Bind(modelTextBox, "info.ModelName");
Bind(idTextBox, "info.SerialCode");
Bind(gyroSensorTextBox, "info.GyroSensor");
Bind(magnetometerTextBox, "info.Magnetometer");
Bind(minValueTextBox, "data.ServoMin");
Bind(idleValueTextBox, "data.ServoIdle");
Bind(hoverValueTextBox, "data.ServoHover");
Bind(maxValueTextBox, "data.ServoMax");
Bind(safeMotorValueTextBox, "data.SafeServoValue");
Bind(safeTemperatureTextBox, "data.MaxTemperature");
Bind(safePitchTextBox, "data.SafePitch");
Bind(safeRollTextBox, "data.SafeRoll");
Bind(pitchKpTextBox, "data.PitchPid.Kp");
Bind(pitchKiTextBox, "data.PitchPid.Ki");
Bind(pitchKdTextBox, "data.PitchPid.Kd");
Bind(rollKpTextBox, "data.RollPid.Kp");
Bind(rollKiTextBox, "data.RollPid.Ki");
Bind(rollKdTextBox, "data.RollPid.Kd");
Bind(yawKpTextBox, "data.YawPid.Kp");
Bind(yawKiTextBox, "data.YawPid.Ki");
Bind(yawKdTextBox, "data.YawPid.Kd");
Bind(enableStabilizationCheckBox, "data.EnableStabilization");
Bind(negativeMixingCheckBox, "data.NegativeMixing");
Bind(keepMotorsOnCheckBox, "data.KeepMotorsOn");
Bind(maxThrustForFlyingTextBox, "data.MaxThrustForFlying");
Bind(onlyArmWhenStillCheckBox, "data.OnlyArmWhenStill");
Bind(gyroUseDmpCheckBox, "data.GyroUseDMP");
Bind(gyroUseRawCheckBox, "data.GyroUseRaw");
Bind(engineUseGyroCheckBox, "data.EngineUseGyro");
drone.OnSettingsChange += Drone_OnSettingsChange;
drone.OnInfoChange += Drone_OnInfoChange;
}
public LogForm(Drone drone)
{
InitializeComponent();
// Log Klasse vorbereiten
Log.OnFlushBuffer += Log_OnFlushBuffer;
Log.FlushBuffer();
Log.AutomaticFlush = true;
this.drone = drone;
drone.OnLogMessage += Drone_OnLogMessage;
}
public void Init(Drone drone)
{
if (drone == null)
throw new ArgumentNullException(nameof(drone));
this.drone = drone;
drone.OnDataChange += OnDroneDataChange;
drone.OnSettingsChange += Drone_OnSettingsChange;
UpdateValueBounds(drone.Settings);
UpdateServoValue();
UpdateEnabled(drone.Data.State);
}
public DebugForm(Drone drone, InputManager inputManager)
{
if (drone == null)
throw new ArgumentNullException(nameof(drone));
InitializeComponent();
this.Drone = drone;
this.Drone.OnDebugDataChange += Drone_OnDebugDataChange;
this.InputManager = inputManager;
UpdateDebugData(drone.DebugData);
}
public UpdateOTAForm(Drone drone)
{
InitializeComponent();
this.drone = drone;
this.ota = new DroneOTA(drone);
this.ota.OnProgress += (s, e) => UpdateProgress();
drone.OnDataChange += Drone_OnDataChange;
drone.OnConnected += Drone_OnConnected;
drone.OnDisconnect += Drone_OnDisconnect;
UpdateProgress();
}
public void Init(Drone drone)
{
if (drone == null)
throw new ArgumentNullException(nameof(drone));
this.drone = drone;
this.InputManager = new InputManager(drone);
this.InputManager.OnDeviceInfoChanged += InputManager_OnDeviceInfoChanged;
this.InputManager.OnTargetDataChanged += InputManager_OnTargetDataChanged;
this.drone.OnDebugDataChange += Drone_OnDebugDataChange;
SearchInputDevices();
UpdateTargetData();
UpdateDeviceInfo();
UpdateInputConfig();
}
public LIDAR(WorldHandle world, string name, PlayerInteraction Player, Drone drone)
{
//Register to actor insertion
_name = "LIDAR" + Guid.NewGuid().ToString();
_player = Player;
world.World.ActorAddedFiltered.Subscribe(new RegexFilter<IActor>("[.]ComplexObject[.]Body"), BindActor);
world.World.AddActuator(this);
_world = world.World;
_drone = drone;
desc = new RayDesc();
origin = new Vector3(0, (float)0.1, 0);
rayVec = new Vector3[1081];
iter = 0;
int index = 0;
for (double i = -45.0; i <= 225; i += 0.25)
{
rayVec[index] = new Vector3((float)(Math.Cos(i*(Math.PI/180.0))), 0, (float)(-1.0 * Math.Sin(i*(Math.PI/180.0))));
index++;
}
}
public GraphForm(Drone drone, FlightControl flightControl)
{
InitializeComponent();
this.Drone = drone;
this.Drone.OnSettingsChange += Drone_OnSettingsChange;
this.Drone.OnDataChange += Drone_OnDataChange;
this.FlightControl = flightControl;
UpdateSettings(Drone.Settings);
orientationGraphList.ValueMinimums = new double[] { -90, -90, 0 };
orientationGraphList.ValueMaximums = new double[] { 90, 90, 360 };
const double rotationRange = 25;
rotationGraphList.ValueMinimums = new double[] { -rotationRange, -rotationRange, -rotationRange };
rotationGraphList.ValueMaximums = new double[] { rotationRange, rotationRange, rotationRange };
const double accelerationRange = 2;
accelerationGraphList.ValueMinimums = new double[] { -accelerationRange, -accelerationRange, -accelerationRange };
accelerationGraphList.ValueMaximums = new double[] { accelerationRange, accelerationRange, accelerationRange };
}
public SettingsChangedEventArgs(Drone drone)
{
this.Settings = drone.Settings;
}
public InfoChangedEventArgs(Drone drone)
{
this.Info = drone.Info;
}
public PingChangedEventArgs(Drone drone)
{
this.IsConnected = drone.IsConnected;
this.Ping = drone.Ping;
}
public InputManager(Drone drone)
{
if (drone == null)
throw new ArgumentNullException(nameof(drone));
this.drone = drone;
}
private void OpenMainForm(Drone drone)
{
new MainForm(drone).Show();
Hide();
}
public DataChangedEventArgs(Drone drone)
{
this.Data = drone.Data;
}
public PingChangedEventArgs(Drone drone)
{
this.IsConnected = drone.IsConnected;
this.Ping = drone.Ping;
}
/// <summary>
/// Versucht eine Verbindung zur IP-Adresse aufzubauen.
/// </summary>
private void Connect()
{
Drone = new Drone(ipAddress, new Config());
Drone.OnConnected += OnDroneConnected;
// TODO: drone.Connect() einbauen, damit das Event schon gesetzt ist bevor wir verbinden
if (Drone.IsConnected) // schauen ob wir schon verbunden wurden, als wir das Event gesetzt haben
OnDroneConnected(this, EventArgs.Empty);
StartTimers();
}
/// <summary>
/// Initializes a new instance of Simulation class with a default window and a viewport
/// </summary>
public Simulation(EngineWindow window)
{
Path contentRoot = Path.CreateDirectory(@"..\..\..\Content\drones").GetAbsolute();
_viewport = window.AddViewport("Default", 1, 1, 0, 0, false);
_cameras = new Dictionary<string, PerspectiveCamera>();
_player = new PlayerInteraction(3030);
SimplyEnvironment env = SimplyEnvironment.Load(contentRoot + new Path("scene.env"));
SimplySim.Xna.Engine.Construction engineConstruction = new SimplySim.Xna.Engine.Construction(this);
SimplySim.Simulation.Engine.Construction simulationConstruction = new SimplySim.Simulation.Engine.Construction(engineConstruction);
WorldDesc worldDesc = simulationConstruction.GenerateWorldDesc(env);
worldDesc.FPS = 600;
worldDesc.MaxIter = 12;
WorldHandle world = AddWorld("DroneWorld", new NewtonWorld(worldDesc));
_scene = CreateSceneManager("Drone Simulation");
simulationConstruction.LoadEnvironment(env, world, _scene);
_actualCameraName = GlobalCameraName;
PerspectiveCamera globalCamera = (PerspectiveCamera)_scene[GlobalCameraName].Entities[0];
_viewport.Camera = globalCamera;
_cameras.Add(GlobalCameraName, globalCamera);
engineConstruction.AddControllers(env, _scene, _viewport);
_scene.PostProcessingManager.ComponentAddedFiltered.Subscribe(new RegexFilter<PostProcessingComponent>("Screen Space Ambient Occlusion"), BindSSAO);
#region Drones
// Box drone
_boxDrone = new Drone(world, BoxDroneName, _player);
_boxDroneHUD = new DroneSensorsHUD(_boxDrone, window.HUDManager);
FourHelixBoxDroneCommand _droneBoxController = new FourHelixBoxDroneCommand(_boxDrone.Rotors);
Parameter altParam = new Parameter(ParameterType.Altitude, new Coefficients(1, 0.1f, 0));
Parameter yawParam = new Parameter(ParameterType.Yaw, new Coefficients(0.5f, 0.1f, 0));
Parameter pitchParam = new Parameter(ParameterType.Pitch, new Coefficients(0.3f, 0.05f, 0.05f));
Parameter rollParam = new Parameter(ParameterType.Roll, new Coefficients(0.3f, 0.05f, 0.05f));
Parameter[] parameters = new Parameter[] { altParam, yawParam, pitchParam, rollParam };
//_fourHelixBoxDroneController = new KeyBoardPIDController(_boxDrone, 1, worldDesc.Gravity.Length(), _droneBoxController, parameters);
_fourHelixBoxDroneController = new PlayerController(_player, _boxDrone, 1, worldDesc.Gravity.Length(), _droneBoxController, parameters);
#endregion
world.World.ActorAddedFiltered.Subscribe(new RegexFilter<IActor> ("[.]ComplexObject[.]Body"), BindCameras);
_scene.Enabled = true;
world.Enabled = true;
}
public DataChangedEventArgs(Drone drone)
{
this.Data = drone.Data;
}
public InfoChangedEventArgs(Drone drone)
{
this.Info = drone.Info;
}
