private void UpdateStatus()
{
if (!droneControl.IsConnected)
{
labelCamera.Content = "No picture";
labelStatusCamera.Content = "None";
labelStatusBattery.Content = "N/A";
labelStatusAltitude.Content = "N/A";
labelStatusFrameRate.Content = "No video";
imageVideo.Source = null;
}
else
{
DroneData data = droneControl.NavigationData;
int frameRate = GetCurrentFrameRate();
ChangeCameraStatus();
labelStatusBattery.Content = data.BatteryLevel.ToString() + "%";
labelStatusAltitude.Content = data.Altitude.ToString();
labelStatusFrameRate.Content = frameRate.ToString();
}
labelStatusConnected.Content = droneControl.IsConnected.ToString();
labelStatusFlying.Content = droneControl.IsFlying.ToString();
labelStatusHovering.Content = droneControl.IsHovering.ToString();
UpdateCurrentBooleanInputState();
}
public SDrone(DroneData data, Drone drone)
{
count = DroneData.Count;
uid = data.UID;
totalDeliveryCount = data.DeliveryCount;
totalBatterySwaps = data.batterySwaps;
totalHubHandovers = data.hubsAssigned;
collisionOn = data.collisionOn;
isWaiting = data.isWaiting;
inHub = data.inHub;
movement = data.movement;
status = data.state;
totalDelay = data.totalDelay;
totalAudibleDuration = data.audibleDuration;
totalPackageWeight = data.packageWeight;
totalDistanceTravelled = data.distanceTravelled;
totalEnergy = data.totalEnergy;
targetAltitude = data.targetAltitude;
waypointsQueue = new List <SVector3>();
completedJobs = new List <uint>(data.completedJobs.Keys);
maxSpeed = data.MaxSpeed;
position = drone.transform.position;
previousWaypoint = data.previousWaypoint;
waypoint = data.currentWaypoint;
job = data.job;
hub = data.hub;
battery = data.battery;
charge = drone.GetBattery().Charge;
foreach (var point in data.waypoints)
{
waypointsQueue.Add(point);
}
}
IEnumerator FollowRoutine(int id, NetworkModule networkModule)
{
int lastTime = 0;
while (chasing)
{
StartCoroutine(networkModule.GetDrone(id, lastTime, null, 1, (info) =>
{
if (info != null)
{
string jsonFree = info.Remove(0, 1);
jsonFree = jsonFree.Remove(jsonFree.Length - 1, 1);
if (jsonFree.Length > 0)
{
DroneData drone_data = JsonUtility.FromJson <DroneData>(jsonFree);
lastTime = drone_data.timestamp;
downloadMapTexture(networkModule, drone_data.latitude, drone_data.longitude, followZoom);
}
}
})
);
yield return(new WaitForSeconds(2));
}
}
private void updateInstrument(AttitudeIndicatorInstrumentControl control, DroneData droneData)
{
control.BeginInvoke((MethodInvoker) delegate
{
control.SetAttitudeIndicatorParameters((droneData.Theta), -(droneData.Phi));
});
}
public void droneRadar(DroneData drone)
{
int dist = int.MaxValue;
List <Vector2Int> points = new List <Vector2Int>();
PlayerData lootPlayer = pm.getPlayerWithLoot();
if (lootPlayer != null)
{
moveTowardsPoint(drone.getToken(), lootPlayer.myToken.boardPosition);
return;
}
foreach (int id in pm.getIds())
{
Vector2Int pos = pm.getPlayerPosition(id);
int droneDist = board.distanceToTile(drone.getToken().boardPosition, pos);
if (droneDist < dist)
{
points.Clear();
dist = droneDist;
}
if (droneDist == dist)
{
points.Add(pos);
}
}
if (points.Count > 0)
{
moveTowardsPoint(drone.getToken(), points[UnityEngine.Random.Range(0, points.Count)]);
}
}
private void updateInstrument(AltimeterInstrumentControl control, DroneData droneData)
{
control.BeginInvoke((MethodInvoker) delegate
{
control.SetAlimeterParameters(droneData.Altitude);
});
}
// Use this for initialization
void Start()
{
_DroneData = GameObject.Find("ScriptObject");
_dm = (DroneData)_DroneData.GetComponent("DroneData");
_prof = (Preference)_DroneData.GetComponent("Preference");
txtCountList = (Text)GetComponent("Text");
dCount = new Dictionary <string, int>();
}
void Start()
{
displayUIImageFill = GetComponent <DisplayUIImageFill>();
droneData = GetComponent <DroneData>();
navMeshAgent = GetComponent <NavMeshAgent>();
droneCrash = transform.GetChild(1).GetComponent <DroneCrash>();
displayUIImageFill.Display((float)droneData.sO_Drone.currentFuelLevel / 100);
}
private void QuitToMainMenu()
{
SimManager.SimStatus = SimulationStatus.Paused;
SimManager.ClearObjects();
BatteryData.Reset();
DroneData.Reset();
HubData.Reset();
NFZData.Reset();
StartCoroutine(LoadMainMenu());
}
private void button11_Click(object sender, EventArgs e)
{
MAVLink.mavlink_set_mode_t cmd = new MAVLink.mavlink_set_mode_t();
cmd.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
cmd.custom_mode = 4;
cmd.target_system = 1;
DroneData drone = drones["bebop2"];
byte[] pkt = mavlinkParse.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.SET_MODE, cmd);
mavSock.SendTo(pkt, drone.ep);
}
public void OnGet(Transform parent = null)
{
_data = new DroneData(this);
SimManager.AllDrones.Add(_data.UID, this);
transform.SetParent(parent);
gameObject.SetActive(true);
InPool = false;
_waitForAltitude = new WaitUntil(ReachedAltitude);
_waitForWaypoint = new WaitUntil(ReachedWaypoint);
_waitForDeployment = new WaitUntil(() => !_data.isWaiting);
}
protected override void ResetVariables()
{
base.ResetVariables();
currentNavigationDataStruct = new NavigationDataStruct();
currentNavigationDataHeaderStruct = new NavigationDataHeaderStruct();
currentNavigationData = new DroneData();
currentSequenceNumber = initialSequenceNumber;
}
public override void InitializeAgent()
{
Data = new DroneData();
Drone = GetComponentInChildren <Drone>();
Drone.Initialize();
World = GetComponentInChildren <BlockWorld>();
World.Initialize();
Cam = GetComponentInChildren <Cam>();
Cam.Initialize();
}
public void OnGet(Transform parent = null)
{
_Data = new DroneData(this);
SimManager.AllDrones.Add(_Data.UID, this);
Trail.enabled = true;
transform.SetParent(parent);
gameObject.SetActive(true);
JobManager.AddToQueue(this);
InPool = false;
StartCoroutine(PollRoute());
}
private void DetermineNavigationData(byte[] buffer, int position)
{
unsafe
{
fixed(byte *entry = &buffer[position])
{
currentNavigationDataStruct = *(NavigationDataStruct *)entry;
}
}
currentNavigationData = new DroneData(currentNavigationDataStruct);
}
private void button9_Click(object sender, EventArgs e)
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t();
cmd.command = (ushort)MAVLink.MAV_CMD.TAKEOFF;
cmd.target_system = 1;
//cmd.target_component = 250;
cmd.param7 = 1.2f;
DroneData drone = drones["bebop2"];
byte[] pkt = mavlinkParse.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
mavSock.SendTo(pkt, drone.ep);
}
private void button12_Click(object sender, EventArgs e)
{
MAVLink.mavlink_set_gps_global_origin_t cmd = new MAVLink.mavlink_set_gps_global_origin_t();
cmd.target_system = 1;
cmd.latitude = (int)(24.7733321 * 10000000);
cmd.longitude = (int)(121.0449535 * 10000000);
cmd.altitude = 100;
DroneData drone = drones["bebop2"];
byte[] pkt = mavlinkParse.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.SET_GPS_GLOBAL_ORIGIN, cmd);
mavSock.SendTo(pkt, drone.ep);
}
private void button8_Click(object sender, EventArgs e)
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t();
cmd.command = (ushort)MAVLink.MAV_CMD.COMPONENT_ARM_DISARM;
cmd.target_system = 1;
cmd.param1 = 0;
cmd.param2 = 21196;
byte[] pkt = mavlinkParse.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
DroneData drone = drones["bebop2"];
mavSock.SendTo(pkt, drone.ep);
}
private void UpdateHudStatus()
{
if (droneControl.IsConnected)
{
DroneData data = droneControl.NavigationData;
hudInterface.SetFlightVariables(data.Phi, data.Theta, data.Psi);
hudInterface.SetAltitude(data.Altitude);
hudInterface.SetOverallSpeed(data.VX, data.VY, data.VZ);
hudInterface.SetBatteryLevel(data.BatteryLevel);
}
}
public void addNavDataPoint()
{
InputState inputState = inputManager.lastInputState;
DroneData droneData = droneControl.NavigationData;
XElement navDataPoint = new XElement("Data");
navDataPoint.Add(
new XElement("Info",
new XElement("TimeStamp", DateTime.Now.ToString()),
new XElement("Time_ms", (DateTime.Now - startTime).TotalMilliseconds),
new XElement("DataPoint", dataPoints++)
)
);
if (inputState != null)
{
navDataPoint.Add(
new XElement("InputState",
new XElement("Roll", inputState.Roll),
new XElement("Pitch", inputState.Pitch),
new XElement("Yaw", inputState.Yaw),
new XElement("Gaz", inputState.Gaz),
new XElement("CameraSwap", inputState.CameraSwap),
new XElement("Emergency", inputState.Emergency),
new XElement("FlatTrim", inputState.FlatTrim),
new XElement("Hover", inputState.Hover),
new XElement("Land", inputState.Land),
new XElement("TakeOff", inputState.TakeOff)
)
);
}
if (droneData != null)
{
navDataPoint.Add(
new XElement("DroneData",
new XElement("Theta", droneData.Theta),
new XElement("Phi", droneData.Phi),
new XElement("Psi", droneData.Psi),
new XElement("VX", droneData.VX),
new XElement("VY", droneData.VY),
new XElement("VZ", droneData.VZ),
new XElement("Altitude", droneData.Altitude),
new XElement("BatteryLevel", droneData.batteryLevel)
)
);
}
xNavData.Add(navDataPoint);
}
private void updateInstrument(HeadingIndicatorInstrumentControl control, DroneData droneData)
{
control.BeginInvoke((MethodInvoker) delegate
{
// Psi range -180..0..180 but heading indicator require 0..360
if (droneData.Psi > 0)
{
control.SetHeadingIndicatorParameters(Convert.ToInt32(droneData.Psi));
}
else
{
control.SetHeadingIndicatorParameters(360 + Convert.ToInt32(droneData.Psi));
}
});
}
private void spawnDrones()
{
int droneCount = RuleManager.Instance.minNumberOfDrones;
if (pm.Count >= RuleManager.Instance.minPlayersForExtraDrone)
{
droneCount++;
}
for (int i = 0; i < droneCount; i++)
{
Token t = spawnToken(DronePrefab, resources.droneColors[i]);
DroneData dd = new DroneData(-(i + 1));
dd.setToken(t);
drones.Add(dd);
}
}
private void button10_Click(object sender, EventArgs e)
{
MAVLink.mavlink_set_position_target_local_ned_t cmd = new MAVLink.mavlink_set_position_target_local_ned_t();
cmd.target_system = 1;
cmd.coordinate_frame = (byte)MAVLink.MAV_FRAME.LOCAL_NED;
cmd.type_mask = 0xff8;
cmd.x = 0.0f + float.Parse(textBox3.Text);
label33.Text = Convert.ToString(cmd.x);
cmd.y = 0.0f + float.Parse(textBox4.Text);
label34.Text = Convert.ToString(cmd.y);
cmd.z = -1.2f;
DroneData drone = drones["bebop2"];
byte[] pkt = mavlinkParse.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.SET_POSITION_TARGET_LOCAL_NED, cmd);
mavSock.SendTo(pkt, drone.ep);
}
public void SetDroneData(DroneData ActiveDroneData)
{
_DroneData = ActiveDroneData;
Decimal.TryParse(_DroneData.BBFlightID, out _BBFlightID);
_DroneID = (int)_DroneData.DroneId;
try {
_ReadTime = DateTime.ParseExact(_DroneData.ReadTime, "dd/MM/yyyy-HH:mm:ss", null);
_DroneFlight.FlightDate = _ReadTime;
} catch (Exception ex) {
OnTaskError?.Invoke(new TaskExceptionInfo {
TaskException = ex,
TaskName = "SetDroneData - _ReadTime Parse Error"
});
}
_SetFlightMapData();
}
private void updateInstruments()
{
DroneData droneData = null;
if (droneControl.IsConnected)
{
droneData = droneControl.NavigationData;
}
else
{
droneData = new DroneData();
}
foreach (InstrumentControl instrumentControl in instrumentList)
{
try
{
switch (instrumentControl.GetType().Name)
{
case "AttitudeIndicatorInstrumentControl":
updateInstrument((AttitudeIndicatorInstrumentControl)instrumentControl, droneData);
break;
case "AltimeterInstrumentControl":
updateInstrument((AltimeterInstrumentControl)instrumentControl, droneData);
break;
case "HeadingIndicatorInstrumentControl":
updateInstrument((HeadingIndicatorInstrumentControl)instrumentControl, droneData);
break;
case "VerticalSpeedIndicatorInstrumentControl":
updateInstrument((VerticalSpeedIndicatorInstrumentControl)instrumentControl, droneData);
break;
}
}
catch (InvalidOperationException)
{ }
catch (Exception)
{
this.stopManage();
}
}
}
// Use this for initialization
void Start()
{
// Prvy dron je vzdy ten defaultny
//string path = Application.streamingAssetsPath + "/mission.json";
//if(File.Exists(path))
//{
// string jsonContent = File.ReadAllText(path);
// bool parse = true;
//}
//Mission mission;
// try
// {
// JsonUtility.FromJson<Mission>(jsonContent);
// }
// catch (System.Exception)
// {
// parse = false;
// }
// if(parse){
// mission = JsonUtility.FromJson<Mission>(jsonContent);
// Vector3 pos;
// Mapbox.Utils.Vector2d p = new Mapbox.Utils.Vector2d(mission.drones[0].latitude,mission.drones[0].longitude);
// pos = Map.GeoToWorldPosition(p,false);
// float groundAltitude = Map.QueryElevationInUnityUnitsAt(Map.WorldToGeoPosition(pos));
// pos.y = groundAltitude;
// positionDataS = new DroneData(Map, pos);
// positionData = positionDataM = new DroneDataManual(Map, pos);
// positionDataR = new DroneRosData(Map, pos);
// }else{
positionDataS = new DroneData(Map, transform.position);
positionDataM = new DroneDataManual(Map, transform.position);
positionDataR = new DroneRosData(Map, transform.position);
positionData = positionDataM;
// }
// Generate Unique ID for our drone
drone = new Drone(transform.gameObject, new DroneFlightData());
drone.FlightData.DroneId = GetUniqueID();
Drones.drones.Add(drone);
}
private void UpdateStatus()
{
if (!droneControl.IsConnected)
{
labelStatusCamera.Text = "No picture";
labelStatusBattery.Text = "N/A";
labelStatusAltitude.Text = "N/A";
labelStatusPitch.Text = "+0.0000°";
labelStatusRoll.Text = "+0.0000°";
labelStatusYaw.Text = "+0.0000°";
labelStatusGaz.Text = "+0.0000°";
labelStatusFrameRate.Text = "No video";
//imageVideo.Source = null;
}
else
{
DroneData data = droneControl.NavigationData;
int frameRate = GetCurrentFrameRate();
ChangeCameraStatus();
labelStatusBattery.Text = data.BatteryLevel + "%";
labelStatusAltitude.Text = data.Altitude.ToString();
labelStatusPitch.Text = String.Format("{0:+0.000;-0.000;+0.000}", data.Theta);
labelStatusRoll.Text = String.Format("{0:+0.000;-0.000;+0.000}", data.Phi);
labelStatusYaw.Text = String.Format("{0:+0.000;-0.000;+0.000}", data.Psi);
labelStatusVX.Text = String.Format("{0:+0.000;-0.000;+0.000}", data.VX);
labelStatusRoll.Text = String.Format("{0:+0.000;-0.000;+0.000}", data.Phi);
labelStatusYaw.Text = String.Format("{0:+0.000;-0.000;+0.000}", data.Psi);
//labelStatusGaz.Text = String.Format("{0:+0.000;-0.000;+0.000}", data.);
}
labelStatusConnected.Text = droneControl.IsConnected.ToString();
labelStatusFlying.Text = droneControl.IsFlying.ToString();
labelStatusHovering.Text = droneControl.IsHovering.ToString();
UpdateCurrentBooleanInputState();
}
public void Dispatch()
{
if (DroneManager.instance.currentDrone != null)
{
droneData = DroneManager.instance.currentDrone.GetComponent <DroneData>();
}
if (!droneData.isDispatched && DroneManager.instance.currentDrone != null && DroneManager.instance.currentDestination != null)
{
droneData.destAir = DroneManager.instance.currentDestination.GetComponent <DestData>().dropspotAir;
droneData.destGround = DroneManager.instance.currentDestination.GetComponent <DestData>().dropspotGround;
droneData.sO_Package = DroneManager.instance.currentPackage;
droneData.sO_Destination = DroneManager.instance.currentsO_Destination;
droneData.droneMesh.GetComponent <DroneVertMovement>().Ascend();
droneData.sO_Package.isEnRoute = true;
droneData.hasPackage = true;
droneData.isDispatched = true;
DroneManager.instance.currentDrone.GetComponent <DroneAlertSystem>().OnLeftWarehouse();
droneData.StartDeliveryTimer();
droneData.GetComponentInChildren <DroneCrash>().StartCalcCrash();
droneData.droneButton.SetActive(false);
DestinationButtonManager.instance.lastButtonClicked.mapButton.SetActive(false);
canDispatchEvent.Invoke();
}
else if (DroneManager.instance.currentDrone == null)
{
noDroneEvent.Invoke();
}
else if (DroneManager.instance.currentDestination == null)
{
noDestEvent.Invoke();
}
else
{
cantDispatchEvent.Invoke();
}
}
public Point Scan(float hrz, float vrt, float range)
{
RaycastHit hit;
Vector3 scan = new Vector3(hrz, vrt, 1f);
Ray ray = new Ray(Position, Quaternion.Euler(vrt * 180f, hrz * 180f, 0f) * Vector3.forward);
Point point = new Point(PointType.ScanOutOfRange, Position + ray.direction * range, Time.time);
laser.enabled = Physics.Raycast(ray.origin, ray.direction, out hit, range, layerMask);
if (laser.enabled)
{
scan.z = DroneData.NormalizeDistance(hit.distance);
// Grid nodes align with blocks:
// Offset point slightly so it doesn't sit right on the boundary between two nodes.
point.Position = ray.origin + ray.direction * (hit.distance + 0.01f);
point.Type = PointType.ScanPoint;
laser.SetPosition(0, ray.origin);
laser.SetPosition(1, hit.point);
}
ScanBuffer.Add(scan);
return(point);
}
private void UpdateStatus()
{
if (!droneControl.IsConnected)
{
labelCamera.Text = "No picture";
labelStatusPitch.Text = "+0.0000°";
labelStatusRoll.Text = "+0.0000°";
}
else
{
DroneData data = droneControl.NavigationData;
labelCamera.Text = "Bottom camera";
labelStatusPitch.Text = String.Format("{0:+0.000;-0.000;+0.000}", data.Theta);
labelStatusRoll.Text = String.Format("{0:+0.000;-0.000;+0.000}", data.Phi);
labelStatusBattery.Text = data.BatteryLevel + "%";
}
labelStatusConnected.Text = droneControl.IsConnected.ToString();
labelStatusFlying.Text = droneControl.IsFlying.ToString();
labelStatusHovering.Text = droneControl.IsHovering.ToString();
}
private void UpdateData(DroneData data)
{
SuspendLayout();
if (!drone.IsConnected)
{
statusArmedLabel.ForeColor = Color.Red;
statusArmedLabel.Text = "Status: not connected";
if (data.State != DroneState.Unknown)
statusArmedLabel.Text += string.Format(" (last: {0})", data.State);
statusButton.Enabled = false;
wifiRssiLabel.Visible = false;
}
else
{
statusButton.Enabled = true;
switch (drone.Data.State)
{
case DroneState.Unknown:
statusButton.Enabled = false;
statusArmedLabel.ForeColor = Color.DarkRed;
statusButton.Text = "Unknown";
break;
case DroneState.Stopped:
case DroneState.Reset:
statusArmedLabel.ForeColor = Color.Green;
statusButton.Text = "Clear";
break;
case DroneState.Idle:
statusArmedLabel.ForeColor = Color.DarkGreen;
statusButton.Text = "Arm";
break;
case DroneState.Armed:
case DroneState.Flying:
statusArmedLabel.ForeColor = Color.DarkOrange;
statusButton.Text = "Disarm";
break;
}
statusArmedLabel.Text = $"Status: {data.State}";
// RSSI ist immer unter 0, wenn die Drohne mit einem Netzwerk verbunden ist
wifiRssiLabel.Visible = data.WifiRssi < 0;
StringBuilder wifiText = new StringBuilder();
wifiText.Append("WiFi signal: ");
wifiText.Append(data.WifiRssi);
wifiText.Append("dBm ");
if (data.WifiRssi > -40)
{
wifiText.Append("very good");
wifiRssiLabel.ForeColor = Color.DarkGreen;
}
else if (data.WifiRssi > -70)
{
wifiText.Append("good");
wifiRssiLabel.ForeColor = Color.Green;
}
else
{
wifiText.Append("bad");
wifiRssiLabel.ForeColor = Color.DarkRed;
}
wifiRssiLabel.Text = wifiText.ToString();
}
ResumeLayout();
}
private void HandleDataPacket(byte[] packet)
{
using (MemoryStream stream = new MemoryStream(packet))
{
PacketBuffer buffer = new PacketBuffer(stream);
if (buffer.Size < 3 || buffer.ReadByte() != 'F' || buffer.ReadByte() != 'L' || buffer.ReadByte() != 'Y')
return;
int revision = buffer.ReadInt();
DataPacketType type = (DataPacketType)buffer.ReadByte();
lastDataTime = Environment.TickCount;
switch (type)
{
case DataPacketType.Drone:
if (!CheckRevision(lastDataDroneRevision, revision))
return;
DroneState state = (DroneState)buffer.ReadByte();
QuadMotorValues motorValues = new QuadMotorValues(buffer);
GyroData gyro = new GyroData(buffer);
float batteryVoltage = buffer.ReadFloat();
int wifiRssi = buffer.ReadInt();
Data = new DroneData(state, motorValues, gyro, batteryVoltage, wifiRssi);
lastDataDroneRevision = revision;
break;
case DataPacketType.Log:
if (!CheckRevision(lastDataLogRevision, revision))
return;
int lines = buffer.ReadInt();
for (int i = 0; i < lines; i++)
{
string msg = buffer.ReadString();
if (OnLogMessage == null)
Log.Info("[Drone] " + msg);
else
OnLogMessage(msg + Environment.NewLine);
}
lastDataLogRevision = revision;
break;
case DataPacketType.Debug:
if (!CheckRevision(lastDataDebugRevision, revision))
return;
DebugData = new DebugData(buffer);
lastDataDebugRevision = revision;
break;
}
}
}
public DataChangedEventArgs(Drone drone)
{
this.Data = drone.Data;
}
