150billionsteps/TelemetryIO
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Bug fix, finish motor control, add some comments

Browse Source
This commit is contained in:
dr-i-boleet
2025-03-31 16:39:53 +11:00
parent ffa9b50b12
commit 5c85a024c2
10 changed files with 979 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
TelemetryIO/MainForm.Designer.cs generated
View File

@ -31,8 +31,11 @@
System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(MainForm));
this.InfoLabel = new System.Windows.Forms.Label();
this.tabControl1 = new System.Windows.Forms.TabControl();
this.tabPage1 = new System.Windows.Forms.TabPage();
this.tabPage2 = new System.Windows.Forms.TabPage();
this.PIDs_tab = new System.Windows.Forms.TabPage();
this.ratePID_control = new TelemetryIO.RatePID();
this.Motors_tab = new System.Windows.Forms.TabPage();
this.motors1 = new TelemetryIO.Motors();
this.Monitoring_tab = new System.Windows.Forms.TabPage();
this.monitorVars1 = new TelemetryIO.MonitorVars();
this.name_label = new System.Windows.Forms.Label();
this.pictureBox1 = new System.Windows.Forms.PictureBox();
@ -43,10 +46,10 @@
this.label1 = new System.Windows.Forms.Label();
this.PortSelector = new System.Windows.Forms.ComboBox();
this.label3 = new System.Windows.Forms.Label();
this.ratePID_control = new TelemetryIO.RatePID();
this.tabControl1.SuspendLayout();
this.tabPage1.SuspendLayout();
this.tabPage2.SuspendLayout();
this.PIDs_tab.SuspendLayout();
this.Motors_tab.SuspendLayout();
this.Monitoring_tab.SuspendLayout();
((System.ComponentModel.ISupportInitialize)(this.pictureBox1)).BeginInit();
this.groupBox1.SuspendLayout();
this.SuspendLayout();
@ -62,8 +65,9 @@
// tabControl1
//
this.tabControl1.Alignment = System.Windows.Forms.TabAlignment.Bottom;
this.tabControl1.Controls.Add(this.tabPage1);
this.tabControl1.Controls.Add(this.tabPage2);
this.tabControl1.Controls.Add(this.PIDs_tab);
this.tabControl1.Controls.Add(this.Motors_tab);
this.tabControl1.Controls.Add(this.Monitoring_tab);
this.tabControl1.Location = new System.Drawing.Point(12, 110);
this.tabControl1.Multiline = true;
this.tabControl1.Name = "tabControl1";
@ -71,28 +75,56 @@
this.tabControl1.Size = new System.Drawing.Size(1339, 680);
this.tabControl1.TabIndex = 10;
this.tabControl1.Visible = false;
this.tabControl1.SelectedIndexChanged += new System.EventHandler(this.tabControl1_SelectedIndexChanged);
//
// tabPage1
// PIDs_tab
//
this.tabPage1.Controls.Add(this.ratePID_control);
this.tabPage1.Location = new System.Drawing.Point(4, 4);
this.tabPage1.Name = "tabPage1";
this.tabPage1.Padding = new System.Windows.Forms.Padding(3);
this.tabPage1.Size = new System.Drawing.Size(1331, 654);
this.tabPage1.TabIndex = 0;
this.tabPage1.Text = "PIDs";
this.tabPage1.UseVisualStyleBackColor = true;
this.PIDs_tab.Controls.Add(this.ratePID_control);
this.PIDs_tab.Location = new System.Drawing.Point(4, 4);
this.PIDs_tab.Name = "PIDs_tab";
this.PIDs_tab.Padding = new System.Windows.Forms.Padding(3);
this.PIDs_tab.Size = new System.Drawing.Size(1331, 654);
this.PIDs_tab.TabIndex = 0;
this.PIDs_tab.Text = "PIDs";
this.PIDs_tab.UseVisualStyleBackColor = true;
//
// tabPage2
// ratePID_control
//
this.tabPage2.Controls.Add(this.monitorVars1);
this.tabPage2.Location = new System.Drawing.Point(4, 4);
this.tabPage2.Name = "tabPage2";
this.tabPage2.Padding = new System.Windows.Forms.Padding(3);
this.tabPage2.Size = new System.Drawing.Size(1331, 654);
this.tabPage2.TabIndex = 1;
this.tabPage2.Text = "Monitor";
this.tabPage2.UseVisualStyleBackColor = true;
this.ratePID_control.BackColor = System.Drawing.Color.AntiqueWhite;
this.ratePID_control.bottom_label = null;
this.ratePID_control.Location = new System.Drawing.Point(6, 6);
this.ratePID_control.Name = "ratePID_control";
this.ratePID_control.serial = null;
this.ratePID_control.Size = new System.Drawing.Size(1299, 642);
this.ratePID_control.TabIndex = 11;
//
// Motors_tab
//
this.Motors_tab.Controls.Add(this.motors1);
this.Motors_tab.Location = new System.Drawing.Point(4, 4);
this.Motors_tab.Name = "Motors_tab";
this.Motors_tab.Size = new System.Drawing.Size(1331, 654);
this.Motors_tab.TabIndex = 2;
this.Motors_tab.Text = "Моторы";
this.Motors_tab.UseVisualStyleBackColor = true;
//
// motors1
//
this.motors1.Location = new System.Drawing.Point(0, 30);
this.motors1.Name = "motors1";
this.motors1.Size = new System.Drawing.Size(1240, 600);
this.motors1.TabIndex = 0;
//
// Monitoring_tab
//
this.Monitoring_tab.Controls.Add(this.monitorVars1);
this.Monitoring_tab.Location = new System.Drawing.Point(4, 4);
this.Monitoring_tab.Name = "Monitoring_tab";
this.Monitoring_tab.Padding = new System.Windows.Forms.Padding(3);
this.Monitoring_tab.Size = new System.Drawing.Size(1331, 654);
this.Monitoring_tab.TabIndex = 1;
this.Monitoring_tab.Text = "Monitor";
this.Monitoring_tab.UseVisualStyleBackColor = true;
//
// monitorVars1
//
@ -192,16 +224,6 @@
this.label3.TabIndex = 14;
this.label3.Text = "Имя устройства";
//
// ratePID_control
//
this.ratePID_control.BackColor = System.Drawing.Color.AntiqueWhite;
this.ratePID_control.bottom_label = null;
this.ratePID_control.Location = new System.Drawing.Point(6, 6);
this.ratePID_control.Name = "ratePID_control";
this.ratePID_control.serial = null;
this.ratePID_control.Size = new System.Drawing.Size(1299, 642);
this.ratePID_control.TabIndex = 11;
//
// MainForm
//
this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F);
@ -217,8 +239,9 @@
this.Name = "MainForm";
this.Text = "TelemetryIO";
this.tabControl1.ResumeLayout(false);
this.tabPage1.ResumeLayout(false);
this.tabPage2.ResumeLayout(false);
this.PIDs_tab.ResumeLayout(false);
this.Motors_tab.ResumeLayout(false);
this.Monitoring_tab.ResumeLayout(false);
((System.ComponentModel.ISupportInitialize)(this.pictureBox1)).EndInit();
this.groupBox1.ResumeLayout(false);
this.groupBox1.PerformLayout();
@ -230,8 +253,8 @@
#endregion
private System.Windows.Forms.Label InfoLabel;
private System.Windows.Forms.TabControl tabControl1;
private System.Windows.Forms.TabPage tabPage1;
private System.Windows.Forms.TabPage tabPage2;
private System.Windows.Forms.TabPage PIDs_tab;
private System.Windows.Forms.TabPage Monitoring_tab;
private MonitorVars monitorVars1;
private System.Windows.Forms.Label name_label;
private System.Windows.Forms.PictureBox pictureBox1;
@ -243,6 +266,8 @@
private System.Windows.Forms.ComboBox PortSelector;
private System.Windows.Forms.Label label3;
private RatePID ratePID_control;
private System.Windows.Forms.TabPage Motors_tab;
private Motors motors1;
}
}

