some fix

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

TelemetryIO/MainForm.cs

@@ -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

TelemetryIO/Models/Telemetry.cs

@@ -16,6 +16,9 @@ namespace TelemetryIO.Models
         private Dictionary<string, VarAddress> dictMonitor = new Dictionary<string, VarAddress>();//Словарь адресов, для целей мониторинга
         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;
 

TelemetryIO/MonitorVars.Designer.cs

@@ -28,15 +28,19 @@
         /// </summary>
         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;
     }
 }

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;
@@ -38,6 +37,16 @@ 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()

TelemetryIO/MonitorVars.resx

@@ -117,4 +117,7 @@
   <resheader name="writer">
     <value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
   </resheader>
+  <metadata name="ListBoxMenu.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
+    <value>17, 17</value>
+  </metadata>
 </root>

TelemetryIO/SerialHandler.cs

@@ -133,6 +133,7 @@ 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)
                             {
@@ -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);
             }
         }
 

TelemetryIO/TelemetryIO.csproj

@@ -123,6 +123,12 @@
       <DependentUpon>RatePID.cs</DependentUpon>
     </Compile>
     <Compile Include="SerialHandler.cs" />
+    <Compile Include="TestQuaternion.cs">
+      <SubType>UserControl</SubType>
+    </Compile>
+    <Compile Include="TestQuaternion.Designer.cs">
+      <DependentUpon>TestQuaternion.cs</DependentUpon>
+    </Compile>
     <EmbeddedResource Include="MainForm.resx">
       <DependentUpon>MainForm.cs</DependentUpon>
       <SubType>Designer</SubType>
@@ -145,6 +151,9 @@
     <EmbeddedResource Include="RatePID.resx">
       <DependentUpon>RatePID.cs</DependentUpon>
     </EmbeddedResource>
+    <EmbeddedResource Include="TestQuaternion.resx">
+      <DependentUpon>TestQuaternion.cs</DependentUpon>
+    </EmbeddedResource>
     <None Include="OpenTK.dll.config" />
     <None Include="packages.config" />
     <None Include="Properties\Settings.settings">