From 4cffa68ac82cfbacf7a55c4065b1692170f02505 Mon Sep 17 00:00:00 2001 From: dr-i-boleet Date: Fri, 4 Apr 2025 17:47:53 +1100 Subject: [PATCH] some fix --- TelemetryIO/MainForm.Designer.cs | 13 +++++++++ TelemetryIO/MainForm.cs | 28 ++++++++++++++++-- TelemetryIO/Models/Telemetry.cs | 35 ++++++++++++++++++++++- TelemetryIO/MonitorVars.Designer.cs | 44 ++++++++++++++++++++--------- TelemetryIO/MonitorVars.cs | 39 ++++++++++++++++++------- TelemetryIO/MonitorVars.resx | 3 ++ TelemetryIO/SerialHandler.cs | 7 +++-- TelemetryIO/TelemetryIO.csproj | 9 ++++++ 8 files changed, 149 insertions(+), 29 deletions(-) diff --git a/TelemetryIO/MainForm.Designer.cs b/TelemetryIO/MainForm.Designer.cs index b97ec54..ddb5e61 100644 --- a/TelemetryIO/MainForm.Designer.cs +++ b/TelemetryIO/MainForm.Designer.cs @@ -46,6 +46,7 @@ this.label1 = new System.Windows.Forms.Label(); this.PortSelector = new System.Windows.Forms.ComboBox(); this.label3 = new System.Windows.Forms.Label(); + this.TestQ_button = new System.Windows.Forms.Button(); this.tabControl1.SuspendLayout(); this.PIDs_tab.SuspendLayout(); this.Motors_tab.SuspendLayout(); @@ -224,11 +225,22 @@ this.label3.TabIndex = 14; this.label3.Text = "Имя устройства"; // + // TestQ_button + // + this.TestQ_button.Location = new System.Drawing.Point(1194, 12); + this.TestQ_button.Name = "TestQ_button"; + this.TestQ_button.Size = new System.Drawing.Size(75, 23); + this.TestQ_button.TabIndex = 15; + this.TestQ_button.Text = "Test Q"; + this.TestQ_button.UseVisualStyleBackColor = true; + this.TestQ_button.Click += new System.EventHandler(this.TestQ_button_Click); + // // MainForm // this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; this.ClientSize = new System.Drawing.Size(1363, 820); + this.Controls.Add(this.TestQ_button); this.Controls.Add(this.label3); this.Controls.Add(this.groupBox1); this.Controls.Add(this.pictureBox1); @@ -268,6 +280,7 @@ private RatePID ratePID_control; private System.Windows.Forms.TabPage Motors_tab; private Motors motors1; + private System.Windows.Forms.Button TestQ_button; } } diff --git a/TelemetryIO/MainForm.cs b/TelemetryIO/MainForm.cs index 111ebfa..1204709 100644 --- a/TelemetryIO/MainForm.cs +++ b/TelemetryIO/MainForm.cs @@ -48,12 +48,12 @@ namespace TelemetryIO private void Serial_AnswerReceived(object sender, SerialEventArgs e) { - if(e.Answer == "EXITAPP") + if (e.Answer == "EXITAPP") { serial.Close(); base.OnFormClosing(formArgs); } - else if(e.Answer == "TIMEOUT") + else if (e.Answer == "TIMEOUT") { serial.Close(); if (Connect_btn.InvokeRequired) @@ -72,6 +72,20 @@ namespace TelemetryIO ResetTabControl(); } + else if (e.Answer.StartsWith("EXCEPTION")) + { + if (InfoLabel.InvokeRequired) + { + InfoLabel.BeginInvoke(new Action(() => + { + InfoLabel.Text = e.Answer.Substring(9); + })); + } + else + { + InfoLabel.Text = e.Answer.Substring(9); + } + } } @@ -201,6 +215,16 @@ namespace TelemetryIO serial.stopMonitoring(); motors1.StopMotorsControl(); } + + private void TestQ_button_Click(object sender, EventArgs e) + { + Form form = new Form(); + form.Size = new Size(640, 480); + TestQuaternion tq = new TestQuaternion(); + tq.Location = new Point(10, 10); + form.Controls.Add(tq); + form.Show(); + } } public class PopupForm : Form diff --git a/TelemetryIO/Models/Telemetry.cs b/TelemetryIO/Models/Telemetry.cs index afa2777..b15cd2c 100644 --- a/TelemetryIO/Models/Telemetry.cs +++ b/TelemetryIO/Models/Telemetry.cs @@ -16,6 +16,9 @@ namespace TelemetryIO.Models private Dictionary dictMonitor = new Dictionary();//Словарь адресов, для целей мониторинга public string name = ""; public float[] raw_imu = new float[6]; + public float[] imu = new float[6]; + public float[] g_integration = new float[3]; + public float[] euler = new float[3]; public float[] quaternion = new float[4]; public float[] battery = new float[4]; public float[] motor_act = new float[8]; @@ -33,6 +36,21 @@ namespace TelemetryIO.Models dictMonitor.Add("rawGy", new VarAddress(RAW_IMU_ADDRESS, 4)); dictMonitor.Add("rawGz", new VarAddress(RAW_IMU_ADDRESS, 5)); + dictMonitor.Add("Ax", new VarAddress(IMU_ADDRESS, 0)); + dictMonitor.Add("Ay", new VarAddress(IMU_ADDRESS, 1)); + dictMonitor.Add("Az", new VarAddress(IMU_ADDRESS, 2)); + dictMonitor.Add("Gx", new VarAddress(IMU_ADDRESS, 3)); + dictMonitor.Add("Gy", new VarAddress(IMU_ADDRESS, 4)); + dictMonitor.Add("Gz", new VarAddress(IMU_ADDRESS, 5)); + + dictMonitor.Add("Gx_int", new VarAddress(G_INTEGRATION_ADDRESS, 0)); + dictMonitor.Add("Gy_int", new VarAddress(G_INTEGRATION_ADDRESS, 1)); + dictMonitor.Add("Gz_int", new VarAddress(G_INTEGRATION_ADDRESS, 2)); + + dictMonitor.Add("Euler.Phi", new VarAddress(EULER_ADDRESS, 0)); + dictMonitor.Add("Euler.Theta", new VarAddress(EULER_ADDRESS, 1)); + dictMonitor.Add("Euler.Psi", new VarAddress(EULER_ADDRESS, 2)); + dictMonitor.Add("Qw", new VarAddress(Q_ADDRESS, 0)); dictMonitor.Add("Qx", new VarAddress(Q_ADDRESS, 1)); dictMonitor.Add("Qy", new VarAddress(Q_ADDRESS, 2)); @@ -88,10 +106,22 @@ namespace TelemetryIO.Models set_float(raw_imu, data, offset, len); break; + case IMU_ADDRESS://IMU + set_float(imu, data, offset, len); + break; + case Q_ADDRESS://quaternion set_float(quaternion, data, offset, len); break; + case G_INTEGRATION_ADDRESS://quaternion + set_float(g_integration, data, offset, len); + break; + + case EULER_ADDRESS://quaternion + set_float(euler, data, offset, len); + break; + case BAT_ADDRESS://battery set_float(battery, data, offset, len); break; @@ -144,7 +174,10 @@ namespace TelemetryIO.Models } public const int RAW_IMU_ADDRESS = 0; - public const int Q_ADDRESS = 1; + public const int IMU_ADDRESS = 1; + public const int Q_ADDRESS = 2; + public const int G_INTEGRATION_ADDRESS = 3; + public const int EULER_ADDRESS = 4; public const int BAT_ADDRESS = 50; diff --git a/TelemetryIO/MonitorVars.Designer.cs b/TelemetryIO/MonitorVars.Designer.cs index 59953fe..7bb0450 100644 --- a/TelemetryIO/MonitorVars.Designer.cs +++ b/TelemetryIO/MonitorVars.Designer.cs @@ -28,15 +28,19 @@ /// private void InitializeComponent() { + this.components = new System.ComponentModel.Container(); this.MainFormPlot = new ScottPlot.WinForms.FormsPlot(); this.add_btn = new System.Windows.Forms.Button(); - this.del_btn = new System.Windows.Forms.Button(); this.monitorItems_cb = new System.Windows.Forms.ComboBox(); this.monitorItems_lb = new System.Windows.Forms.ListBox(); this.groupBox1 = new System.Windows.Forms.GroupBox(); this.AutoScale_check = new System.Windows.Forms.CheckBox(); this.StartStop_btn = new System.Windows.Forms.Button(); + this.ListBoxMenu = new System.Windows.Forms.ContextMenuStrip(this.components); + this.DeleteElement_menu = new System.Windows.Forms.ToolStripMenuItem(); + this.MoveElementToPlot1 = new System.Windows.Forms.ToolStripMenuItem(); this.groupBox1.SuspendLayout(); + this.ListBoxMenu.SuspendLayout(); this.SuspendLayout(); // // MainFormPlot @@ -58,17 +62,6 @@ this.add_btn.UseVisualStyleBackColor = true; this.add_btn.Click += new System.EventHandler(this.add_btn_Click); // - // del_btn - // - this.del_btn.Font = new System.Drawing.Font("Microsoft Sans Serif", 10F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(204))); - this.del_btn.Location = new System.Drawing.Point(67, 439); - this.del_btn.Name = "del_btn"; - this.del_btn.Size = new System.Drawing.Size(75, 23); - this.del_btn.TabIndex = 4; - this.del_btn.Text = "Удалить"; - this.del_btn.UseVisualStyleBackColor = true; - this.del_btn.Click += new System.EventHandler(this.del_btn_Click); - // // monitorItems_cb // this.monitorItems_cb.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList; @@ -88,6 +81,7 @@ this.monitorItems_lb.Name = "monitorItems_lb"; this.monitorItems_lb.Size = new System.Drawing.Size(214, 372); this.monitorItems_lb.TabIndex = 8; + this.monitorItems_lb.MouseUp += new System.Windows.Forms.MouseEventHandler(this.monitorItems_lb_MouseUp); // // groupBox1 // @@ -123,6 +117,26 @@ this.StartStop_btn.UseVisualStyleBackColor = true; this.StartStop_btn.Click += new System.EventHandler(this.StartStop_btn_Click); // + // ListBoxMenu + // + this.ListBoxMenu.Items.AddRange(new System.Windows.Forms.ToolStripItem[] { + this.DeleteElement_menu, + this.MoveElementToPlot1}); + this.ListBoxMenu.Name = "ListBoxMenu"; + this.ListBoxMenu.Size = new System.Drawing.Size(208, 48); + // + // DeleteElement_menu + // + this.DeleteElement_menu.Name = "DeleteElement_menu"; + this.DeleteElement_menu.Size = new System.Drawing.Size(207, 22); + this.DeleteElement_menu.Text = "Удалить"; + // + // MoveElementToPlot1 + // + this.MoveElementToPlot1.Name = "MoveElementToPlot1"; + this.MoveElementToPlot1.Size = new System.Drawing.Size(207, 22); + this.MoveElementToPlot1.Text = "Перекинуть на график 1"; + // // MonitorVars // this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); @@ -130,13 +144,13 @@ this.Controls.Add(this.groupBox1); this.Controls.Add(this.monitorItems_lb); this.Controls.Add(this.monitorItems_cb); - this.Controls.Add(this.del_btn); this.Controls.Add(this.add_btn); this.Controls.Add(this.MainFormPlot); this.Name = "MonitorVars"; this.Size = new System.Drawing.Size(1240, 600); this.groupBox1.ResumeLayout(false); this.groupBox1.PerformLayout(); + this.ListBoxMenu.ResumeLayout(false); this.ResumeLayout(false); } @@ -145,11 +159,13 @@ private ScottPlot.WinForms.FormsPlot MainFormPlot; private System.Windows.Forms.Button add_btn; - private System.Windows.Forms.Button del_btn; private System.Windows.Forms.ComboBox monitorItems_cb; private System.Windows.Forms.ListBox monitorItems_lb; private System.Windows.Forms.GroupBox groupBox1; private System.Windows.Forms.CheckBox AutoScale_check; private System.Windows.Forms.Button StartStop_btn; + private System.Windows.Forms.ContextMenuStrip ListBoxMenu; + private System.Windows.Forms.ToolStripMenuItem DeleteElement_menu; + private System.Windows.Forms.ToolStripMenuItem MoveElementToPlot1; } } diff --git a/TelemetryIO/MonitorVars.cs b/TelemetryIO/MonitorVars.cs index 04cd01e..2e93290 100644 --- a/TelemetryIO/MonitorVars.cs +++ b/TelemetryIO/MonitorVars.cs @@ -23,7 +23,6 @@ namespace TelemetryIO private System.Windows.Forms.Timer updateTimer = new System.Windows.Forms.Timer(); private float[] monitoring = new float[32]; - private int test_index = 0; private bool isSubscribed = false; private bool isMonitoring = true; private bool isAutoScale = true; @@ -37,7 +36,17 @@ namespace TelemetryIO updateTimer.Interval = 100; AutoScale_check.Checked = isAutoScale; - + + DeleteElement_menu.Click += (s, e) => + { + if(monitorItems_lb.SelectedIndex != ListBox.NoMatches) + { + processingIndex = monitorItems_lb.SelectedIndex; + serial.RemoveMonitoringItem(processingIndex); + } + }; + + monitorItems_cb.Items.AddRange(Telemetry.Instance.getKeys()); monitorItems_cb.SelectedIndex = 0; updateTimer.Tick += UpdateTimer_Tick; @@ -84,9 +93,11 @@ namespace TelemetryIO else if(e.Answer == "REMOVE") { monItems.RemoveAt(processingIndex); + if (monitorItems_lb.InvokeRequired) { - monitorItems_lb.Invoke(new Action(() => monitorItems_lb.Items.RemoveAt(processingIndex))); + monitorItems_lb.Invoke(new Action(() => + monitorItems_lb.Items.RemoveAt(processingIndex))); } else { @@ -173,7 +184,7 @@ namespace TelemetryIO } else { - monitorItems_lb.Items[i] = monItems[i].name + "\t" + monItems[i].Last; + monitorItems_lb.Items[i] = monItems[i].name + "\t" + monItems[i].Last.ToString("0.000"); } } MainFormPlot.Plot.Legend.IsVisible = true; @@ -197,12 +208,6 @@ namespace TelemetryIO serial.AddMonitoringItem(processingName); } - private void del_btn_Click(object sender, EventArgs e) - { - processingIndex = monitorItems_lb.SelectedIndex; - serial.RemoveMonitoringItem(processingIndex); - } - private void StartStop_btn_Click(object sender, EventArgs e) { if(isMonitoring) @@ -221,6 +226,19 @@ namespace TelemetryIO if (cb.Checked) isAutoScale = true; else isAutoScale = false; } + + private void monitorItems_lb_MouseUp(object sender, MouseEventArgs e) + { + if(e.Button == MouseButtons.Right) + { + int index = monitorItems_lb.IndexFromPoint(e.Location); + if(index != ListBox.NoMatches) + { + monitorItems_lb.SelectedIndex = index; + ListBoxMenu.Show(monitorItems_lb, e.Location); + } + } + } } public class MonitorItem @@ -259,6 +277,7 @@ namespace TelemetryIO System.Drawing.Color.LightGreen, System.Drawing.Color.YellowGreen, System.Drawing.Color.DarkRed, + System.Drawing.Color.Olive }; private void init() diff --git a/TelemetryIO/MonitorVars.resx b/TelemetryIO/MonitorVars.resx index 1af7de1..376698c 100644 --- a/TelemetryIO/MonitorVars.resx +++ b/TelemetryIO/MonitorVars.resx @@ -117,4 +117,7 @@ System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + 17, 17 + \ No newline at end of file diff --git a/TelemetryIO/SerialHandler.cs b/TelemetryIO/SerialHandler.cs index d560539..a0e80eb 100644 --- a/TelemetryIO/SerialHandler.cs +++ b/TelemetryIO/SerialHandler.cs @@ -133,8 +133,9 @@ namespace TelemetryIO int offset = DataSlots.bytes_to_int(unscape, 12); byte[] data = new byte[len]; Array.Copy(unscape, 16, data, 0, len); + Debug.WriteLine($"cmd = {cmd} *** slot = {slot} *** len = {len}"); - if (cmd == TELE_CMD_RD_ONCE) + if (cmd == TELE_CMD_RD_ONCE) { telemetry.setSlot(slot, data, offset, len); OnAnswerReceived(slot.ToString()); @@ -171,11 +172,13 @@ namespace TelemetryIO else if (cmd == TELE_CMD_RD_MON_ADD) { OnAnswerReceived("ADD"); + Debug.WriteLine($"cmd = {cmd} *** slot = {slot} *** len = {len}"); isMonitorEmpty = false; } else if (cmd == TELE_CMD_RD_MON_REMOVE) { OnAnswerReceived("REMOVE"); + Debug.WriteLine($"cmd = {cmd} *** slot = {slot} *** len = {len}"); } else if (cmd == TELE_CMD_RD_MON_REMOVEALL) { @@ -207,7 +210,7 @@ namespace TelemetryIO } }catch(Exception ex) { - Debug.WriteLine(ex.Message); + OnAnswerReceived("EXCEPTION" + ex.Message); } } diff --git a/TelemetryIO/TelemetryIO.csproj b/TelemetryIO/TelemetryIO.csproj index 6b42482..f5d8bb1 100644 --- a/TelemetryIO/TelemetryIO.csproj +++ b/TelemetryIO/TelemetryIO.csproj @@ -123,6 +123,12 @@ RatePID.cs + + UserControl + + + TestQuaternion.cs + MainForm.cs Designer @@ -145,6 +151,9 @@ RatePID.cs + + TestQuaternion.cs +