130
TelemetryIO/MainForm.cs
View File

@ -24,6 +24,7 @@ namespace TelemetryIO
private System.Windows.Forms.Timer timer = new System.Windows.Forms.Timer();
private System.Timers.Timer packageTransferTimer = new System.Timers.Timer(50);
private FormClosingEventArgs formArgs;
public string zhopa = "ZHOPA!!!";
public MainForm()
{
InitializeComponent();
@ -39,6 +40,7 @@ namespace TelemetryIO
timer.Tick += Timer_Tick;
ratePID_control.SubscribeOnEvent(serial);
monitorVars1.subscribeOnMonitoringItemsReceived(serial);
motors1.SubscribeAnswerReceived(serial);
ratePID_control.bottom_label = InfoLabel;
serial.HandShakeOccurred += Serial_HandShakeOccurred;
serial.AnswerReceived += Serial_AnswerReceived;
@ -46,7 +48,31 @@ namespace TelemetryIO
private void Serial_AnswerReceived(object sender, SerialEventArgs e)
{
if(e.Answer == "EXITAPP") base.OnFormClosing(formArgs);
if(e.Answer == "EXITAPP")
{
serial.Close();
base.OnFormClosing(formArgs);
}
else if(e.Answer == "TIMEOUT")
{
serial.Close();
if (Connect_btn.InvokeRequired)
{
Connect_btn.BeginInvoke(new Action(() =>
{
Connect_btn.Text = "Подключить";
Connect_btn.Enabled = true;
}));
}
else
{
Connect_btn.Text = "Подключить";
Connect_btn.Enabled = true;
}
ResetTabControl();
}
}
private void Serial_HandShakeOccurred(object sender, EventArgs e)
@ -60,6 +86,20 @@ namespace TelemetryIO
tabControl1.Visible = true;
}
if (Connect_btn.InvokeRequired)
{
Connect_btn.BeginInvoke(new Action(() =>
{
Connect_btn.Text = "Отключить";
Connect_btn.Enabled = true;
}));
}
else
{
Connect_btn.Text = "Отключить";
Connect_btn.Enabled = true;
}
serial.getPIDs();
}
private void Timer_Tick(object sender, EventArgs e)
@ -106,16 +146,35 @@ namespace TelemetryIO
InfoLabel.Text = ex.Message;
return;
}
b.Text = "Отключить";
b.Enabled = false;
}
}
else
{
tabControl1.Visible = false;
serial.Close();
b.Text = "Подключить";
}
}
private void ResetTabControl()
{
if (tabControl1.InvokeRequired)
{
tabControl1.BeginInvoke(new Action(() =>
{
tabControl1.Visible = false;
tabControl1.SelectedIndex = 0;
}));
}
else
{
tabControl1.Visible = false;
tabControl1.SelectedIndex = 0;
}
motors1.StopMotorsControl();
}
private void PortSelector_RefreshItems(object sender, MouseEventArgs e)
{
PortSelector.Items.Clear();
@ -128,5 +187,72 @@ namespace TelemetryIO
formArgs = e;
if(serial.isOpen)serial.requestExit();
}
private void tabControl1_SelectedIndexChanged(object sender, EventArgs e)
{
TabControl tb = sender as TabControl;
Debug.WriteLine(tb.SelectedTab.Name);
switch (tb.SelectedTab.Name)
{
case "PIDs_tab":
serial.getPIDs();
break;
}
serial.stopMonitoring();
motors1.StopMotorsControl();
}
}
public class PopupForm : Form
{
public PopupForm(string message)
{
// 1. Настройки формы
this.Text = "Уведомление";
this.Size = new Size(300, 150);
this.FormBorderStyle = FormBorderStyle.FixedDialog; // Стандартная рамка
this.StartPosition = FormStartPosition.Manual;
this.ShowInTaskbar = false;
this.BackColor = Color.White;
this.ControlBox = false; // Убираем стандартные кнопки
this.MinimizeBox = false;
this.MaximizeBox = false;
// 2. Содержимое формы
System.Windows.Forms.Label label = new System.Windows.Forms.Label()
{
Text = message,
Dock = DockStyle.Fill,
TextAlign = ContentAlignment.MiddleCenter,
Font = new Font("Segoe UI", 10),
Padding = new Padding(20)
};
Button closeButton = new Button()
{
Text = "OK",
Size = new Size(80, 30),
Anchor = AnchorStyles.Bottom | AnchorStyles.Right
};
closeButton.Click += (s, e) => this.Close();
// 3. Добавление элементов
this.Controls.Add(label);
this.Controls.Add(closeButton);
// 4. Позиционирование кнопки
closeButton.Location = new Point(
this.ClientSize.Width - closeButton.Width - 20,
this.ClientSize.Height - closeButton.Height - 10);
}
// 5. Правильное отображение (без блокировки главной формы)
public new void Show()
{
base.Show(); // Немодальное отображение
this.Location = new Point(
Cursor.Position.X - this.Width / 2,
Cursor.Position.Y - this.Height / 2);
}
}
}

8
TelemetryIO/Models/Telemetry.cs
View File

