forked from CPL/Simulator
Добавлен сервер для отправки телеметрии
This commit is contained in:
@ -3,170 +3,278 @@ using System.Numerics;
|
||||
using System.Windows.Forms;
|
||||
using static DroneSimulator.NetClient;
|
||||
using DroneClient;
|
||||
using TelemetryIO.Models;
|
||||
using TelemetryIO;
|
||||
using DroneClient.Models;
|
||||
|
||||
namespace DroneSimulator
|
||||
{
|
||||
public partial class Form_Main : Form
|
||||
{
|
||||
private NetClient netClient = new NetClient();
|
||||
|
||||
public Form_Main()
|
||||
public partial class Form_Main : Form
|
||||
{
|
||||
InitializeComponent();
|
||||
}
|
||||
private NetClient netClient = new NetClient();
|
||||
private Telemetry telemetry = Telemetry.Instance;
|
||||
private MonitorContainer monitoring = MonitorContainer.Instance;
|
||||
private System.Windows.Forms.Timer copyDataTimer = new System.Windows.Forms.Timer();
|
||||
private System.Windows.Forms.Timer MonitoringTimer = new System.Windows.Forms.Timer();
|
||||
TelemetryServer tele_server;
|
||||
string connectIco = Path.Combine(Application.StartupPath, "Images", "connect.ico");
|
||||
string disconnectIco = Path.Combine(Application.StartupPath, "Images", "disconnect.ico");
|
||||
|
||||
private void ConnectionCallback(object o)
|
||||
{
|
||||
ConnectData data = (ConnectData)o;
|
||||
|
||||
if (!data.Connect)
|
||||
{
|
||||
try
|
||||
public Form_Main()
|
||||
{
|
||||
Invoke((MethodInvoker)delegate
|
||||
{
|
||||
button_Connect.Text = "Connect";
|
||||
button_Connect.BackColor = Color.Transparent;
|
||||
MessageBox.Show("Connection closed");
|
||||
});
|
||||
InitializeComponent();
|
||||
tele_server = new TelemetryServer();
|
||||
if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco);
|
||||
}
|
||||
catch { }
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
Drone dataDrone = new Drone();
|
||||
|
||||
private void ReceiveCallback(object o)
|
||||
{
|
||||
ReceiveData data = (ReceiveData)o;
|
||||
|
||||
List<byte[]?>? send = dataDrone.DataStream(data.Buffer, data.Size);
|
||||
|
||||
if (send == null) return;
|
||||
try
|
||||
{
|
||||
foreach (byte[]? b in send)
|
||||
private void Tele_server_OnTeleClientDisconnected(object? sender, EventArgs e)
|
||||
{
|
||||
if (b != null) data.Server?.Send(b);
|
||||
if (TeleClientStatusPicture.InvokeRequired)
|
||||
{
|
||||
TeleClientStatusPicture.Invoke(new Action(() => { if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco); }));
|
||||
}
|
||||
else
|
||||
{
|
||||
if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco);
|
||||
}
|
||||
monitoring.resetMonitorContainer();
|
||||
}
|
||||
|
||||
private void Tele_server_OnTeleClientСonnected(object? sender, EventArgs e)
|
||||
{
|
||||
if (TeleClientStatusPicture.InvokeRequired)
|
||||
{
|
||||
TeleClientStatusPicture.Invoke(new Action(() => { if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(connectIco); }));
|
||||
}
|
||||
else
|
||||
{
|
||||
if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(connectIco);
|
||||
}
|
||||
}
|
||||
|
||||
private void MonitoringTimer_Tick(object? sender, EventArgs e)
|
||||
{
|
||||
if (monitoring.isMonitor == 1)
|
||||
{
|
||||
byte[] load = new byte[monitoring.monitorFillLevel * sizeof(float)];
|
||||
for (int i = 0; i < monitoring.monitorFillLevel; i++)
|
||||
{
|
||||
int slot = monitoring.monitorItems[i].slotNo;
|
||||
int offset = monitoring.monitorItems[i].offset;
|
||||
int len = monitoring.monitorItems[i].len;
|
||||
if(len == 0)continue;
|
||||
Array.Copy(telemetry.getSlot(slot, offset, len), 0, load, i * sizeof(float), sizeof(float));
|
||||
}
|
||||
if (tele_server != null)
|
||||
{
|
||||
tele_server.putRequest(tele_server.prepareTelegram(BaseCommHandler.TELE_CMD_RD_MON_ON, 0, load, 0, load.Length));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private void CopyDataTimer_Tick(object? sender, EventArgs e)
|
||||
{
|
||||
/*if (dataDrone != null)
|
||||
{
|
||||
float[] imu = { dataDrone.AccX, dataDrone.AccY, dataDrone.AccZ, dataDrone.GyrX, dataDrone.GyrY, dataDrone.GyrZ };
|
||||
Array.Copy(imu, telemetry.imu, 6);
|
||||
}*/
|
||||
|
||||
if (monitoring.isMonitor == 1) MonitoringTimer.Start();
|
||||
else MonitoringTimer.Stop();
|
||||
}
|
||||
|
||||
private void ConnectionCallback(object o)
|
||||
{
|
||||
ConnectData data = (ConnectData)o;
|
||||
|
||||
if (!data.Connect)
|
||||
{
|
||||
try
|
||||
{
|
||||
Invoke((MethodInvoker)delegate
|
||||
{
|
||||
button_Connect.Text = "Connect";
|
||||
button_Connect.BackColor = Color.Transparent;
|
||||
MessageBox.Show("Connection closed");
|
||||
});
|
||||
}
|
||||
catch { }
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
Drone dataDrone = new Drone();
|
||||
|
||||
private void ReceiveCallback(object o)
|
||||
{
|
||||
ReceiveData data = (ReceiveData)o;
|
||||
|
||||
List<byte[]?>? send = dataDrone.DataStream(data.Buffer, data.Size);
|
||||
|
||||
if (send == null) return;
|
||||
try
|
||||
{
|
||||
foreach (byte[]? b in send)
|
||||
{
|
||||
if (b != null) data.Server?.Send(b);
|
||||
}
|
||||
}
|
||||
catch { }
|
||||
}
|
||||
|
||||
private void button_Connect_Click(object sender, EventArgs e)
|
||||
{
|
||||
var done = netClient.Connect(textBox_Server_Addr.Text, (int)numericUpDown_Server_Port.Value, ConnectionCallback, ReceiveCallback);
|
||||
|
||||
switch (done)
|
||||
{
|
||||
case NetClient.ClientState.Error:
|
||||
{
|
||||
MessageBox.Show("Error connecting to server");
|
||||
break;
|
||||
}
|
||||
case NetClient.ClientState.Connected:
|
||||
{
|
||||
button_Connect.Text = "Disconnect";
|
||||
button_Connect.BackColor = Color.LimeGreen;
|
||||
break;
|
||||
}
|
||||
case NetClient.ClientState.Stop:
|
||||
{
|
||||
button_Connect.Text = "Connect";
|
||||
button_Connect.BackColor = Color.Transparent;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (done != NetClient.ClientState.Connected) return;
|
||||
|
||||
|
||||
copyDataTimer.Interval = 10;
|
||||
copyDataTimer.Tick += CopyDataTimer_Tick;
|
||||
tele_server.setCommParams(new TCPCommParams("", (int)TeleServerPortCtrl.Value));
|
||||
var t = Task.Run(() => tele_server.Open());
|
||||
tele_server.OnTeleClientConnected += Tele_server_OnTeleClientСonnected;
|
||||
tele_server.OnTeleClientDisconnected += Tele_server_OnTeleClientDisconnected;
|
||||
MonitoringTimer.Interval = 25;
|
||||
MonitoringTimer.Tick += MonitoringTimer_Tick;
|
||||
copyDataTimer.Start();
|
||||
button1.BackColor = Color.LimeGreen;
|
||||
}
|
||||
|
||||
private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
|
||||
{
|
||||
netClient?.Close();
|
||||
netClient = null;
|
||||
}
|
||||
|
||||
private void timer_Test_Tick(object sender, EventArgs e)
|
||||
{
|
||||
label_Acc_X.Text = dataDrone.AccX.ToString();
|
||||
label_Acc_Y.Text = dataDrone.AccY.ToString();
|
||||
label_Acc_Z.Text = dataDrone.AccZ.ToString();
|
||||
|
||||
label_time_acc.Text = dataDrone.TimeAcc.ToString();
|
||||
|
||||
label_Gyr_X.Text = dataDrone.GyrX.ToString();
|
||||
label_Gyr_Y.Text = dataDrone.GyrY.ToString();
|
||||
label_Gyr_Z.Text = dataDrone.GyrZ.ToString();
|
||||
|
||||
label_time_gyr.Text = dataDrone.TimeGyr.ToString();
|
||||
|
||||
float[] imu = { dataDrone.AccX, dataDrone.AccY, dataDrone.AccZ, dataDrone.GyrX, dataDrone.GyrY, dataDrone.GyrZ };
|
||||
Array.Copy(imu, telemetry.imu, 6);
|
||||
|
||||
label_Pos_X.Text = dataDrone.PosX.ToString();
|
||||
label_Pos_Y.Text = dataDrone.PosY.ToString();
|
||||
label_Pos_L.Text = dataDrone.LaserRange.ToString();
|
||||
|
||||
label_time_range.Text = dataDrone.TimeRange.ToString();
|
||||
|
||||
float[] pos = { dataDrone.PosX, dataDrone.PosY, dataDrone.LaserRange };
|
||||
Array.Copy(pos, telemetry.pos, 3);
|
||||
|
||||
label_OF_X.Text = dataDrone.OF.X.ToString();
|
||||
label_OF_Y.Text = dataDrone.OF.Y.ToString();
|
||||
|
||||
label_time_of.Text = dataDrone.TimeRange.ToString();
|
||||
|
||||
float[] of = { dataDrone.OF.X, dataDrone.OF.Y };
|
||||
Array.Copy(of, telemetry.of, 2);
|
||||
|
||||
if (monitoring.isMonitor == 1) MonitoringTimer.Start();
|
||||
else MonitoringTimer.Stop();
|
||||
|
||||
netClient.SendData(dataDrone.SendReqest());
|
||||
}
|
||||
|
||||
private void trackBar_Power_Scroll(object sender, EventArgs e)
|
||||
{
|
||||
float pow = (float)trackBar_Power.Value / 100;
|
||||
|
||||
label_Pow.Text = pow.ToString();
|
||||
|
||||
dataDrone.MotorUL = dataDrone.MotorUR = dataDrone.MotorDL = dataDrone.MotorDR = pow;
|
||||
}
|
||||
|
||||
private void button_UU_MouseDown(object sender, MouseEventArgs e)
|
||||
{
|
||||
float pow = ((float)trackBar_Value.Value) / 10.0f;
|
||||
|
||||
if (sender == button_UU)
|
||||
{
|
||||
dataDrone.MotorUL -= pow; dataDrone.MotorUR -= pow;
|
||||
dataDrone.MotorDL += pow; dataDrone.MotorDR += pow;
|
||||
}
|
||||
if (sender == button_DD)
|
||||
{
|
||||
dataDrone.MotorUL += pow; dataDrone.MotorUR += pow;
|
||||
dataDrone.MotorDL -= pow; dataDrone.MotorDR -= pow;
|
||||
}
|
||||
if (sender == button_LL)
|
||||
{
|
||||
dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
|
||||
dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
|
||||
}
|
||||
if (sender == button_RR)
|
||||
{
|
||||
dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
|
||||
dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
|
||||
}
|
||||
|
||||
if (sender == button_ML)
|
||||
{
|
||||
dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
|
||||
dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
|
||||
}
|
||||
|
||||
if (sender == button_MR)
|
||||
{
|
||||
dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
|
||||
dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
|
||||
}
|
||||
}
|
||||
|
||||
private void button_UU_MouseUp(object sender, MouseEventArgs e)
|
||||
{
|
||||
trackBar_Power_Scroll(null, null);
|
||||
}
|
||||
|
||||
private void button1_Click(object sender, EventArgs e)
|
||||
{
|
||||
|
||||
copyDataTimer.Interval = 10;
|
||||
copyDataTimer.Tick += CopyDataTimer_Tick;
|
||||
tele_server.setCommParams(new TCPCommParams("", (int)TeleServerPortCtrl.Value));
|
||||
var t = Task.Run(() => tele_server.Open());
|
||||
tele_server.OnTeleClientConnected += Tele_server_OnTeleClientСonnected;
|
||||
tele_server.OnTeleClientDisconnected += Tele_server_OnTeleClientDisconnected;
|
||||
MonitoringTimer.Interval = 25;
|
||||
MonitoringTimer.Tick += MonitoringTimer_Tick;
|
||||
copyDataTimer.Start();
|
||||
button1.BackColor = Color.LimeGreen;
|
||||
}
|
||||
}
|
||||
catch { }
|
||||
}
|
||||
|
||||
private void button_Connect_Click(object sender, EventArgs e)
|
||||
{
|
||||
var done = netClient.Connect(textBox_Server_Addr.Text, (int)numericUpDown_Server_Port.Value, ConnectionCallback, ReceiveCallback);
|
||||
|
||||
switch (done)
|
||||
{
|
||||
case NetClient.ClientState.Error:
|
||||
{
|
||||
MessageBox.Show("Error connecting to server");
|
||||
break;
|
||||
}
|
||||
case NetClient.ClientState.Connected:
|
||||
{
|
||||
button_Connect.Text = "Disconnect";
|
||||
button_Connect.BackColor = Color.LimeGreen;
|
||||
break;
|
||||
}
|
||||
case NetClient.ClientState.Stop:
|
||||
{
|
||||
button_Connect.Text = "Connect";
|
||||
button_Connect.BackColor = Color.Transparent;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (done != NetClient.ClientState.Connected) return;
|
||||
}
|
||||
|
||||
private void Form_Main_FormClosing(object sender, FormClosingEventArgs e)
|
||||
{
|
||||
netClient?.Close();
|
||||
netClient = null;
|
||||
}
|
||||
|
||||
private void timer_Test_Tick(object sender, EventArgs e)
|
||||
{
|
||||
label_Acc_X.Text = dataDrone.AccX.ToString();
|
||||
label_Acc_Y.Text = dataDrone.AccY.ToString();
|
||||
label_Acc_Z.Text = dataDrone.AccZ.ToString();
|
||||
|
||||
label_time_acc.Text = dataDrone.TimeAcc.ToString();
|
||||
|
||||
label_Gyr_X.Text = dataDrone.GyrX.ToString();
|
||||
label_Gyr_Y.Text = dataDrone.GyrY.ToString();
|
||||
label_Gyr_Z.Text = dataDrone.GyrZ.ToString();
|
||||
|
||||
label_time_gyr.Text = dataDrone.TimeGyr.ToString();
|
||||
|
||||
label_Pos_X.Text = dataDrone.PosX.ToString();
|
||||
label_Pos_Y.Text = dataDrone.PosY.ToString();
|
||||
label_Pos_L.Text = dataDrone.LaserRange.ToString();
|
||||
|
||||
label_time_range.Text = dataDrone.TimeRange.ToString();
|
||||
|
||||
label_OF_X.Text = dataDrone.OF.X.ToString();
|
||||
label_OF_Y.Text = dataDrone.OF.Y.ToString();
|
||||
|
||||
label_time_of.Text = dataDrone.TimeRange.ToString();
|
||||
|
||||
netClient.SendData(dataDrone.SendReqest());
|
||||
}
|
||||
|
||||
private void trackBar_Power_Scroll(object sender, EventArgs e)
|
||||
{
|
||||
float pow = (float)trackBar_Power.Value / 100;
|
||||
|
||||
label_Pow.Text = pow.ToString();
|
||||
|
||||
dataDrone.MotorUL = dataDrone.MotorUR = dataDrone.MotorDL = dataDrone.MotorDR = pow;
|
||||
}
|
||||
|
||||
private void button_UU_MouseDown(object sender, MouseEventArgs e)
|
||||
{
|
||||
float pow = ((float)trackBar_Value.Value) / 10.0f;
|
||||
|
||||
if (sender == button_UU)
|
||||
{
|
||||
dataDrone.MotorUL -= pow; dataDrone.MotorUR -= pow;
|
||||
dataDrone.MotorDL += pow; dataDrone.MotorDR += pow;
|
||||
}
|
||||
if (sender == button_DD)
|
||||
{
|
||||
dataDrone.MotorUL += pow; dataDrone.MotorUR += pow;
|
||||
dataDrone.MotorDL -= pow; dataDrone.MotorDR -= pow;
|
||||
}
|
||||
if (sender == button_LL)
|
||||
{
|
||||
dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
|
||||
dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
|
||||
}
|
||||
if (sender == button_RR)
|
||||
{
|
||||
dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
|
||||
dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
|
||||
}
|
||||
|
||||
if (sender == button_ML)
|
||||
{
|
||||
dataDrone.MotorUL -= pow; dataDrone.MotorUR += pow;
|
||||
dataDrone.MotorDL += pow; dataDrone.MotorDR -= pow;
|
||||
}
|
||||
|
||||
if (sender == button_MR)
|
||||
{
|
||||
dataDrone.MotorUL += pow; dataDrone.MotorUR -= pow;
|
||||
dataDrone.MotorDL -= pow; dataDrone.MotorDR += pow;
|
||||
}
|
||||
}
|
||||
|
||||
private void button_UU_MouseUp(object sender, MouseEventArgs e)
|
||||
{
|
||||
trackBar_Power_Scroll(null, null);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
Reference in New Issue
Block a user