forked from CPL/Simulator
281 lines
10 KiB
C#
281 lines
10 KiB
C#
using System.Text;
|
||
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();
|
||
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");
|
||
|
||
public Form_Main()
|
||
{
|
||
InitializeComponent();
|
||
tele_server = new TelemetryServer();
|
||
if (Path.Exists(connectIco)) TeleClientStatusPicture.Image = Image.FromFile(disconnectIco);
|
||
}
|
||
|
||
private void Tele_server_OnTeleClientDisconnected(object? sender, EventArgs e)
|
||
{
|
||
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;
|
||
}
|
||
}
|
||
}
|