@ -10,15 +10,16 @@ namespace TelemetryIO.Models
{
public sealed class Telemetry
{
//Класс синглтон для хранения данных телеметрии
private static volatile Telemetry instance;
private static readonly object _lock = new object();
private Dictionary<string, VarAddress> dictMonitor = new Dictionary<string, VarAddress>();
private Dictionary<string, VarAddress> dictMonitor = new Dictionary<string, VarAddress>();//Словарь адресов, для целей мониторинга
public string name = "";
public float[] raw_imu = new float[6];
public float[] quaternion = new float[4];
public float[] battery = new float[4];
public float[] motor_act = new float[4];
public float[] motor_sp = new float[4];
public float[] motor_act = new float[8];
public float[] motor_sp = new float[8];
public float[] pid0 = new float[9];
public float[] pid1 = new float[9];
public float[] pid2 = new float[9];
@ -81,7 +82,6 @@ namespace TelemetryIO.Models
public void setSlot(int slot, byte[] data, int offset, int len)
{
if (slot != 9999) Debug.WriteLine(slot);
switch (slot)
{
case RAW_IMU_ADDRESS://RAW IMU

17
TelemetryIO/MonitorVars.cs
View File

@ -18,14 +18,9 @@ namespace TelemetryIO
{
public SerialHandler serial { get; set; }
public float[][] monitoringBuffer = new float[32][];
private List<double[]> monitorList = new List<double[]>();
private List<string> monitorNames = new List<string>();
private List<ScottPlot.Plottables.Signal> signals = new List<ScottPlot.Plottables.Signal>();
private List<MonitorItem> monItems = new List<MonitorItem>();
private List<int[]>[] plotContent = new List<int[]>[32];
private System.Windows.Forms.Timer updateTimer = new System.Windows.Forms.Timer();
private float[] monitoring = new float[32];
private int test_index = 0;
@ -60,7 +55,9 @@ namespace TelemetryIO
private void S_AnswerReceived(object sender, SerialEventArgs e)
{
if(e.Answer == "ADD")
//Получили подтверждение о выполнении операции
//вносим изменения в monItems(массив элементов для мониторинга)
if (e.Answer == "ADD")
{
MonitorItem item = new MonitorItem(processingName, processingIndex);
processingIndex++;
@ -146,6 +143,7 @@ namespace TelemetryIO
private void Serial_MonitoringItemsReceived(object sender, MonitoringEventArgs e)
{
//заполнение массивов данными
for(int i = 0; i < monItems.Count; i++)
{
monItems[i].Insert(e.Data[i]);
@ -160,6 +158,7 @@ namespace TelemetryIO
private void UpdateTimer_Tick(object sender, EventArgs e)
{
//обновление трендов
if (monItems.Count > 0)
{
MainFormPlot.Plot.Clear();
@ -181,7 +180,6 @@ namespace TelemetryIO
MainFormPlot.Plot.Legend.Orientation = ScottPlot.Orientation.Horizontal;
if(isAutoScale) MainFormPlot.Plot.Axes.AutoScale();
MainFormPlot.Refresh();
}
}
@ -197,10 +195,6 @@ namespace TelemetryIO
{
processingName = (string)monitorItems_cb.SelectedItem;
serial.AddMonitoringItem(processingName);
/*serial.AddMonitoringItem(0, test_index);
processingName = "item." + test_index;
if (test_index < 5) test_index++;
else test_index = 0;*/
}
private void del_btn_Click(object sender, EventArgs e)
@ -231,6 +225,7 @@ namespace TelemetryIO
public class MonitorItem
{
//Вспомогательный класс для хранения данных сигналов мониторинга
public double[] data = new double[2048];
public System.Drawing.Color color = System.Drawing.Color.Red;
public string name = "";

357
TelemetryIO/Motors.Designer.cs generated Normal file
View File

@ -0,0 +1,357 @@
namespace TelemetryIO
{
partial class Motors
{
/// <summary>
/// Обязательная переменная конструктора.
/// </summary>
private System.ComponentModel.IContainer components = null;
/// <summary>
/// Освободить все используемые ресурсы.
/// </summary>
/// <param name="disposing">истинно, если управляемый ресурс должен быть удален; иначе ложно.</param>
protected override void Dispose(bool disposing)
{
if (disposing && (components != null))
{
components.Dispose();
}
base.Dispose(disposing);
}
#region Код, автоматически созданный конструктором компонентов
/// <summary>
/// Требуемый метод для поддержки конструктора — не изменяйте
/// содержимое этого метода с помощью редактора кода.
/// </summary>
private void InitializeComponent()
{
this.MotorsPlotForm = new ScottPlot.WinForms.FormsPlot();
this.Motor1SP_track = new System.Windows.Forms.TrackBar();
this.label1 = new System.Windows.Forms.Label();
this.label2 = new System.Windows.Forms.Label();
this.Motor2SP_track = new System.Windows.Forms.TrackBar();
this.label3 = new System.Windows.Forms.Label();
this.Motor3SP_track = new System.Windows.Forms.TrackBar();
this.label4 = new System.Windows.Forms.Label();
this.Motor4SP_track = new System.Windows.Forms.TrackBar();
this.label5 = new System.Windows.Forms.Label();
this.Motor8SP_track = new System.Windows.Forms.TrackBar();
this.label6 = new System.Windows.Forms.Label();
this.Motor7SP_track = new System.Windows.Forms.TrackBar();
this.label7 = new System.Windows.Forms.Label();
this.Motor6SP_track = new System.Windows.Forms.TrackBar();
this.label8 = new System.Windows.Forms.Label();
this.Motor5SP_track = new System.Windows.Forms.TrackBar();
this.MotorMSP_track = new System.Windows.Forms.TrackBar();
this.label9 = new System.Windows.Forms.Label();
this.EnableMotors_cb = new System.Windows.Forms.CheckBox();
this.label10 = new System.Windows.Forms.Label();
this.label11 = new System.Windows.Forms.Label();
((System.ComponentModel.ISupportInitialize)(this.Motor1SP_track)).BeginInit();
((System.ComponentModel.ISupportInitialize)(this.Motor2SP_track)).BeginInit();
((System.ComponentModel.ISupportInitialize)(this.Motor3SP_track)).BeginInit();
((System.ComponentModel.ISupportInitialize)(this.Motor4SP_track)).BeginInit();
((System.ComponentModel.ISupportInitialize)(this.Motor8SP_track)).BeginInit();
((System.ComponentModel.ISupportInitialize)(this.Motor7SP_track)).BeginInit();
((System.ComponentModel.ISupportInitialize)(this.Motor6SP_track)).BeginInit();
((System.ComponentModel.ISupportInitialize)(this.Motor5SP_track)).BeginInit();
((System.ComponentModel.ISupportInitialize)(this.MotorMSP_track)).BeginInit();
this.SuspendLayout();
//
// MotorsPlotForm
//
this.MotorsPlotForm.DisplayScale = 0F;
this.MotorsPlotForm.Location = new System.Drawing.Point(294, 3);
this.MotorsPlotForm.Name = "MotorsPlotForm";
this.MotorsPlotForm.Size = new System.Drawing.Size(916, 594);
this.MotorsPlotForm.TabIndex = 0;
//
// Motor1SP_track
//
this.Motor1SP_track.Location = new System.Drawing.Point(39, 162);
this.Motor1SP_track.Maximum = 100000;
this.Motor1SP_track.Name = "Motor1SP_track";
this.Motor1SP_track.Orientation = System.Windows.Forms.Orientation.Vertical;
this.Motor1SP_track.Size = new System.Drawing.Size(45, 104);
this.Motor1SP_track.TabIndex = 1;
this.Motor1SP_track.TickStyle = System.Windows.Forms.TickStyle.TopLeft;
this.Motor1SP_track.Scroll += new System.EventHandler(this.Motor1SP_track_Scroll);
//
// label1
//
this.label1.AutoSize = true;
this.label1.Font = new System.Drawing.Font("Microsoft Sans Serif", 10F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(204)));
this.label1.Location = new System.Drawing.Point(48, 273);
this.label1.Name = "label1";
this.label1.Size = new System.Drawing.Size(27, 17);
this.label1.TabIndex = 2;
this.label1.Text = "M1";
//
// label2
//
this.label2.AutoSize = true;
this.label2.Font = new System.Drawing.Font("Microsoft Sans Serif", 10F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(204)));
this.label2.Location = new System.Drawing.Point(99, 273);
this.label2.Name = "label2";
this.label2.Size = new System.Drawing.Size(27, 17);
this.label2.TabIndex = 4;
this.label2.Text = "M2";
//
// Motor2SP_track
//
this.Motor2SP_track.Location = new System.Drawing.Point(90, 162);
this.Motor2SP_track.Maximum = 100000;
this.Motor2SP_track.Name = "Motor2SP_track";
this.Motor2SP_track.Orientation = System.Windows.Forms.Orientation.Vertical;
this.Motor2SP_track.Size = new System.Drawing.Size(45, 104);
this.Motor2SP_track.TabIndex = 3;
this.Motor2SP_track.TickStyle = System.Windows.Forms.TickStyle.TopLeft;
this.Motor2SP_track.Scroll += new System.EventHandler(this.Motor2SP_track_Scroll);
//
// label3
//
this.label3.AutoSize = true;
this.label3.Font = new System.Drawing.Font("Microsoft Sans Serif", 10F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(204)));
this.label3.Location = new System.Drawing.Point(150, 273);
this.label3.Name = "label3";
this.label3.Size = new System.Drawing.Size(27, 17);
this.label3.TabIndex = 6;
this.label3.Text = "M3";
//
// Motor3SP_track
//
this.Motor3SP_track.Location = new System.Drawing.Point(141, 162);
this.Motor3SP_track.Maximum = 100000;
this.Motor3SP_track.Name = "Motor3SP_track";
this.Motor3SP_track.Orientation = System.Windows.Forms.Orientation.Vertical;
this.Motor3SP_track.Size = new System.Drawing.Size(45, 104);
this.Motor3SP_track.TabIndex = 5;
this.Motor3SP_track.TickStyle = System.Windows.Forms.TickStyle.TopLeft;
this.Motor3SP_track.Scroll += new System.EventHandler(this.Motor3SP_track_Scroll);
//
// label4
//
this.label4.AutoSize = true;
this.label4.Font = new System.Drawing.Font("Microsoft Sans Serif", 10F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(204)));
this.label4.Location = new System.Drawing.Point(201, 273);
this.label4.Name = "label4";
this.label4.Size = new System.Drawing.Size(27, 17);
this.label4.TabIndex = 8;
this.label4.Text = "M4";
//
// Motor4SP_track
//
this.Motor4SP_track.Location = new System.Drawing.Point(192, 162);
this.Motor4SP_track.Maximum = 100000;
this.Motor4SP_track.Name = "Motor4SP_track";
this.Motor4SP_track.Orientation = System.Windows.Forms.Orientation.Vertical;
this.Motor4SP_track.Size = new System.Drawing.Size(45, 104);
this.Motor4SP_track.TabIndex = 7;
this.Motor4SP_track.TickStyle = System.Windows.Forms.TickStyle.TopLeft;
this.Motor4SP_track.Scroll += new System.EventHandler(this.Motor4SP_track_Scroll);
//
// label5
//
this.label5.AutoSize = true;
this.label5.Font = new System.Drawing.Font("Microsoft Sans Serif", 10F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(204)));
this.label5.Location = new System.Drawing.Point(201, 444);
this.label5.Name = "label5";
this.label5.Size = new System.Drawing.Size(27, 17);
this.label5.TabIndex = 16;
this.label5.Text = "M8";
//
// Motor8SP_track
//
this.Motor8SP_track.Location = new System.Drawing.Point(192, 333);
this.Motor8SP_track.Maximum = 100000;
this.Motor8SP_track.Name = "Motor8SP_track";
this.Motor8SP_track.Orientation = System.Windows.Forms.Orientation.Vertical;
this.Motor8SP_track.Size = new System.Drawing.Size(45, 104);
this.Motor8SP_track.TabIndex = 15;
this.Motor8SP_track.TickStyle = System.Windows.Forms.TickStyle.TopLeft;
this.Motor8SP_track.Scroll += new System.EventHandler(this.Motor8SP_track_Scroll);
//
// label6
//
this.label6.AutoSize = true;
this.label6.Font = new System.Drawing.Font("Microsoft Sans Serif", 10F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(204)));
this.label6.Location = new System.Drawing.Point(150, 444);
this.label6.Name = "label6";
this.label6.Size = new System.Drawing.Size(27, 17);
this.label6.TabIndex = 14;
this.label6.Text = "M7";
//
// Motor7SP_track
//
this.Motor7SP_track.Location = new System.Drawing.Point(141, 333);
this.Motor7SP_track.Maximum = 100000;
this.Motor7SP_track.Name = "Motor7SP_track";
this.Motor7SP_track.Orientation = System.Windows.Forms.Orientation.Vertical;
this.Motor7SP_track.Size = new System.Drawing.Size(45, 104);
this.Motor7SP_track.TabIndex = 13;
this.Motor7SP_track.TickStyle = System.Windows.Forms.TickStyle.TopLeft;
this.Motor7SP_track.Scroll += new System.EventHandler(this.Motor7SP_track_Scroll);
//
// label7
//
this.label7.AutoSize = true;
this.label7.Font = new System.Drawing.Font("Microsoft Sans Serif", 10F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(204)));
this.label7.Location = new System.Drawing.Point(99, 444);
this.label7.Name = "label7";
this.label7.Size = new System.Drawing.Size(27, 17);
this.label7.TabIndex = 12;
this.label7.Text = "M6";
//
// Motor6SP_track
//
this.Motor6SP_track.Location = new System.Drawing.Point(90, 333);
this.Motor6SP_track.Maximum = 100000;
this.Motor6SP_track.Name = "Motor6SP_track";
this.Motor6SP_track.Orientation = System.Windows.Forms.Orientation.Vertical;
this.Motor6SP_track.Size = new System.Drawing.Size(45, 104);
this.Motor6SP_track.TabIndex = 11;
this.Motor6SP_track.TickStyle = System.Windows.Forms.TickStyle.TopLeft;
this.Motor6SP_track.Scroll += new System.EventHandler(this.Motor6SP_track_Scroll);
//
// label8
//
this.label8.AutoSize = true;
this.label8.Font = new System.Drawing.Font("Microsoft Sans Serif", 10F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(204)));
this.label8.Location = new System.Drawing.Point(48, 444);
this.label8.Name = "label8";
this.label8.Size = new System.Drawing.Size(27, 17);
this.label8.TabIndex = 10;
this.label8.Text = "M5";
//
// Motor5SP_track
//
this.Motor5SP_track.Location = new System.Drawing.Point(39, 333);
this.Motor5SP_track.Maximum = 100000;
this.Motor5SP_track.Name = "Motor5SP_track";
this.Motor5SP_track.Orientation = System.Windows.Forms.Orientation.Vertical;
this.Motor5SP_track.Size = new System.Drawing.Size(45, 104);
this.Motor5SP_track.TabIndex = 9;
this.Motor5SP_track.TickStyle = System.Windows.Forms.TickStyle.TopLeft;
this.Motor5SP_track.Scroll += new System.EventHandler(this.Motor5SP_track_Scroll);
//
// MotorMSP_track
//
this.MotorMSP_track.Location = new System.Drawing.Point(51, 518);
this.MotorMSP_track.Maximum = 100000;
this.MotorMSP_track.Name = "MotorMSP_track";
this.MotorMSP_track.Size = new System.Drawing.Size(177, 45);
this.MotorMSP_track.TabIndex = 17;
this.MotorMSP_track.Scroll += new System.EventHandler(this.MotorMSP_track_Scroll);
//
// label9
//
this.label9.AutoSize = true;
this.label9.Font = new System.Drawing.Font("Microsoft Sans Serif", 10F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(204)));
this.label9.Location = new System.Drawing.Point(111, 570);
this.label9.Name = "label9";
this.label9.Size = new System.Drawing.Size(57, 17);
this.label9.TabIndex = 18;
this.label9.Text = "Мастер";
//
// EnableMotors_cb
//
this.EnableMotors_cb.AutoSize = true;
this.EnableMotors_cb.Font = new System.Drawing.Font("Microsoft Sans Serif", 14F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(204)));
this.EnableMotors_cb.Location = new System.Drawing.Point(48, 60);
this.EnableMotors_cb.Name = "EnableMotors_cb";
this.EnableMotors_cb.Size = new System.Drawing.Size(15, 14);
this.EnableMotors_cb.TabIndex = 19;
this.EnableMotors_cb.UseVisualStyleBackColor = true;
this.EnableMotors_cb.CheckedChanged += new System.EventHandler(this.EnableMotors_cb_CheckedChanged);
//
// label10
//
this.label10.AutoSize = true;
this.label10.Font = new System.Drawing.Font("Microsoft Sans Serif", 10F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(204)));
this.label10.Location = new System.Drawing.Point(69, 43);
this.label10.Name = "label10";
this.label10.Size = new System.Drawing.Size(142, 17);
this.label10.TabIndex = 21;
this.label10.Text = "Я подтверждаю, что";
//
// label11
//
this.label11.AutoSize = true;
this.label11.Font = new System.Drawing.Font("Microsoft Sans Serif", 10F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(204)));
this.label11.Location = new System.Drawing.Point(71, 71);
this.label11.Name = "label11";
this.label11.Size = new System.Drawing.Size(138, 17);
this.label11.TabIndex = 22;
this.label11.Text = "пропеллеры сняты.";
//
// Motors
//
this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F);
this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font;
this.Controls.Add(this.label11);
this.Controls.Add(this.label10);
this.Controls.Add(this.EnableMotors_cb);
this.Controls.Add(this.label9);
this.Controls.Add(this.MotorMSP_track);
this.Controls.Add(this.label5);
this.Controls.Add(this.Motor8SP_track);
this.Controls.Add(this.label6);
this.Controls.Add(this.Motor7SP_track);
this.Controls.Add(this.label7);
this.Controls.Add(this.Motor6SP_track);
this.Controls.Add(this.label8);
this.Controls.Add(this.Motor5SP_track);
this.Controls.Add(this.label4);
this.Controls.Add(this.Motor4SP_track);
this.Controls.Add(this.label3);
this.Controls.Add(this.Motor3SP_track);
this.Controls.Add(this.label2);
this.Controls.Add(this.Motor2SP_track);
this.Controls.Add(this.label1);
this.Controls.Add(this.Motor1SP_track);
this.Controls.Add(this.MotorsPlotForm);
this.Name = "Motors";
this.Size = new System.Drawing.Size(1240, 600);
((System.ComponentModel.ISupportInitialize)(this.Motor1SP_track)).EndInit();
((System.ComponentModel.ISupportInitialize)(this.Motor2SP_track)).EndInit();
((System.ComponentModel.ISupportInitialize)(this.Motor3SP_track)).EndInit();
((System.ComponentModel.ISupportInitialize)(this.Motor4SP_track)).EndInit();
((System.ComponentModel.ISupportInitialize)(this.Motor8SP_track)).EndInit();
((System.ComponentModel.ISupportInitialize)(this.Motor7SP_track)).EndInit();
((System.ComponentModel.ISupportInitialize)(this.Motor6SP_track)).EndInit();
((System.ComponentModel.ISupportInitialize)(this.Motor5SP_track)).EndInit();
((System.ComponentModel.ISupportInitialize)(this.MotorMSP_track)).EndInit();
this.ResumeLayout(false);
this.PerformLayout();
}
#endregion
private ScottPlot.WinForms.FormsPlot MotorsPlotForm;
private System.Windows.Forms.TrackBar Motor1SP_track;
private System.Windows.Forms.Label label1;
private System.Windows.Forms.Label label2;
private System.Windows.Forms.TrackBar Motor2SP_track;
private System.Windows.Forms.Label label3;
private System.Windows.Forms.TrackBar Motor3SP_track;
private System.Windows.Forms.Label label4;
private System.Windows.Forms.TrackBar Motor4SP_track;
private System.Windows.Forms.Label label5;
private System.Windows.Forms.TrackBar Motor8SP_track;
private System.Windows.Forms.Label label6;
private System.Windows.Forms.TrackBar Motor7SP_track;
private System.Windows.Forms.Label label7;
private System.Windows.Forms.TrackBar Motor6SP_track;
private System.Windows.Forms.Label label8;
private System.Windows.Forms.TrackBar Motor5SP_track;
private System.Windows.Forms.TrackBar MotorMSP_track;
private System.Windows.Forms.Label label9;
private System.Windows.Forms.CheckBox EnableMotors_cb;
private System.Windows.Forms.Label label10;
private System.Windows.Forms.Label label11;
}
}

