Добавлен сервер для отправки телеметрии

This commit is contained in:
2025-07-05 08:27:19 +11:00
parent 15d4fa5011
commit e5d8a9c507
13 changed files with 2347 additions and 736 deletions

View File

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