/// <summary>
/// Simple network discovery
/// TODO : To Improve
/// </summary>
private void StartPingThread(string _deviceIp)
{
Task.Run(() =>
{
while (Ping_Should_run)
{
Ping pinger = new Ping();
try
{
PingReply reply = pinger.Send(_deviceIp, 100);
if (reply.Status == IPStatus.Success)
{
sumoInformations.DeviceName = _deviceIp;
SumoEventArgs evtArgs = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.Discovered, null)
{
SumoInformations = sumoInformations
};
OnSumoEvents(evtArgs);
if (!AlwaysPing)
{
return;
}
}
}
catch (PingException)
{
// Discard PingExceptions
}
Thread.Sleep(1000);
}
});
}
protected virtual void OnSumoEvents(SumoEventArgs e)
{
SumoEvents?.Invoke(this, e);
}
private void _process_frame(byte[] frame)
{
var Result = _read_header(frame);
byte data_type = Result.Item1;
byte buffer_id = Result.Item2;
byte seq_no = Result.Item3;
UInt32 frame_size = Result.Item4;
byte[] payload = frame.SubArray("7:");
// We received an ACK for a packet we sent
if (data_type == (byte)SumoConstantsCustom.ARNETWORKAL_FRAME_TYPE.ARNETWORKAL_FRAME_TYPE_ACK)
{
LOGGER.GetInstance.Debug("ACK packet received");
}
else if (data_type == (byte)SumoConstantsCustom.ARNETWORKAL_FRAME_TYPE.ARNETWORKAL_FRAME_TYPE_DATA ||
data_type == (byte)SumoConstantsCustom.ARNETWORKAL_FRAME_TYPE.ARNETWORKAL_FRAME_TYPE_DATA_WITH_ACK)
{
// Frame requires an ACK, send one
if (data_type == (byte)SumoConstantsCustom.ARNETWORKAL_FRAME_TYPE.ARNETWORKAL_FRAME_TYPE_DATA_WITH_ACK)
{
byte[] ack = _create_ack_packet(data_type, buffer_id, seq_no);
this._sender.SendAck(ack);
//LOGGER.GetInstance.Debug(String.Format("Sending ACK for {0} {1} {2} {3}", data_type, buffer_id, seq_no, frame_size));
}
// We received a data packet
var Result1 = StructConverter.Unpack("<BBH", payload.SubArray(":4"));
byte cmd_project = (byte)Result1[0];
byte cmd_class = (byte)Result1[1];
UInt16 cmd_id = (UInt16)Result1[2];
if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.common_SettingsState_ProductNameChanged))
{
// Product name Changed
var name = Encoding.ASCII.GetString(payload.SubArray("4:")).Trim('\0');
LOGGER.GetInstance.Info(String.Format("Drone Name: {0}", name));
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.common_CommonState_CurrentDateChanged))
{
var date = payload.SubArray("4:");
LOGGER.GetInstance.Info(String.Format("Date updated to: {0}", Encoding.ASCII.GetString(date)));
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.common_CommonState_CurrentTimeChanged))
{
var time = payload.SubArray("4:");
LOGGER.GetInstance.Info(String.Format("Time updated to: {0}", Encoding.ASCII.GetString(time)));
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.common_CommonState_WifiSignalChanged))
{
Int16 rssi = (Int16)StructConverter.Unpack("<h", payload.SubArray("4:"))[0];
_sumoInformations.Rssi = rssi;
var evt = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.RSSI, _sumoInformations);
OnSumoEvents(evt);
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.common_CommonState_BatteryStateChanged))
{
var battery = payload.SubArray("4:5")[0];
BatteryLevel = battery;
_sumoInformations.BatteryLevel = BatteryLevel;
var BatEvt = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.BatteryLevelEvent, _sumoInformations);
OnSumoEvents(BatEvt);
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.common_CommonState_AllStatesChanged))
{
// All states have been sent
var State = payload.SubArray("4:");
LOGGER.GetInstance.Debug(String.Format("All states have been sent: {0}", BitConverter.ToString(State)));
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.common_CommonState_ProductModel))
{
// Device model
byte Model = payload.SubArray("4:")[0];
LOGGER.GetInstance.Info(String.Format("Device Model: {1} {0}", Model, (SumoEnumGenerated.ProductModel_model)Model).ToString());
// Add Capabilities
_sumoInformations.BuildCapabilities((SumoEnumGenerated.ProductModel_model)Model);
var CapEvt = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.CapabilitiesChange, _sumoInformations);
OnSumoEvents(CapEvt);
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.common_HeadlightsState_intensityChanged))
{
var HeadData = StructConverter.Unpack("<BB", payload.SubArray("4:"));
LOGGER.GetInstance.Debug(String.Format("Headlight change: {0} {1}", HeadData[0], HeadData[1]));
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.common_AudioState_AudioStreamingRunning))
{
var AudioStateData = payload.SubArray("4:")[0];
if ((AudioStateData & (1 << 0)) != 0)
{
LOGGER.GetInstance.Info(String.Format("Drone is ready to stream to controller (Drone TX ON)"));
if (_audioPlayer != null)
{
_audioPlayer.Start();
}
}
else if ((AudioStateData & (1 << 0)) == 0)
{
LOGGER.GetInstance.Info(String.Format("Drone has cancelled to stream to controller (Drone TX OFF)"));
if (_audioPlayer != null)
{
_audioPlayer.Stop();
}
}
if ((AudioStateData & (1 << 1)) != 0)
{
LOGGER.GetInstance.Info(String.Format("Drone is ready to receive stream from Controller (Drone RX ON)"));
}
else if ((AudioStateData & (1 << 1)) == 0)
{
LOGGER.GetInstance.Info(String.Format("Drone has cancelled to receive stream from Controller (Drone RX OFF)"));
}
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_PilotingState_SpeedChanged))
{
var Result2 = StructConverter.Unpack("<bh", payload.SubArray("4:"));
sbyte speed = (sbyte)Result2[0];
Int16 real_speed = (Int16)Result2[1];
LOGGER.GetInstance.Debug(String.Format("Speed updated to {0} ({1} cm/s)", speed, real_speed));
_sumoInformations.Speed = speed;
var evt = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.SpeedChange, _sumoInformations);
OnSumoEvents(evt);
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_PilotingState_PostureChanged))
{
var state = (Int32)StructConverter.Unpack("<i", payload.SubArray("4:"))[0];
LOGGER.GetInstance.Debug(String.Format("State of posture changed: {1} ({0})", state, (SumoEnumGenerated.PostureChanged_state)state).ToString());
_sumoInformations.Posture = (SumoEnumGenerated.PostureChanged_state)state;
var evt = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.PostureEvent, _sumoInformations);
OnSumoEvents(evt);
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_AnimationsState_JumpLoadChanged))
{
var state = (Int32)StructConverter.Unpack("<i", payload.SubArray("4:"))[0];
LOGGER.GetInstance.Warn(String.Format("State of jump load changed: {1} ({0})", state, (SumoEnumGenerated.JumpLoadChanged_state)state).ToString());
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_AnimationsState_JumpTypeChanged))
{
var state = (Int32)StructConverter.Unpack("<i", payload.SubArray("4:"))[0];
LOGGER.GetInstance.Warn(String.Format("State of jump type changed: {1} ({0})", state, (SumoEnumGenerated.JumpTypeChanged_state)state).ToString());
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_AnimationsState_JumpMotorProblemChanged))
{
var state = (Int32)StructConverter.Unpack("<i", payload.SubArray("4:"))[0];
LOGGER.GetInstance.Error(String.Format("State about the jump motor problem: {1} ({0})", state, (SumoEnumGenerated.JumpMotorProblemChanged_error)state).ToString());
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_NetworkState_LinkQualityChanged))
{
var linkQuality = (byte)StructConverter.Unpack("<B", payload.SubArray("4:"))[0];
_sumoInformations.LinkQuality = linkQuality;
var evt = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.LinkQuality, _sumoInformations);
OnSumoEvents(evt);
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_MediaStreamingState_VideoEnableChanged))
{
var state = (Int32)StructConverter.Unpack("<i", payload.SubArray("4:"))[0];
LOGGER.GetInstance.Info(String.Format("Media streaming state is: {1}({0})", state, (SumoEnumGenerated.VideoStateChangedV2_state)state).ToString());
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_AudioSettingsState_MasterVolumeChanged))
{
var VolumeState = (byte)payload.SubArray("4:")[0];
LOGGER.GetInstance.Debug(String.Format("Volume state is: {0}", VolumeState));
_sumoInformations.Volume = VolumeState;
var evt = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.VolumeChange, _sumoInformations);
OnSumoEvents(evt);
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_PilotingState_AlertStateChanged))
{
var state = (Int32)StructConverter.Unpack("<i", payload.SubArray("4:"))[0];
_sumoInformations.Alert = (SumoEnumGenerated.AlertStateChanged_state)state;
var evt = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.AlertEvent, _sumoInformations);
OnSumoEvents(evt);
LOGGER.GetInstance.Debug(String.Format("AlertStateChanged: {0}", ((SumoEnumGenerated.AlertStateChanged_state)state).ToString()));
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_NetworkSettingsState_WifiSelectionChanged))
{
var Wifi = StructConverter.Unpack("<iiB", payload.SubArray("4:"));
int WifiType = (int)Wifi[0];
int WifiBand = (int)Wifi[1];
byte WifiChannel = (byte)Wifi[2];
LOGGER.GetInstance.Info(String.Format("Wifi Type:{0} Band:{1} Channel:{2}", ((SumoEnumGenerated.WifiSelectionChanged_type)WifiType).ToString(), ((SumoEnumGenerated.WifiSelectionChanged_band)WifiBand).ToString(), WifiChannel));
_sumoInformations.WifiType = (SumoEnumGenerated.WifiSelectionChanged_type)WifiType;
_sumoInformations.WifiBand = (SumoEnumGenerated.WifiSelectionChanged_band)WifiBand;
_sumoInformations.WifiChannel = WifiChannel;
var evt = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.WifiChanged, _sumoInformations);
OnSumoEvents(evt);
}
else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_AudioSettingsState_ThemeChanged))
{
var AudioTheme = (byte)StructConverter.Unpack("<B", payload.SubArray("4:"))[0];
LOGGER.GetInstance.Info(String.Format("Audio Theme Changed: {0} ", ((SumoEnumGenerated.ThemeChanged_theme)AudioTheme).ToString()));
}
else
{
var data = payload.SubArray("4:");
LOGGER.GetInstance.Debug(String.Format("DataFrame | Project: {0}, Class: {1}, Id: {2} data: {3}", cmd_project, cmd_class, cmd_id, BitConverter.ToString(data)));
}
}
else if (data_type == (byte)SumoConstantsCustom.ARNETWORKAL_FRAME_TYPE.ARNETWORKAL_FRAME_TYPE_DATA_LOW_LATENCY)
{
// We received an ARStream packet, process it
this._process_stream_frame(buffer_id, frame_size, payload);
}
else
{
LOGGER.GetInstance.Warn(String.Format("Unknown header type: {0} {1} {2} {3}", data_type, buffer_id, seq_no, frame_size));
}
}
private void Receiver_SumoEvents(object sender, SumoEventArgs e)
{
OnSumoEvents(e);
}
private void Controller_SumoEvents(object sender, SumoEventArgs e)
{
if (e.SumoInformations == null)
{
return;
}
sumoInformations = e.SumoInformations;
switch (e.TypeOfEvent)
{
case (SumoEnumCustom.TypeOfEvents.AlertEvent):
break;
case (SumoEnumCustom.TypeOfEvents.BatteryLevelEvent):
lblBatteryLevel.Dispatcher.BeginInvoke((Action)(() =>
{
lblBatteryLevel.Content = sumoInformations.BatteryLevel + "%";
}));
break;
case (SumoEnumCustom.TypeOfEvents.Connected):
// Enable Btn
InitUI();
break;
case (SumoEnumCustom.TypeOfEvents.Disconnected):
break;
case (SumoEnumCustom.TypeOfEvents.Discovered):
txtBox.Dispatcher.BeginInvoke((Action)(() =>
{
txtBox.AppendText(String.Format("Sumo {1} is available {0} ", Environment.NewLine, sumoInformations.DeviceName));
txtBox.ScrollToEnd();
}));
#if MULTIPLEDRONES
// Get btn reference for multidrone And display it
try
{
MiniDevice d = ListOfDevices.Find(x => x.Name == sumoInformations.DeviceName);
Button btn = d.Button;
int suffixe = d.Id;
btn.Dispatcher.BeginInvoke((Action)(() =>
{
btn.Content = prefixName + suffixe.ToString();
btn.Visibility = Visibility.Visible;
}));
}
catch { }
#endif
break;
case (SumoEnumCustom.TypeOfEvents.PilotingEvent):
break;
case (SumoEnumCustom.TypeOfEvents.PostureEvent):
lblPostureState.Dispatcher.BeginInvoke((Action)(() =>
{
lblPostureState.Content = "Sumo in: " + sumoInformations.Posture.ToString() + " position";
}));
break;
case (SumoEnumCustom.TypeOfEvents.RSSI):
lblRssi.Dispatcher.BeginInvoke((Action)(() =>
{
lblRssi.Content = "Wifi Signal : " + sumoInformations.Rssi.ToString() + " dbm";
}));
break;
case (SumoEnumCustom.TypeOfEvents.LinkQuality):
lblQuality.Dispatcher.BeginInvoke((Action)(() =>
{
lblQuality.Content = "Link Quality: " + sumoInformations.LinkQuality.ToString() + "/6";
}));
break;
case (SumoEnumCustom.TypeOfEvents.VolumeChange):
slVolume.Dispatcher.BeginInvoke((Action)(() =>
{
slVolume.Value = sumoInformations.Volume;
}));
break;
case (SumoEnumCustom.TypeOfEvents.CapabilitiesChange):
chkBox.Dispatcher.BeginInvoke((Action)(() =>
{
chkBox.IsChecked = sumoInformations.IsCapapableOf(SumoInformations.Capability.Box);
chkBoost.IsChecked = sumoInformations.IsCapapableOf(SumoInformations.Capability.Boost);
pnlAudio.IsEnabled = sumoInformations.IsCapapableOf(SumoInformations.Capability.Audio);
slLight.IsEnabled = sumoInformations.IsCapapableOf(SumoInformations.Capability.Light);
if (pnlAudio.IsEnabled)
{
InitSoundUI();
controller.InitAudio();
}
}));
break;
case (SumoEnumCustom.TypeOfEvents.WifiChanged):
cbxWifiBand.Dispatcher.BeginInvoke((Action)(() =>
{
cbxWifiBand.SelectedIndex = (int)sumoInformations.WifiBand;
}));
break;
case (SumoEnumCustom.TypeOfEvents.AudioThemeChanged):
cbxAudioTheme.Dispatcher.BeginInvoke((Action)(() =>
{
cbxAudioTheme.SelectedIndex = (int)sumoInformations.AudioTheme;
}));
break;
}
}