187
TelemetryIO/Motors.cs Normal file
View File

@ -0,0 +1,187 @@
using ScottPlot;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Data.SqlTypes;
using System.Diagnostics;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using TelemetryIO.Models;
namespace TelemetryIO
{
public partial class Motors : UserControl
{
private float motorsMasterSP = 0f;
private SerialHandler serial { get; set; }
private System.Windows.Forms.Timer updateTimer = new System.Windows.Forms.Timer();
private List<MonitorItem> monItems = new List<MonitorItem>();
private bool requestNewData = false;
public Motors()
{
InitializeComponent();
for(int i = 0; i < 8; i++)
{
monItems.Add(new MonitorItem());
}
}
private void Serial_AnswerReceived(object sender, SerialEventArgs e)
{
if(e.Answer == "MOTORSACT")
{
Debug.WriteLine(string.Join(", ", Telemetry.Instance.motor_act));
for (int i = 0; i < monItems.Count; i++)
{
monItems[i].Insert(Telemetry.Instance.motor_act[i]);
}
if (requestNewData)
{
if (serial.isOpen) serial.sendMotorsControl();
}
}
}
public void SubscribeAnswerReceived(SerialHandler serial)
{
this.serial = serial;
this.serial.AnswerReceived += Serial_AnswerReceived;
updateTimer.Tick += UpdateTimer_Tick;
updateTimer.Interval = 100;
}
private void UpdateTimer_Tick(object sender, EventArgs e)
{
if (monItems.Count > 0)
{
MotorsPlotForm.Plot.Clear();
for (int i = 0; i < monItems.Count; i++)
{
var sig = MotorsPlotForm.Plot.Add.Signal(monItems[i].data);
sig.LegendText = monItems[i].name;
sig.Color = ScottPlot.Color.FromColor(monItems[i].color);
}
MotorsPlotForm.Plot.Legend.IsVisible = true;
MotorsPlotForm.Plot.Legend.Orientation = ScottPlot.Orientation.Horizontal;
MotorsPlotForm.Plot.Axes.AutoScale();
MotorsPlotForm.Refresh();
}
}
private void MotorMSP_track_Scroll(object sender, EventArgs e)
{
TrackBar tb = sender as TrackBar;
motorsMasterSP = tb.Value/100000f;
if (tb.Value == 0) motorsMasterSP = 0f;
Debug.WriteLine(motorsMasterSP);
if(motorsMasterSP > 0)
{
float[] floats = new float[8];
for(int i = 0; i < floats.Length; i++)
{
floats[i] = motorsMasterSP;
}
Telemetry.Instance.motor_sp = floats;
}
}
private void EnableMotors_cb_CheckedChanged(object sender, EventArgs e)
{
CheckBox cb = sender as CheckBox;
if(cb.Checked)
{
updateTimer.Start();
requestNewData = true;
serial.sendMotorsControl();
}
else
{
requestNewData = false;
updateTimer.Stop();
}
}
public void StopMotorsControl()
{
updateTimer.Stop();
if (EnableMotors_cb.InvokeRequired)
{
EnableMotors_cb.Invoke(new Action(() => EnableMotors_cb.Checked = false));
}
else
{
EnableMotors_cb.Checked = false;
}
}
private void Motor1SP_track_Scroll(object sender, EventArgs e)
{
TrackBar tb = sender as TrackBar;
float v = tb.Value / 100000f;
if (tb.Value == 0) v = 0f;
Telemetry.Instance.motor_sp[0] = v;
}
private void Motor2SP_track_Scroll(object sender, EventArgs e)
{
TrackBar tb = sender as TrackBar;
float v = tb.Value / 100000f;
if (tb.Value == 0) v = 0f;
Telemetry.Instance.motor_sp[1] = v;
}
private void Motor3SP_track_Scroll(object sender, EventArgs e)
{
TrackBar tb = sender as TrackBar;
float v = tb.Value / 100000f;
if (tb.Value == 0) v = 0f;
Telemetry.Instance.motor_sp[2] = v;
}
private void Motor4SP_track_Scroll(object sender, EventArgs e)
{
TrackBar tb = sender as TrackBar;
float v = tb.Value / 100000f;
if (tb.Value == 0) v = 0f;
Telemetry.Instance.motor_sp[3] = v;
}
private void Motor8SP_track_Scroll(object sender, EventArgs e)
{
TrackBar tb = sender as TrackBar;
float v = tb.Value / 100000f;
if (tb.Value == 0) v = 0f;
Telemetry.Instance.motor_sp[7] = v;
}
private void Motor7SP_track_Scroll(object sender, EventArgs e)
{
TrackBar tb = sender as TrackBar;
float v = tb.Value / 100000f;
if (tb.Value == 0) v = 0f;
Telemetry.Instance.motor_sp[6] = v;
}
private void Motor6SP_track_Scroll(object sender, EventArgs e)
{
TrackBar tb = sender as TrackBar;
float v = tb.Value / 100000f;
if (tb.Value == 0) v = 0f;
Telemetry.Instance.motor_sp[5] = v;
}
private void Motor5SP_track_Scroll(object sender, EventArgs e)
{
TrackBar tb = sender as TrackBar;
float v = tb.Value / 100000f;
if (tb.Value == 0) v = 0f;
Telemetry.Instance.motor_sp[4] = v;
}
}
}

120
TelemetryIO/Motors.resx Normal file
View File

@ -0,0 +1,120 @@
<?xml version="1.0" encoding="utf-8"?>
<root>
<!--
Microsoft ResX Schema
Version 2.0
The primary goals of this format is to allow a simple XML format
that is mostly human readable. The generation and parsing of the
various data types are done through the TypeConverter classes
associated with the data types.
Example:
... ado.net/XML headers & schema ...
<resheader name="resmimetype">text/microsoft-resx</resheader>
<resheader name="version">2.0</resheader>
<resheader name="reader">System.Resources.ResXResourceReader, System.Windows.Forms, ...</resheader>
<resheader name="writer">System.Resources.ResXResourceWriter, System.Windows.Forms, ...</resheader>
<data name="Name1"><value>this is my long string</value><comment>this is a comment</comment></data>
<data name="Color1" type="System.Drawing.Color, System.Drawing">Blue</data>
<data name="Bitmap1" mimetype="application/x-microsoft.net.object.binary.base64">
<value>[base64 mime encoded serialized .NET Framework object]</value>
</data>
<data name="Icon1" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">
<value>[base64 mime encoded string representing a byte array form of the .NET Framework object]</value>
<comment>This is a comment</comment>
</data>
There are any number of "resheader" rows that contain simple
name/value pairs.
Each data row contains a name, and value. The row also contains a
type or mimetype. Type corresponds to a .NET class that support
text/value conversion through the TypeConverter architecture.
Classes that don't support this are serialized and stored with the
mimetype set.
The mimetype is used for serialized objects, and tells the
ResXResourceReader how to depersist the object. This is currently not
extensible. For a given mimetype the value must be set accordingly:
Note - application/x-microsoft.net.object.binary.base64 is the format
that the ResXResourceWriter will generate, however the reader can
read any of the formats listed below.
mimetype: application/x-microsoft.net.object.binary.base64
value : The object must be serialized with
: System.Runtime.Serialization.Formatters.Binary.BinaryFormatter
: and then encoded with base64 encoding.
mimetype: application/x-microsoft.net.object.soap.base64
value : The object must be serialized with
: System.Runtime.Serialization.Formatters.Soap.SoapFormatter
: and then encoded with base64 encoding.
mimetype: application/x-microsoft.net.object.bytearray.base64
value : The object must be serialized into a byte array
: using a System.ComponentModel.TypeConverter
: and then encoded with base64 encoding.
-->
<xsd:schema id="root" xmlns="" xmlns:xsd="http://www.w3.org/2001/XMLSchema" xmlns:msdata="urn:schemas-microsoft-com:xml-msdata">
<xsd:import namespace="http://www.w3.org/XML/1998/namespace" />
<xsd:element name="root" msdata:IsDataSet="true">
<xsd:complexType>
<xsd:choice maxOccurs="unbounded">
<xsd:element name="metadata">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" />
</xsd:sequence>
<xsd:attribute name="name" use="required" type="xsd:string" />
<xsd:attribute name="type" type="xsd:string" />
<xsd:attribute name="mimetype" type="xsd:string" />
<xsd:attribute ref="xml:space" />
</xsd:complexType>
</xsd:element>
<xsd:element name="assembly">
<xsd:complexType>
<xsd:attribute name="alias" type="xsd:string" />
<xsd:attribute name="name" type="xsd:string" />
</xsd:complexType>
</xsd:element>
<xsd:element name="data">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
<xsd:element name="comment" type="xsd:string" minOccurs="0" msdata:Ordinal="2" />
</xsd:sequence>
<xsd:attribute name="name" type="xsd:string" use="required" msdata:Ordinal="1" />
<xsd:attribute name="type" type="xsd:string" msdata:Ordinal="3" />
<xsd:attribute name="mimetype" type="xsd:string" msdata:Ordinal="4" />
<xsd:attribute ref="xml:space" />
</xsd:complexType>
</xsd:element>
<xsd:element name="resheader">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
</xsd:sequence>
<xsd:attribute name="name" type="xsd:string" use="required" />
</xsd:complexType>
</xsd:element>
</xsd:choice>
</xsd:complexType>
</xsd:element>
</xsd:schema>
<resheader name="resmimetype">
<value>text/microsoft-resx</value>
</resheader>
<resheader name="version">
<value>2.0</value>
</resheader>
<resheader name="reader">
<value>System.Resources.ResXResourceReader, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</resheader>
<resheader name="writer">
<value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</resheader>
</root>

40
TelemetryIO/RatePID.cs
View File

@ -27,7 +27,6 @@ namespace TelemetryIO
public RatePID()
{
InitializeComponent();
Debug.Write("tab elements = " + tabControl1.TabPages[0].Controls[0].Controls);
}
private void Serial_AnswerReceived(object sender, SerialEventArgs e)
@ -84,16 +83,49 @@ namespace TelemetryIO
private void Save_btn_Click(object sender, EventArgs e)
{
float[] floats = new float[9];
float[] floats0 = new float[9];
for(int i = 0; i < pid0_panel.Controls.Count; i++)
{
if (pid0_panel.Controls[i] is NumericUpDown)
{
NumericUpDown c = pid0_panel.Controls[i] as NumericUpDown;
floats[c.TabIndex - 1] = (float)c.Value;
floats0[c.TabIndex - 1] = (float)c.Value;
}
}
serial.sendTestWrite(floats);
serial.sendFloats(Telemetry.PID0_ADDRESS, floats0);
float[] floats1 = new float[9];
for (int i = 0; i < pid1_panel.Controls.Count; i++)
{
if (pid1_panel.Controls[i] is NumericUpDown)
{
NumericUpDown c = pid1_panel.Controls[i] as NumericUpDown;
floats1[c.TabIndex - 1] = (float)c.Value;
}
}
serial.sendFloats(Telemetry.PID1_ADDRESS, floats1);
float[] floats2 = new float[9];
for (int i = 0; i < pid2_panel.Controls.Count; i++)
{
if (pid2_panel.Controls[i] is NumericUpDown)
{
NumericUpDown c = pid2_panel.Controls[i] as NumericUpDown;
floats2[c.TabIndex - 1] = (float)c.Value;
}
}
serial.sendFloats(Telemetry.PID2_ADDRESS, floats2);
float[] floats3 = new float[9];
for (int i = 0; i < pid3_panel.Controls.Count; i++)
{
if (pid3_panel.Controls[i] is NumericUpDown)
{
NumericUpDown c = pid3_panel.Controls[i] as NumericUpDown;
floats3[c.TabIndex - 1] = (float)c.Value;
}
}
serial.sendFloats(Telemetry.PID3_ADDRESS, floats3);
}
}
}

106
TelemetryIO/SerialHandler.cs
View File

@ -21,6 +21,7 @@ namespace TelemetryIO
private const int TELE_CMD_RD_MON_REMOVE = 5;
private const int TELE_CMD_RD_MON_REMOVEALL = 6;
private const int TELE_CMD_WR = 10;
private const int TELE_CMD_MOTORS_CTRL = 100;
private const int TELE_CMD_ABORT = 999;
private const int TELE_CMD_HELLO = 9999;
private const byte CRC8_POLYNOMIAL = 0x07;
@ -30,27 +31,30 @@ namespace TelemetryIO
private const byte ESCAPE_CHAR = 0x0E;
private SerialPort com_port;
static private List<byte> rx_buf = new List<byte>();
static private byte[] data;
private byte[] saving_request = new byte[128];
//Generate event when answer is received
public delegate void AnswerEventHandler<SerialEventArgs>(object sender, SerialEventArgs e);
public event AnswerEventHandler<SerialEventArgs> AnswerReceived;
public event AnswerEventHandler<SerialEventArgs> AnswerReceived;//событие получены данные в ответ на запрос
//Generate event when handshake is occurred
public delegate void HandShakeHandler(object sender, EventArgs e);
public event HandShakeHandler HandShakeOccurred;
public event HandShakeHandler HandShakeOccurred;//событие полетник ответил на запрос HELLO
//Generate event when monitoring telegram is received
public delegate void MonitoringEventHandler<MonitoringEventArgs>(object sender, MonitoringEventArgs e);
public event MonitoringEventHandler<MonitoringEventArgs> MonitoringItemsReceived;
public event MonitoringEventHandler<MonitoringEventArgs> MonitoringItemsReceived;//событие получены данные мониторинга
private int int_test = 0;
private float[] monitor = new float[32];
public string view_str = "";
Telemetry telemetry = Telemetry.Instance;
private int currentCmd = 0;
private Queue<byte[]> req_buffer = new Queue<byte[]>();
private object lock_obj = new object();
private object lock_obj_put = new object();
private bool waitingForResponse = false;
private bool isMonitorEmpty = false;
private bool isTimeout = false;
@ -102,6 +106,10 @@ namespace TelemetryIO
private void Serial_DataReceived(object sender, SerialDataReceivedEventArgs e)
{
//Прием данных при получении ESCAPE_END считаем, что пакет полностью получен
//и его можно парсить
//СТРУКТУРА ПАКЕТА
//КОМАНДА[4 байта], АДРЕСС ЯЧЕЙКИ[4 байта], ДЛИНА ДАННЫХ[4 байта], СМЕЩЕНИЕ[4 байта], ДАННЫЕ[в пределах размера буффера], CRC32[4 байта]
SerialPort sp = (SerialPort)sender;
try
{
@ -112,7 +120,6 @@ namespace TelemetryIO
if (b == ESCAPE_END)
{//END BYTE IS RECEIVED
timeout.Stop();
isTimeout = false;
byte[] unscape = EscapeSeqToBytes(rx_buf.ToArray());
uint checksum = crc32(unscape, unscape.Length - 4);
@ -145,11 +152,14 @@ namespace TelemetryIO
}
else if (cmd == TELE_CMD_WR)
{//WRITTING OPERATION IS DONE WITH SUCCESS
if (data[0] == 'O' && data[1] == 'k')
{
Debug.Write("Data is written\n");
OnAnswerReceived("Data is written");
}
else if (cmd == TELE_CMD_MOTORS_CTRL)
{//WRITTING OPERATION IS DONE WITH SUCCESS
//Debug.WriteLine($"cmd = {cmd} *** slot = {slot} *** len = {len}");
telemetry.setSlot(Telemetry.MOTORS_ACT_ADDRESS, data, offset, len);
OnAnswerReceived("MOTORSACT");
}
else if (cmd == TELE_CMD_HELLO)
{//HANDSHAKING ANSWER
@ -157,7 +167,6 @@ namespace TelemetryIO
view_str = Encoding.ASCII.GetString(data);
req_buffer.Clear();
OnHandShakeOccurred();
getPIDs();
}
else if (cmd == TELE_CMD_RD_MON_ADD)
{
@ -181,8 +190,9 @@ namespace TelemetryIO
{
OnAnswerReceived("MONITOROFF");
}
Debug.WriteLine($"cmd = {cmd}");
waitingForResponse = false;
timeout.Stop();
sendNextRequest();
}
}
@ -211,17 +221,21 @@ namespace TelemetryIO
private void Timeout_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
{
//Таймаут коммуникации. Без проверки на правильность ответа
System.Timers.Timer t = sender as System.Timers.Timer;
lock(lock_obj)
{
if(waitingForResponse)
if (waitingForResponse)
{
//generate timeout event
Debug.WriteLine("Timeout!!!!!");
waitingForResponse = false;
isTimeout = true;
req_buffer.Clear();
this.Close();
if(exitPending) OnAnswerReceived("EXITAPP");
else OnAnswerReceived("TIMEOUT");
req_buffer.Clear();
//this.Close();
}
}
}
@ -235,13 +249,16 @@ namespace TelemetryIO
public void requestExit()
{
//запрос на закрытие приложения, отправляем команду на очистку массива мониторинга и ждем ответ, либо таймаут,
//после чего приложение закрывается
RemoveMonitoringItems();
exitPending = true;
}
public void putRequest(byte[] request)
{
lock(lock_obj)
//добавить в очередь или отправить(если очередь пуста) пакет в порт
lock(lock_obj_put)
{
if (waitingForResponse)
{
@ -255,14 +272,16 @@ namespace TelemetryIO
private void sendRequest(byte[] request)
{
//отправка пакета
waitingForResponse = true;
com_port.Write(request, 0, request.Length);
if(com_port.IsOpen)com_port.Write(request, 0, request.Length);
timeout.Stop();
timeout.Start();
}
private void sendNextRequest()
{
//вытащить из очереди пакет и отправить в порт
waitingForResponse = false;
if (req_buffer.Count > 0)
{
@ -270,47 +289,72 @@ namespace TelemetryIO
}
}
//**********************************************************************************************
//*************************************** REQUESTS BLOCK ***************************************
//**********************************************************************************************
public void getPIDs()
{
//отправить наборы ПИДов
putRequest(prepareTelegram(TELE_CMD_RD_ONCE, 1000, new byte[0], 0, 4 * 9));
putRequest(prepareTelegram(TELE_CMD_RD_ONCE, 1001, new byte[0], 0, 4 * 9));
putRequest(prepareTelegram(TELE_CMD_RD_ONCE, 1002, new byte[0], 0, 4 * 9));
putRequest(prepareTelegram(TELE_CMD_RD_ONCE, 1003, new byte[0], 0, 4 * 9));
}
//**********************************************************************************************
//*************************************** REQUESTS BLOCK ***************************************
//**********************************************************************************************
public void stopMonitoring()
{
//остановить мониторинг
putRequest(prepareTelegram(TELE_CMD_RD_MON_OFF, 0, new byte[0], 0, 0));
}
public void startMonitoring()
{
//начать мониторинг
putRequest(prepareTelegram(TELE_CMD_RD_MON_ON, 0, new byte[0], 0, 0));
}
public void AddMonitoringItem(int slot, int offset)
{
//добавить элемент из массива мониторинга по адресу
putRequest(prepareTelegram(TELE_CMD_RD_MON_ADD, slot, new byte[0], offset));
}
public void AddMonitoringItem(string name)
{
//добавить элемент из массива мониторинга по имени
putRequest(prepareTelegram(TELE_CMD_RD_MON_ADD, new byte[0], name));
}
public void RemoveMonitoringItem(int position)
{
//удалить элемент из массива мониторинга
putRequest(prepareTelegram(TELE_CMD_RD_MON_REMOVE, position, new byte[0], 0));
}
public void RemoveMonitoringItems()
{
//удалить все элементы из массива мониторинга
putRequest(prepareTelegram(TELE_CMD_RD_MON_REMOVEALL, 0, new byte[0], 0));
}
public void sendFloats(int slot, float[] sp)
{
//Записать массив чисел с плавающей точкой
putRequest(prepareTelegram(TELE_CMD_WR, slot, sp, 0, sp.Length*4));
}
public void sendMotorsControl()
{
//Отправляет задание на моторы в полетник. Тот в ответ посылает актуальные скорости на моторах
//В offset передается количество моторов
putRequest(prepareTelegram(TELE_CMD_MOTORS_CTRL, Telemetry.MOTORS_SP_ADDRESS, telemetry.motor_sp, 8));
}
//**********************************************************************************************
//**********************************************************************************************
//**********************************************************************************************
public void saveFloats(float[] data)
{
prepareTelegram(TELE_CMD_WR, 1000, data, 0);
@ -363,28 +407,6 @@ namespace TelemetryIO
return crc;
}
public void sendTest()
{
//data - index in monitor
byte[] to = prepareTelegram(TELE_CMD_RD_MON_ADD, 0, BitConverter.GetBytes(int_test), int_test, 4);
com_port.Write(to, 0, to.Length);
int_test++;
}
public void sendTestCont()
{
byte[] b = new byte[0];
byte[] to = prepareTelegram(TELE_CMD_RD_MON_ON, 0, b, 0, 24);
com_port.Write(to, 0, to.Length);
}
public void sendTestWrite(float[] f)
{
byte[] b = prepareTelegram(TELE_CMD_WR, 1000, f, 0);
com_port.Write(b, 0, b.Length);
}
byte[] prepareTelegram<T>(int cmd, T[] load, string var_name)
{
VarAddress va = telemetry.getVarAdress(var_name);
@ -439,6 +461,7 @@ namespace TelemetryIO
Array.Copy(escape, 0, ret, 1, escape.Length);
ret[0] = ESCAPE_BEGIN;
ret[ret.Length - 1] = ESCAPE_END;
Array.Copy(ret, saving_request, ret.Length);
return ret;
}
@ -491,7 +514,8 @@ namespace TelemetryIO
List<byte> ret = new List<byte>();
for (int i = 0; i < EscSeq.Length; i++)
{
if ((EscSeq[i] == ESCAPE_BEGIN) || (EscSeq[i] == ESCAPE_CHAR) || (EscSeq[i] == ESCAPE_END))
//if ((EscSeq[i] == ESCAPE_BEGIN) || (EscSeq[i] == ESCAPE_CHAR) || (EscSeq[i] == ESCAPE_END))
if (EscSeq[i] == ESCAPE_CHAR)
{
i++;
ret.Add((byte)(EscSeq[i] + 0x0E));

9
TelemetryIO/TelemetryIO.csproj
View File

@ -108,6 +108,12 @@
<Compile Include="MonitorVars.Designer.cs">
<DependentUpon>MonitorVars.cs</DependentUpon>
</Compile>
<Compile Include="Motors.cs">
<SubType>UserControl</SubType>
</Compile>
<Compile Include="Motors.Designer.cs">
<DependentUpon>Motors.cs</DependentUpon>
</Compile>
<Compile Include="Program.cs" />
<Compile Include="Properties\AssemblyInfo.cs" />
<Compile Include="RatePID.cs">
@ -124,6 +130,9 @@
<EmbeddedResource Include="MonitorVars.resx">
<DependentUpon>MonitorVars.cs</DependentUpon>
</EmbeddedResource>
<EmbeddedResource Include="Motors.resx">
<DependentUpon>Motors.cs</DependentUpon>
</EmbeddedResource>
<EmbeddedResource Include="Properties\Resources.resx">
<Generator>ResXFileCodeGenerator</Generator>
<LastGenOutput>Resources.Designer.cs</LastGenOutput>