From f66f345878174f22abb4abde6d5c7bb17e1d6c83 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sat, 2 Mar 2024 17:45:19 -0500 Subject: [PATCH 01/45] GStreamer: fix crash when EOS occurs --- ExtLibs/Utilities/GStreamer.cs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ExtLibs/Utilities/GStreamer.cs b/ExtLibs/Utilities/GStreamer.cs index cbae46288e..8061d2e1ee 100644 --- a/ExtLibs/Utilities/GStreamer.cs +++ b/ExtLibs/Utilities/GStreamer.cs @@ -1440,7 +1440,8 @@ static void ThreadStart(object datao) } } - NativeMethods.gst_element_set_state(pipeline, GstState.GST_STATE_NULL); + if (!NativeMethods.gst_app_sink_is_eos(appsink)) + NativeMethods.gst_element_set_state(pipeline, GstState.GST_STATE_NULL); NativeMethods.gst_buffer_unref(bus); //callbackhandle.Free(); From 6ff3d05d2a5517c198a9f01c826dc46fc975584e Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sat, 2 Mar 2024 18:52:00 -0500 Subject: [PATCH 02/45] GimbalVideoControl: add mock UI --- GCSViews/FlightData.Designer.cs | 17 +- GCSViews/FlightData.cs | 15 ++ GCSViews/FlightData.resx | 27 ++- GCSViews/GimbalVideoControl.Designer.cs | 260 ++++++++++++++++++++++++ GCSViews/GimbalVideoControl.cs | 54 +++++ GCSViews/GimbalVideoControl.resx | 126 ++++++++++++ Properties/Resources.Designer.cs | 24 ++- Properties/Resources.resx | 5 +- Resources/no-video.png | Bin 0 -> 13959 bytes 9 files changed, 497 insertions(+), 31 deletions(-) create mode 100644 GCSViews/GimbalVideoControl.Designer.cs create mode 100644 GCSViews/GimbalVideoControl.cs create mode 100644 GCSViews/GimbalVideoControl.resx create mode 100644 Resources/no-video.png diff --git a/GCSViews/FlightData.Designer.cs b/GCSViews/FlightData.Designer.cs index d23a1ff518..557eb60268 100644 --- a/GCSViews/FlightData.Designer.cs +++ b/GCSViews/FlightData.Designer.cs @@ -1,4 +1,4 @@ -using System.Windows.Forms; +using System.Windows.Forms; namespace MissionPlanner.GCSViews { @@ -150,7 +150,7 @@ private void InitializeComponent() this.labelScriptStatus = new System.Windows.Forms.Label(); this.BUT_select_script = new MissionPlanner.Controls.MyButton(); this.tabPayload = new System.Windows.Forms.TabPage(); - this.BUT_PayloadFolder = new MissionPlanner.Controls.MyButton(); + this.BUT_GimbalVideo = new MissionPlanner.Controls.MyButton(); this.groupBoxRoll = new System.Windows.Forms.GroupBox(); this.TXT_gimbalRollPos = new System.Windows.Forms.TextBox(); this.bindingSourcePayloadTab = new System.Windows.Forms.BindingSource(this.components); @@ -2057,7 +2057,7 @@ private void InitializeComponent() // // tabPayload // - this.tabPayload.Controls.Add(this.BUT_PayloadFolder); + this.tabPayload.Controls.Add(this.BUT_GimbalVideo); this.tabPayload.Controls.Add(this.groupBoxRoll); this.tabPayload.Controls.Add(this.groupBoxYaw); this.tabPayload.Controls.Add(this.BUT_resetGimbalPos); @@ -2066,11 +2066,12 @@ private void InitializeComponent() this.tabPayload.Name = "tabPayload"; this.tabPayload.UseVisualStyleBackColor = true; // - // BUT_PayloadFolder + // BUT_GimbalVideo // - resources.ApplyResources(this.BUT_PayloadFolder, "BUT_PayloadFolder"); - this.BUT_PayloadFolder.Name = "BUT_PayloadFolder"; - this.BUT_PayloadFolder.UseVisualStyleBackColor = true; + resources.ApplyResources(this.BUT_GimbalVideo, "BUT_GimbalVideo"); + this.BUT_GimbalVideo.Name = "BUT_GimbalVideo"; + this.BUT_GimbalVideo.UseVisualStyleBackColor = true; + this.BUT_GimbalVideo.Click += new System.EventHandler(this.BUT_GimbalVideo_Click); // // groupBoxRoll // @@ -3066,7 +3067,7 @@ private void InitializeComponent() private System.Windows.Forms.GroupBox groupBoxRoll; private System.Windows.Forms.GroupBox groupBoxYaw; private System.Windows.Forms.GroupBox groupBoxPitch; - private Controls.MyButton BUT_PayloadFolder; + private Controls.MyButton BUT_GimbalVideo; private System.Windows.Forms.ToolStripMenuItem setHomeHereToolStripMenuItem1; private System.Windows.Forms.ToolStripMenuItem groundColorToolStripMenuItem; private Controls.RelayOptions relayOptions1; diff --git a/GCSViews/FlightData.cs b/GCSViews/FlightData.cs index 9e66efeb78..1efb58c5aa 100644 --- a/GCSViews/FlightData.cs +++ b/GCSViews/FlightData.cs @@ -6431,5 +6431,20 @@ private void jumpToTagToolStripMenuItem_Click(object sender, EventArgs e) CustomMessageBox.Show(Strings.CommandFailed + ex.ToString(), Strings.ERROR); } } + + private void BUT_GimbalVideo_Click(object sender, EventArgs e) + { + var form = new Form() + { + Text = "Gimbal Control", + Size = new Size(600, 400), + StartPosition = FormStartPosition.CenterParent + }; + form.Controls.Add(new GimbalVideoControl() + { + Dock = DockStyle.Fill + }); + form.Show(); + } } } diff --git a/GCSViews/FlightData.resx b/GCSViews/FlightData.resx index 3c83e3bb3b..d709f2d951 100644 --- a/GCSViews/FlightData.resx +++ b/GCSViews/FlightData.resx @@ -1,4 +1,4 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + text/microsoft-resx + + + 2.0 + + + System.Resources.ResXResourceReader, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + 17, 17 + + + 194, 17 + + \ No newline at end of file diff --git a/Properties/Resources.Designer.cs b/Properties/Resources.Designer.cs index 3cdc41c1ae..3958e7d0c3 100644 --- a/Properties/Resources.Designer.cs +++ b/Properties/Resources.Designer.cs @@ -1,4 +1,4 @@ -//------------------------------------------------------------------------------ +//------------------------------------------------------------------------------ // // This code was generated by a tool. // Runtime Version:4.0.30319.42000 @@ -1162,12 +1162,12 @@ public static string mavcmd { } /// - /// Looks up a localized string similar to == MAVLink Parameters == (this is a copy fo the wiki page FYI) - /// - ///This is a list of all the user-modifiable MAVLink parameters and what they do. You can modify them via the MAVLink parameters window in any compatible GCS, such as the Mission Planner, HK GCS or !QGroundControl. - /// - ///It includes both fixed wing (APM) and rotary wing (!ArduCopter) parameters. Some may only be relevant for one platform or another. - /// + /// Looks up a localized string similar to == MAVLink Parameters == (this is a copy fo the wiki page FYI) + /// + ///This is a list of all the user-modifiable MAVLink parameters and what they do. You can modify them via the MAVLink parameters window in any compatible GCS, such as the Mission Planner, HK GCS or !QGroundControl. + /// + ///It includes both fixed wing (APM) and rotary wing (!ArduCopter) parameters. Some may only be relevant for one platform or another. + /// ///|| *EEPROM variable name* || *Min* || *Max* || *Default* || *Multiplier* || *Enabled (0 = no, 1 = yes)* [rest of string was truncated]";. /// public static string MAVParam { @@ -1226,6 +1226,16 @@ public static System.Drawing.Bitmap new_3DR_04 { } } + /// + /// Looks up a localized resource of type System.Drawing.Bitmap. + /// + public static System.Drawing.Bitmap no_video { + get { + object obj = ResourceManager.GetObject("no-video", resourceCulture); + return ((System.Drawing.Bitmap)(obj)); + } + } + /// /// Looks up a localized resource of type System.Drawing.Bitmap. /// diff --git a/Properties/Resources.resx b/Properties/Resources.resx index 47ba7f8780..b520f8ab6a 100644 --- a/Properties/Resources.resx +++ b/Properties/Resources.resx @@ -1,4 +1,4 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + text/microsoft-resx + + + 2.0 + + + System.Resources.ResXResourceReader, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + \ No newline at end of file diff --git a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs index d038b981ea..e5472f09a1 100644 --- a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs +++ b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs @@ -129,21 +129,29 @@ private async Task RequestCameraInformationAsync() if(parent?.parent != null) { // New-style request - var resp = await parent?.parent?.doCommandAsync( + var resp = await parent.parent.doCommandAsync( parent.sysid, parent.compid, MAVLink.MAV_CMD.REQUEST_MESSAGE, (float)MAVLink.MAVLINK_MSG_ID.CAMERA_INFORMATION, 0, 0, 0, 0, 0, 0 ); // Fall back to deprecated request message - if (resp) + if (!resp) { - await parent?.parent?.doCommandAsync( + await parent.parent.doCommandAsync( parent.sysid, parent.compid, MAVLink.MAV_CMD.REQUEST_CAMERA_INFORMATION, 0, 0, 0, 0, 0, 0, 0 ); } + + // Get video stream information as well + await parent.parent.doCommandAsync( + parent.sysid, parent.compid, + MAVLink.MAV_CMD.REQUEST_MESSAGE, + (float)MAVLink.MAVLINK_MSG_ID.VIDEO_STREAM_INFORMATION, + 0, 0, 0, 0, 0, 0 + ); } } catch (Exception ex) @@ -175,6 +183,10 @@ public void ParseMessages(object sender, MAVLink.MAVLinkMessage message) case MAVLink.MAVLINK_MSG_ID.CAMERA_CAPTURE_STATUS: CameraCaptureStatus = (MAVLink.mavlink_camera_capture_status_t)message.data; break; + case MAVLink.MAVLINK_MSG_ID.VIDEO_STREAM_INFORMATION: + var video_stream_info = (MAVLink.mavlink_video_stream_information_t)message.data; + VideoStreams[(parent.sysid, parent.compid, video_stream_info.stream_id)] = video_stream_info; + break; case MAVLink.MAVLINK_MSG_ID.CAMERA_FOV_STATUS: CameraFOVStatus = (MAVLink.mavlink_camera_fov_status_t)message.data; break; From b76217777715fedbeeab7ebe9ef1ab7eda70bbde Mon Sep 17 00:00:00 2001 From: Bob Long Date: Mon, 5 Aug 2024 00:34:22 +1000 Subject: [PATCH 17/45] GimbalVideoControl: RenderFrame: use Invoke --- Controls/GimbalVideoControl.cs | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs index 99abf5858c..9f3ac98166 100644 --- a/Controls/GimbalVideoControl.cs +++ b/Controls/GimbalVideoControl.cs @@ -143,12 +143,18 @@ private void setCameraControlPanelVisibility(bool visibility) private void RenderFrame(object sender, MPBitmap image) { + if (InvokeRequired) + { + BeginInvoke(new Action(() => RenderFrame(sender, image))); + return; + } try { - if (image == null) + if (image == null || image.Width <= 0 || image.Height <= 0) { VideoBox.Image?.Dispose(); VideoBox.Image = null; + VideoBox.Image = VideoBox.ErrorImage; return; } From bd7cf003c4d1ac10214f53d9b7e0b3c74a3104eb Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sat, 14 Sep 2024 14:19:34 +1000 Subject: [PATCH 18/45] GimbalVideoControl: remove side-panel UI Do everything through keyboard and context menu --- Controls/GimbalVideoControl.Designer.cs | 212 ++++++++---------------- Controls/GimbalVideoControl.cs | 7 - Controls/GimbalVideoControl.resx | 3 + 3 files changed, 76 insertions(+), 146 deletions(-) diff --git a/Controls/GimbalVideoControl.Designer.cs b/Controls/GimbalVideoControl.Designer.cs index 02f1770b20..98fbadd4fe 100644 --- a/Controls/GimbalVideoControl.Designer.cs +++ b/Controls/GimbalVideoControl.Designer.cs @@ -22,37 +22,31 @@ private void InitializeComponent() this.toolStripMenuItem1 = new System.Windows.Forms.ToolStripSeparator(); this.retractToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.neutralToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.pointDownToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.pointHomeToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.yawLockToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.toolStripSeparator1 = new System.Windows.Forms.ToolStripSeparator(); + this.takePictureToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.startRecordingToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.toolStripMenuItem2 = new System.Windows.Forms.ToolStripSeparator(); - this.hideControlsToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.settingsToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); - this.MainLayoutPanel = new System.Windows.Forms.TableLayoutPanel(); - this.CameraLayoutPanel = new System.Windows.Forms.FlowLayoutPanel(); - this.TakePictureButton = new MissionPlanner.Controls.MyButton(); - this.RecordVideoButton = new MissionPlanner.Controls.MyButton(); - this.ZoomTrackBar = new MissionPlanner.Controls.MyTrackBar(); - this.ZoomLabel = new System.Windows.Forms.Label(); - this.LockButton = new MissionPlanner.Controls.MyButton(); this.ControlInfoTooltip = new System.Windows.Forms.ToolTip(this.components); + this.UITimer = new System.Windows.Forms.Timer(this.components); ((System.ComponentModel.ISupportInitialize)(this.VideoBox)).BeginInit(); this.VideoBoxContextMenu.SuspendLayout(); - this.MainLayoutPanel.SuspendLayout(); - this.CameraLayoutPanel.SuspendLayout(); - ((System.ComponentModel.ISupportInitialize)(this.ZoomTrackBar)).BeginInit(); this.SuspendLayout(); // // VideoBox // - this.VideoBox.Anchor = ((System.Windows.Forms.AnchorStyles)((((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Bottom) - | System.Windows.Forms.AnchorStyles.Left) - | System.Windows.Forms.AnchorStyles.Right))); this.VideoBox.ContextMenuStrip = this.VideoBoxContextMenu; + this.VideoBox.Dock = System.Windows.Forms.DockStyle.Fill; this.VideoBox.ErrorImage = global::MissionPlanner.Properties.Resources.no_video; this.VideoBox.Image = global::MissionPlanner.Properties.Resources.no_video; this.VideoBox.InitialImage = null; - this.VideoBox.Location = new System.Drawing.Point(37, 0); + this.VideoBox.Location = new System.Drawing.Point(0, 0); this.VideoBox.Margin = new System.Windows.Forms.Padding(0); this.VideoBox.Name = "VideoBox"; - this.VideoBox.Size = new System.Drawing.Size(511, 382); + this.VideoBox.Size = new System.Drawing.Size(640, 480); this.VideoBox.SizeMode = System.Windows.Forms.PictureBoxSizeMode.Zoom; this.VideoBox.TabIndex = 0; this.VideoBox.TabStop = false; @@ -67,163 +61,104 @@ private void InitializeComponent() this.toolStripMenuItem1, this.retractToolStripMenuItem, this.neutralToolStripMenuItem, + this.pointDownToolStripMenuItem, + this.pointHomeToolStripMenuItem, + this.yawLockToolStripMenuItem, + this.toolStripSeparator1, + this.takePictureToolStripMenuItem, + this.startRecordingToolStripMenuItem, this.toolStripMenuItem2, - this.hideControlsToolStripMenuItem, this.settingsToolStripMenuItem}); this.VideoBoxContextMenu.Name = "VideoBoxContextMenu"; - this.VideoBoxContextMenu.Size = new System.Drawing.Size(148, 126); + this.VideoBoxContextMenu.Size = new System.Drawing.Size(156, 220); // // videoStreamToolStripMenuItem // this.videoStreamToolStripMenuItem.Name = "videoStreamToolStripMenuItem"; - this.videoStreamToolStripMenuItem.Size = new System.Drawing.Size(147, 22); + this.videoStreamToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.videoStreamToolStripMenuItem.Text = "Video Stream"; this.videoStreamToolStripMenuItem.Click += new System.EventHandler(this.videoStreamToolStripMenuItem_Click); // // toolStripMenuItem1 // this.toolStripMenuItem1.Name = "toolStripMenuItem1"; - this.toolStripMenuItem1.Size = new System.Drawing.Size(144, 6); + this.toolStripMenuItem1.Size = new System.Drawing.Size(152, 6); // // retractToolStripMenuItem // this.retractToolStripMenuItem.Name = "retractToolStripMenuItem"; - this.retractToolStripMenuItem.Size = new System.Drawing.Size(147, 22); + this.retractToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.retractToolStripMenuItem.Text = "Retract"; // // neutralToolStripMenuItem // this.neutralToolStripMenuItem.Name = "neutralToolStripMenuItem"; - this.neutralToolStripMenuItem.Size = new System.Drawing.Size(147, 22); + this.neutralToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.neutralToolStripMenuItem.Text = "Neutral"; // - // toolStripMenuItem2 + // pointDownToolStripMenuItem // - this.toolStripMenuItem2.Name = "toolStripMenuItem2"; - this.toolStripMenuItem2.Size = new System.Drawing.Size(144, 6); + this.pointDownToolStripMenuItem.Name = "pointDownToolStripMenuItem"; + this.pointDownToolStripMenuItem.Size = new System.Drawing.Size(155, 22); + this.pointDownToolStripMenuItem.Text = "Point Down"; + // + // pointHomeToolStripMenuItem + // + this.pointHomeToolStripMenuItem.Name = "pointHomeToolStripMenuItem"; + this.pointHomeToolStripMenuItem.Size = new System.Drawing.Size(155, 22); + this.pointHomeToolStripMenuItem.Text = "Point Home"; + // + // yawLockToolStripMenuItem + // + this.yawLockToolStripMenuItem.Name = "yawLockToolStripMenuItem"; + this.yawLockToolStripMenuItem.Size = new System.Drawing.Size(155, 22); + this.yawLockToolStripMenuItem.Text = "Yaw Lock"; + this.yawLockToolStripMenuItem.Click += new System.EventHandler(this.yawLockToolStripMenuItem_Click); + // + // toolStripSeparator1 + // + this.toolStripSeparator1.Name = "toolStripSeparator1"; + this.toolStripSeparator1.Size = new System.Drawing.Size(152, 6); // - // hideControlsToolStripMenuItem + // takePictureToolStripMenuItem // - this.hideControlsToolStripMenuItem.Name = "hideControlsToolStripMenuItem"; - this.hideControlsToolStripMenuItem.Size = new System.Drawing.Size(147, 22); - this.hideControlsToolStripMenuItem.Text = "Hide Controls"; + this.takePictureToolStripMenuItem.Name = "takePictureToolStripMenuItem"; + this.takePictureToolStripMenuItem.Size = new System.Drawing.Size(155, 22); + this.takePictureToolStripMenuItem.Text = "Take Picture"; + // + // startRecordingToolStripMenuItem + // + this.startRecordingToolStripMenuItem.Name = "startRecordingToolStripMenuItem"; + this.startRecordingToolStripMenuItem.Size = new System.Drawing.Size(155, 22); + this.startRecordingToolStripMenuItem.Text = "Start Recording"; + // + // toolStripMenuItem2 + // + this.toolStripMenuItem2.Name = "toolStripMenuItem2"; + this.toolStripMenuItem2.Size = new System.Drawing.Size(152, 6); // // settingsToolStripMenuItem // this.settingsToolStripMenuItem.Name = "settingsToolStripMenuItem"; - this.settingsToolStripMenuItem.Size = new System.Drawing.Size(147, 22); + this.settingsToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.settingsToolStripMenuItem.Text = "Settings"; // - // MainLayoutPanel - // - this.MainLayoutPanel.ColumnCount = 2; - this.MainLayoutPanel.ColumnStyles.Add(new System.Windows.Forms.ColumnStyle(System.Windows.Forms.SizeType.Absolute, 37F)); - this.MainLayoutPanel.ColumnStyles.Add(new System.Windows.Forms.ColumnStyle()); - this.MainLayoutPanel.ColumnStyles.Add(new System.Windows.Forms.ColumnStyle(System.Windows.Forms.SizeType.Percent, 100F)); - this.MainLayoutPanel.Controls.Add(this.VideoBox, 1, 0); - this.MainLayoutPanel.Controls.Add(this.CameraLayoutPanel, 0, 0); - this.MainLayoutPanel.Dock = System.Windows.Forms.DockStyle.Fill; - this.MainLayoutPanel.Location = new System.Drawing.Point(0, 0); - this.MainLayoutPanel.Margin = new System.Windows.Forms.Padding(1); - this.MainLayoutPanel.Name = "MainLayoutPanel"; - this.MainLayoutPanel.RowCount = 1; - this.MainLayoutPanel.RowStyles.Add(new System.Windows.Forms.RowStyle(System.Windows.Forms.SizeType.Percent, 100F)); - this.MainLayoutPanel.Size = new System.Drawing.Size(548, 382); - this.MainLayoutPanel.TabIndex = 1; - // - // CameraLayoutPanel - // - this.CameraLayoutPanel.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Bottom))); - this.CameraLayoutPanel.Controls.Add(this.TakePictureButton); - this.CameraLayoutPanel.Controls.Add(this.RecordVideoButton); - this.CameraLayoutPanel.Controls.Add(this.ZoomTrackBar); - this.CameraLayoutPanel.Controls.Add(this.ZoomLabel); - this.CameraLayoutPanel.Controls.Add(this.LockButton); - this.CameraLayoutPanel.Location = new System.Drawing.Point(0, 0); - this.CameraLayoutPanel.Margin = new System.Windows.Forms.Padding(0); - this.CameraLayoutPanel.Name = "CameraLayoutPanel"; - this.CameraLayoutPanel.Size = new System.Drawing.Size(37, 382); - this.CameraLayoutPanel.TabIndex = 1; - // - // TakePictureButton - // - this.TakePictureButton.Location = new System.Drawing.Point(2, 2); - this.TakePictureButton.Margin = new System.Windows.Forms.Padding(2); - this.TakePictureButton.Name = "TakePictureButton"; - this.TakePictureButton.Size = new System.Drawing.Size(33, 33); - this.TakePictureButton.TabIndex = 0; - this.TakePictureButton.Text = "Pic"; - this.TakePictureButton.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); - this.ControlInfoTooltip.SetToolTip(this.TakePictureButton, "Take a picture"); - this.TakePictureButton.UseVisualStyleBackColor = true; - // - // RecordVideoButton - // - this.RecordVideoButton.Location = new System.Drawing.Point(2, 39); - this.RecordVideoButton.Margin = new System.Windows.Forms.Padding(2); - this.RecordVideoButton.Name = "RecordVideoButton"; - this.RecordVideoButton.Size = new System.Drawing.Size(33, 33); - this.RecordVideoButton.TabIndex = 1; - this.RecordVideoButton.Text = "Rec"; - this.RecordVideoButton.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); - this.ControlInfoTooltip.SetToolTip(this.RecordVideoButton, "Start/stop recording"); - this.RecordVideoButton.UseVisualStyleBackColor = true; - // - // ZoomTrackBar - // - this.ZoomTrackBar.Anchor = ((System.Windows.Forms.AnchorStyles)(((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Left) - | System.Windows.Forms.AnchorStyles.Right))); - this.ZoomTrackBar.AutoSize = false; - this.ZoomTrackBar.LargeChange = 0.005F; - this.ZoomTrackBar.Location = new System.Drawing.Point(2, 76); - this.ZoomTrackBar.Margin = new System.Windows.Forms.Padding(2); - this.ZoomTrackBar.Maximum = 0.01F; - this.ZoomTrackBar.Minimum = 0F; - this.ZoomTrackBar.Name = "ZoomTrackBar"; - this.ZoomTrackBar.Orientation = System.Windows.Forms.Orientation.Vertical; - this.ZoomTrackBar.Size = new System.Drawing.Size(33, 81); - this.ZoomTrackBar.SmallChange = 0.001F; - this.ZoomTrackBar.TabIndex = 4; - this.ZoomTrackBar.TickFrequency = 0.001F; - this.ZoomTrackBar.Value = 0F; - // - // ZoomLabel - // - this.ZoomLabel.AutoSize = true; - this.ZoomLabel.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.5F); - this.ZoomLabel.Location = new System.Drawing.Point(1, 159); - this.ZoomLabel.Margin = new System.Windows.Forms.Padding(1, 0, 1, 0); - this.ZoomLabel.Name = "ZoomLabel"; - this.ZoomLabel.Size = new System.Drawing.Size(33, 13); - this.ZoomLabel.TabIndex = 5; - this.ZoomLabel.Text = "Zoom"; - // - // LockButton - // - this.LockButton.Location = new System.Drawing.Point(2, 174); - this.LockButton.Margin = new System.Windows.Forms.Padding(2); - this.LockButton.Name = "LockButton"; - this.LockButton.Size = new System.Drawing.Size(33, 33); - this.LockButton.TabIndex = 8; - this.LockButton.Text = "Loc"; - this.LockButton.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); - this.ControlInfoTooltip.SetToolTip(this.LockButton, "Yaw frame lock/follow"); - this.LockButton.UseVisualStyleBackColor = true; + // UITimer + // + this.UITimer.Enabled = true; + this.UITimer.Interval = 500; + this.UITimer.Tick += new System.EventHandler(this.UITimer_Tick); // // GimbalVideoControl // this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; - this.Controls.Add(this.MainLayoutPanel); + this.Controls.Add(this.VideoBox); this.Margin = new System.Windows.Forms.Padding(0); this.Name = "GimbalVideoControl"; - this.Size = new System.Drawing.Size(548, 382); + this.Size = new System.Drawing.Size(640, 480); ((System.ComponentModel.ISupportInitialize)(this.VideoBox)).EndInit(); this.VideoBoxContextMenu.ResumeLayout(false); - this.MainLayoutPanel.ResumeLayout(false); - this.CameraLayoutPanel.ResumeLayout(false); - this.CameraLayoutPanel.PerformLayout(); - ((System.ComponentModel.ISupportInitialize)(this.ZoomTrackBar)).EndInit(); this.ResumeLayout(false); } @@ -231,13 +166,6 @@ private void InitializeComponent() #endregion private System.Windows.Forms.PictureBox VideoBox; - private System.Windows.Forms.TableLayoutPanel MainLayoutPanel; - private System.Windows.Forms.FlowLayoutPanel CameraLayoutPanel; - private Controls.MyButton TakePictureButton; - private Controls.MyButton RecordVideoButton; - private Controls.MyTrackBar ZoomTrackBar; - private System.Windows.Forms.Label ZoomLabel; - private Controls.MyButton LockButton; private System.Windows.Forms.ToolTip ControlInfoTooltip; private System.Windows.Forms.ContextMenuStrip VideoBoxContextMenu; private System.Windows.Forms.ToolStripMenuItem videoStreamToolStripMenuItem; @@ -245,7 +173,13 @@ private void InitializeComponent() private System.Windows.Forms.ToolStripMenuItem neutralToolStripMenuItem; private System.Windows.Forms.ToolStripSeparator toolStripMenuItem1; private System.Windows.Forms.ToolStripSeparator toolStripMenuItem2; - private System.Windows.Forms.ToolStripMenuItem hideControlsToolStripMenuItem; private System.Windows.Forms.ToolStripMenuItem settingsToolStripMenuItem; + private System.Windows.Forms.ToolStripMenuItem pointDownToolStripMenuItem; + private System.Windows.Forms.ToolStripMenuItem pointHomeToolStripMenuItem; + private System.Windows.Forms.ToolStripMenuItem yawLockToolStripMenuItem; + private System.Windows.Forms.Timer UITimer; + private System.Windows.Forms.ToolStripSeparator toolStripSeparator1; + private System.Windows.Forms.ToolStripMenuItem takePictureToolStripMenuItem; + private System.Windows.Forms.ToolStripMenuItem startRecordingToolStripMenuItem; } } diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs index 9f3ac98166..91eb1db47b 100644 --- a/Controls/GimbalVideoControl.cs +++ b/Controls/GimbalVideoControl.cs @@ -108,8 +108,6 @@ private void loadPreferences() } } - setCameraControlPanelVisibility(preferences.ShowCameraControls); - // Populate the list of keys that are expected to be pressed boundPressKeys.Clear(); boundPressKeys.Add(preferences.TakePicture); @@ -136,11 +134,6 @@ private void loadPreferences() boundHoldKeys.Add(mod2key[preferences.SlewSlowModifier]); } - private void setCameraControlPanelVisibility(bool visibility) - { - CameraLayoutPanel.Visible = visibility; - } - private void RenderFrame(object sender, MPBitmap image) { if (InvokeRequired) diff --git a/Controls/GimbalVideoControl.resx b/Controls/GimbalVideoControl.resx index 699f12a089..97b70718cb 100644 --- a/Controls/GimbalVideoControl.resx +++ b/Controls/GimbalVideoControl.resx @@ -123,4 +123,7 @@ 194, 17 + + 345, 17 + \ No newline at end of file From c1498e7e870d2e134bdda2b7b85681579b491b9b Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sat, 14 Sep 2024 14:24:58 +1000 Subject: [PATCH 19/45] GimbalManagerProtocol: redefine gimbal [0] Use this to store the lowest found gimbal instead of using it to store capabilities flags of all gimbals. --- .../Mavlink/GimbalManagerProtocol.cs | 37 +++++++++++-------- 1 file changed, 21 insertions(+), 16 deletions(-) diff --git a/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs b/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs index 32251314c8..98ab6e094d 100644 --- a/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs +++ b/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs @@ -6,17 +6,21 @@ namespace MissionPlanner.ArduPilot.Mavlink { public class GimbalManagerProtocol { + // Stores the last GIMBAL_MANAGER_INFORMATION message for each gimbal device/component ID. + // This index will be 1-6, or MAVLink component IDs 154, 171-175. + // Index 0 is used to store the message of the first (lowest) gimbal ID. public ConcurrentDictionary ManagerInfo = - new ConcurrentDictionary( - new Dictionary() - { - {0, new MAVLink.mavlink_gimbal_manager_information_t() {cap_flags = 0} } - } - ); + new ConcurrentDictionary(); + // Stores the GIMBAL_MANAGER_STATUS message for each gimbal device/component ID. + // This index will be 1-6, or MAVLink component IDs 154, 171-175. + // Index 0 is used to store the message of the first (lowest) gimbal ID. public ConcurrentDictionary ManagerStatus = new ConcurrentDictionary(); + // Stores the GIMBAL_DEVICE_ATTITUDE_STATUS message for each gimbal device/component ID. + // This index will be 1-6, or MAVLink component IDs 154, 171-175. + // Index 0 is used to store the message of the first (lowest) gimbal ID. public ConcurrentDictionary GimbalStatus = new ConcurrentDictionary(); @@ -39,30 +43,31 @@ public void Discover() { var gmi = (MAVLink.mavlink_gimbal_manager_information_t)message.data; - // It is invalid to have a gimbal device ID of 0. This field should be a component ID or a number 1-6 - if (gmi.gimbal_device_id == 0) - { - return; - } - ManagerInfo[gmi.gimbal_device_id] = gmi; - // Keep a ManagerInfo element 0 to store capabilities of any gimbal - ManagerInfo[0] = new MAVLink.mavlink_gimbal_manager_information_t() + if (!ManagerInfo.ContainsKey(0) || gmi.gimbal_device_id <= ManagerInfo[0].gimbal_device_id) { - cap_flags = gmi.cap_flags | ManagerInfo[0].cap_flags - }; + ManagerInfo[0] = gmi; + } } if (message.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GIMBAL_MANAGER_STATUS) { var gms = (MAVLink.mavlink_gimbal_manager_status_t)message.data; ManagerStatus[gms.gimbal_device_id] = gms; + if (!ManagerStatus.ContainsKey(0) || gms.gimbal_device_id <= ManagerStatus[0].gimbal_device_id) + { + ManagerStatus[0] = gms; + } } if (message.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GIMBAL_DEVICE_ATTITUDE_STATUS) { var gds = (MAVLink.mavlink_gimbal_device_attitude_status_t)message.data; GimbalStatus[gds.gimbal_device_id] = gds; + if (!GimbalStatus.ContainsKey(0) || gds.gimbal_device_id <= GimbalStatus[0].gimbal_device_id) + { + GimbalStatus[0] = gds; + } } }; } From 020532de7df3fc9e5678fa7545486f8d2d19bef1 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sun, 15 Sep 2024 11:32:18 +1000 Subject: [PATCH 20/45] CameraProtocol: ack not needed for most requests --- ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs index e5472f09a1..a9261f8bcb 100644 --- a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs +++ b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs @@ -141,7 +141,8 @@ private async Task RequestCameraInformationAsync() await parent.parent.doCommandAsync( parent.sysid, parent.compid, MAVLink.MAV_CMD.REQUEST_CAMERA_INFORMATION, - 0, 0, 0, 0, 0, 0, 0 + 0, 0, 0, 0, 0, 0, 0, + false // Don't wait for response ); } @@ -150,7 +151,8 @@ await parent.parent.doCommandAsync( parent.sysid, parent.compid, MAVLink.MAV_CMD.REQUEST_MESSAGE, (float)MAVLink.MAVLINK_MSG_ID.VIDEO_STREAM_INFORMATION, - 0, 0, 0, 0, 0, 0 + 0, 0, 0, 0, 0, 0, + false // Don't wait for response ); } } @@ -207,10 +209,7 @@ public void RequestMessageIntervals(int ratehz) float interval_us = (float)(1e6 / ratehz); - if (!have_camera_information) - { - Task.Run(RequestCameraInformationAsync); - } + Task.Run(RequestCameraInformationAsync); // Request FOV status Task.Run(async () => @@ -220,7 +219,8 @@ await parent.parent.doCommandAsync( MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL, (float)MAVLink.MAVLINK_MSG_ID.CAMERA_FOV_STATUS, interval_us, - 0, 0, 0, 0, 0 + 0, 0, 0, 0, 0, + false // Don't wait for response ).ConfigureAwait(false); }); @@ -234,7 +234,8 @@ await parent.parent.doCommandAsync( MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL, (float)MAVLink.MAVLINK_MSG_ID.CAMERA_SETTINGS, interval_us, - 0, 0, 0, 0, 0 + 0, 0, 0, 0, 0, + false // Don't wait for response ).ConfigureAwait(false); }); } @@ -251,7 +252,8 @@ await parent.parent.doCommandAsync( MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL, (float)MAVLink.MAVLINK_MSG_ID.CAMERA_CAPTURE_STATUS, interval_us, - 0, 0, 0, 0, 0 + 0, 0, 0, 0, 0, + false // Don't wait for response ).ConfigureAwait(false); }); } From 687c6f33018ee37d6acc4e6c4f16029b15eeab2e Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sun, 15 Sep 2024 12:21:30 +1000 Subject: [PATCH 21/45] GimbalVideoControl: implement buttons --- Controls/GimbalVideoControl.Designer.cs | 20 ++- Controls/GimbalVideoControl.cs | 182 +++++++++++++++++++----- 2 files changed, 166 insertions(+), 36 deletions(-) diff --git a/Controls/GimbalVideoControl.Designer.cs b/Controls/GimbalVideoControl.Designer.cs index 98fbadd4fe..dc830d8d99 100644 --- a/Controls/GimbalVideoControl.Designer.cs +++ b/Controls/GimbalVideoControl.Designer.cs @@ -32,6 +32,7 @@ private void InitializeComponent() this.settingsToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.ControlInfoTooltip = new System.Windows.Forms.ToolTip(this.components); this.UITimer = new System.Windows.Forms.Timer(this.components); + this.stopRecordingToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); ((System.ComponentModel.ISupportInitialize)(this.VideoBox)).BeginInit(); this.VideoBoxContextMenu.SuspendLayout(); this.SuspendLayout(); @@ -67,10 +68,11 @@ private void InitializeComponent() this.toolStripSeparator1, this.takePictureToolStripMenuItem, this.startRecordingToolStripMenuItem, + this.stopRecordingToolStripMenuItem, this.toolStripMenuItem2, this.settingsToolStripMenuItem}); this.VideoBoxContextMenu.Name = "VideoBoxContextMenu"; - this.VideoBoxContextMenu.Size = new System.Drawing.Size(156, 220); + this.VideoBoxContextMenu.Size = new System.Drawing.Size(181, 264); // // videoStreamToolStripMenuItem // @@ -89,24 +91,28 @@ private void InitializeComponent() this.retractToolStripMenuItem.Name = "retractToolStripMenuItem"; this.retractToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.retractToolStripMenuItem.Text = "Retract"; + this.retractToolStripMenuItem.Click += new System.EventHandler(this.retractToolStripMenuItem_Click); // // neutralToolStripMenuItem // this.neutralToolStripMenuItem.Name = "neutralToolStripMenuItem"; this.neutralToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.neutralToolStripMenuItem.Text = "Neutral"; + this.neutralToolStripMenuItem.Click += new System.EventHandler(this.neutralToolStripMenuItem_Click); // // pointDownToolStripMenuItem // this.pointDownToolStripMenuItem.Name = "pointDownToolStripMenuItem"; this.pointDownToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.pointDownToolStripMenuItem.Text = "Point Down"; + this.pointDownToolStripMenuItem.Click += new System.EventHandler(this.pointDownToolStripMenuItem_Click); // // pointHomeToolStripMenuItem // this.pointHomeToolStripMenuItem.Name = "pointHomeToolStripMenuItem"; this.pointHomeToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.pointHomeToolStripMenuItem.Text = "Point Home"; + this.pointHomeToolStripMenuItem.Click += new System.EventHandler(this.pointHomeToolStripMenuItem_Click); // // yawLockToolStripMenuItem // @@ -125,12 +131,14 @@ private void InitializeComponent() this.takePictureToolStripMenuItem.Name = "takePictureToolStripMenuItem"; this.takePictureToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.takePictureToolStripMenuItem.Text = "Take Picture"; + this.takePictureToolStripMenuItem.Click += new System.EventHandler(this.takePictureToolStripMenuItem_Click); // // startRecordingToolStripMenuItem // this.startRecordingToolStripMenuItem.Name = "startRecordingToolStripMenuItem"; - this.startRecordingToolStripMenuItem.Size = new System.Drawing.Size(155, 22); + this.startRecordingToolStripMenuItem.Size = new System.Drawing.Size(180, 22); this.startRecordingToolStripMenuItem.Text = "Start Recording"; + this.startRecordingToolStripMenuItem.Click += new System.EventHandler(this.startRecordingToolStripMenuItem_Click); // // toolStripMenuItem2 // @@ -149,6 +157,13 @@ private void InitializeComponent() this.UITimer.Interval = 500; this.UITimer.Tick += new System.EventHandler(this.UITimer_Tick); // + // stopRecordingToolStripMenuItem + // + this.stopRecordingToolStripMenuItem.Name = "stopRecordingToolStripMenuItem"; + this.stopRecordingToolStripMenuItem.Size = new System.Drawing.Size(180, 22); + this.stopRecordingToolStripMenuItem.Text = "Stop Recording"; + this.stopRecordingToolStripMenuItem.Click += new System.EventHandler(this.stopRecordingToolStripMenuItem_Click); + // // GimbalVideoControl // this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); @@ -181,5 +196,6 @@ private void InitializeComponent() private System.Windows.Forms.ToolStripSeparator toolStripSeparator1; private System.Windows.Forms.ToolStripMenuItem takePictureToolStripMenuItem; private System.Windows.Forms.ToolStripMenuItem startRecordingToolStripMenuItem; + private System.Windows.Forms.ToolStripMenuItem stopRecordingToolStripMenuItem; } } diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs index 91eb1db47b..e152288590 100644 --- a/Controls/GimbalVideoControl.cs +++ b/Controls/GimbalVideoControl.cs @@ -47,8 +47,6 @@ public partial class GimbalVideoControl : UserControl, IMessageFilter private float previousPitchRate = 0; private float previousYawRate = 0; private float previousZoomRate = 0; - private UInt32 gimbalManagerFlags = 0; - private byte gimbalDeviceId = 0; private bool yaw_lock = false; private CameraProtocol _selectedCamera; @@ -64,6 +62,20 @@ private CameraProtocol selectedCamera } } + private bool isRecording + { + // TODO: ArduPilot hard-codes this to 0 presently, so we can't check it + /*get + { + return selectedCamera?.CameraCaptureStatus.video_status > 0; + }*/ + + // So for now, we will manually track it + // TODO: Remove this once ArduPilot is fixed + get; + set; + } + private GimbalManagerProtocol _selectedGimbalManager; private GimbalManagerProtocol selectedGimbalManager { @@ -77,6 +89,10 @@ private GimbalManagerProtocol selectedGimbalManager } } + // The selected gimbal ID for the currently-selected gimbal manager + // (0 means all gimbals) + private byte selectedGimbalID = 0; + private GMapOverlay mouseMapMarker; public GimbalVideoControl() { @@ -119,6 +135,7 @@ private void loadPreferences() boundPressKeys.Add(preferences.SetFollow); boundPressKeys.Add(preferences.Retract); boundPressKeys.Add(preferences.Neutral); + boundPressKeys.Add(preferences.PointDown); boundPressKeys.Add(preferences.Home); // Populate the list of keys that are expected to be held down @@ -212,11 +229,6 @@ private void videoStreamToolStripMenuItem_Click(object sender, EventArgs e) { _stream.Start(form.gstreamer_pipeline); } - - //_stream.Start("rtspsrc location=rtsp://192.168.144.25:8554/main.264 latency=41 udp-reconnect=1 timeout=0 do-retransmission=false ! application/x-rtp ! decodebin3 ! queue leaky=2 ! videoconvert ! video/x-raw,format=BGRA ! appsink name=outsink sync=false"); - //_stream.Start("rtspsrc location=rtsp://127.0.0.1:8554/live latency=41 udp-reconnect=1 timeout=0 do-retransmission=false ! application/x-rtp ! decodebin3 ! queue leaky=2 ! videoconvert ! video/x-raw,format=BGRA ! appsink name=outsink sync=false"); - //_stream.Start("videotestsrc ! video/x-raw, width=1280, height=720, framerate=30/1 ! videoconvert ! video/x-raw,format=BGRA ! appsink name=outsink"); - //_stream.Start("dx9screencapsrc x=10 y=60 width=640 height=360 monitor=1 ! videoconvert ! video/x-raw,format=BGRA ! appsink name=outsink sync=false"); } public bool PreFilterMessage(ref Message m) @@ -319,7 +331,7 @@ private void HandleHeldKeys() { previousPitchRate = pitch; previousYawRate = yaw; - selectedGimbalManager?.SetRatesCommandAsync(pitch, yaw, yaw_lock, gimbalDeviceId); + selectedGimbalManager?.SetRatesCommandAsync(pitch, yaw, yaw_lock, selectedGimbalID); Console.WriteLine($"Pitch: {pitch}, Yaw: {yaw}"); } @@ -343,61 +355,109 @@ private void HandleHeldKeys() } } - private bool recording = false; - private bool lockMode = false; private void HandleKeyPress(Keys key) { if (key == preferences.TakePicture) { - Console.WriteLine("Take picture"); - selectedCamera?.TakeSinglePictureAsync(); + TakePicture(); } - else if (key == preferences.StartRecording || (key == preferences.ToggleRecording && !recording)) + else if (key == preferences.ToggleRecording) { - Console.WriteLine("Start recording"); - selectedCamera?.StartRecordingAsync(); + SetRecording(!isRecording); } - else if (key == preferences.StopRecording || (key == preferences.ToggleRecording && recording)) + else if (key == preferences.StartRecording) { - Console.WriteLine("Stop recording"); - selectedCamera?.StopRecordingAsync(); + SetRecording(true); + } + else if (key == preferences.StopRecording) + { + SetRecording(false); } else if (key == preferences.ToggleLockFollow) { - Console.WriteLine("Toggle lock/follow"); - yaw_lock = !yaw_lock; - selectedGimbalManager.SetRatesCommandAsync(previousPitchRate, previousYawRate, yaw_lock); + SetYawLock(!yaw_lock); } else if (key == preferences.SetLock) { - Console.WriteLine("Set lock"); - yaw_lock = true; - selectedGimbalManager.SetRatesCommandAsync(previousPitchRate, previousYawRate, yaw_lock); + SetYawLock(true); } else if (key == preferences.SetFollow) { - Console.WriteLine("Set follow"); - yaw_lock = false; - selectedGimbalManager.SetRatesCommandAsync(previousPitchRate, previousYawRate, yaw_lock); + SetYawLock(false); } else if (key == preferences.Retract) { - Console.WriteLine("Retract"); - selectedGimbalManager?.RetractAsync(); + Retract(); } else if (key == preferences.Neutral) { - Console.WriteLine("Neutral"); - selectedGimbalManager?.NeutralAsync(); + Neutral(); + } + else if (key == preferences.PointDown) + { + PointDown(); } else if (key == preferences.Home) { - Console.WriteLine("Home"); - var loc = MainV2.comPort?.MAV?.cs.HomeLocation; - selectedGimbalManager?.SetROILocationAsync(loc.Lat, loc.Lng, loc.Alt, frame: MAV_FRAME.GLOBAL); + Home(); + } + } + + private void TakePicture() + { + Console.WriteLine("Take picture"); + selectedCamera?.TakeSinglePictureAsync(); + } + + private void SetRecording(bool start) + { + isRecording = start; + if(start) + { + Console.WriteLine("Start recording"); + selectedCamera?.StartRecordingAsync(); + } + else + { + Console.WriteLine("Stop recording"); + selectedCamera?.StopRecordingAsync(); } } + private void SetYawLock(bool locked) + { + string message = locked ? "lock" : "follow"; + Console.WriteLine($"Set yaw {message}"); + yaw_lock = locked; + yawLockToolStripMenuItem.Checked = locked; + selectedGimbalManager?.SetRatesCommandAsync(previousPitchRate, previousYawRate, yaw_lock, selectedGimbalID); + } + + private void Retract() + { + Console.WriteLine("Retract"); + selectedGimbalManager?.RetractAsync(); + } + + private void Neutral() + { + Console.WriteLine("Neutral"); + selectedGimbalManager?.NeutralAsync(); + } + + private void PointDown() + { + Console.WriteLine("Point down"); + selectedGimbalManager?.SetAnglesCommandAsync(-90, 0, false, selectedGimbalID); + } + + private void Home() + { + Console.WriteLine("Home"); + var loc = MainV2.comPort?.MAV?.cs.HomeLocation; + selectedGimbalManager?.SetROILocationAsync(loc.Lat, loc.Lng, loc.Alt, frame: MAV_FRAME.GLOBAL); + } + private DateTime lastMouseMove = DateTime.MinValue; private void VideoBox_MouseMove(object sender, MouseEventArgs e) { @@ -471,6 +531,58 @@ private void VideoBox_Click(object sender, EventArgs e) return (2 * x / (double)imageWidth - 1, 2 * y / (double)imageHeight - 1); } + + /// + /// Update the camera controls based on the current camera capabilities + /// + private void UITimer_Tick(object sender, EventArgs e) + { + takePictureToolStripMenuItem.Enabled = selectedCamera?.CanCaptureImage ?? false; + startRecordingToolStripMenuItem.Enabled = selectedCamera?.CanCaptureVideo ?? false; + stopRecordingToolStripMenuItem.Enabled = selectedCamera?.CanCaptureVideo ?? false; + } + + private void yawLockToolStripMenuItem_Click(object sender, EventArgs e) + { + var item = sender as ToolStripMenuItem; + if (item == null) { return; } + SetYawLock(!item.Checked); + } + + private void retractToolStripMenuItem_Click(object sender, EventArgs e) + { + Retract(); + } + + private void neutralToolStripMenuItem_Click(object sender, EventArgs e) + { + Neutral(); + } + + private void pointDownToolStripMenuItem_Click(object sender, EventArgs e) + { + PointDown(); + } + + private void pointHomeToolStripMenuItem_Click(object sender, EventArgs e) + { + Home(); + } + + private void takePictureToolStripMenuItem_Click(object sender, EventArgs e) + { + TakePicture(); + } + + private void startRecordingToolStripMenuItem_Click(object sender, EventArgs e) + { + SetRecording(true); + } + + private void stopRecordingToolStripMenuItem_Click(object sender, EventArgs e) + { + SetRecording(false); + } } public class GimbalControlPreferences @@ -496,6 +608,7 @@ public class GimbalControlPreferences public Keys SetFollow { get; set; } public Keys Retract { get; set; } public Keys Neutral { get; set; } + public Keys PointDown { get; set; } public Keys Home { get; set; } @@ -544,6 +657,7 @@ public GimbalControlPreferences() SetFollow = Keys.None; Retract = Keys.None; Neutral = Keys.N; + PointDown = Keys.None; Home = Keys.H; MoveCameraToMouseLocation = MouseButton.Left; From 48240d693cf8ec4370d74405dd8073a4e5f45d9a Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sun, 15 Sep 2024 12:57:09 +1000 Subject: [PATCH 22/45] GimbalManagerProtocol: periodically discover --- ExtLibs/ArduPilot/CurrentState.cs | 1 + .../Mavlink/GimbalManagerProtocol.cs | 56 ++++++++++--------- 2 files changed, 32 insertions(+), 25 deletions(-) diff --git a/ExtLibs/ArduPilot/CurrentState.cs b/ExtLibs/ArduPilot/CurrentState.cs index e407b37465..f2d7d61f31 100644 --- a/ExtLibs/ArduPilot/CurrentState.cs +++ b/ExtLibs/ArduPilot/CurrentState.cs @@ -4460,6 +4460,7 @@ public void UpdateCurrentSettings(Action bs, bool updatenow, MAV.sysid, MAV.compid); // request rc info MAV.Camera?.RequestMessageIntervals(MAV.cs.ratestatus); // use ratestatus until we create a new setting for this + MAV.GimbalManager?.Discover(); } catch { diff --git a/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs b/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs index 98ab6e094d..2e68b79d0d 100644 --- a/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs +++ b/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs @@ -1,5 +1,4 @@ using System.Collections.Concurrent; -using System.Collections.Generic; using System.Threading.Tasks; namespace MissionPlanner.ArduPilot.Mavlink @@ -31,45 +30,52 @@ public GimbalManagerProtocol(MAVLinkInterface mavint) this.mavint = mavint; } + private bool first_discover = true; public void Discover() { + if (first_discover) + { + first_discover = false; + mavint.OnPacketReceived += MessagesHandler; + } + mavint.doCommand(0, 0, MAVLink.MAV_CMD.REQUEST_MESSAGE, (float)MAVLink.MAVLINK_MSG_ID.GIMBAL_MANAGER_INFORMATION, 0, 0, 0, 0, 0, 0, false); + } - mavint.OnPacketReceived += (sender, message) => + private void MessagesHandler(object sender, MAVLink.MAVLinkMessage message) + { + if (message.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GIMBAL_MANAGER_INFORMATION) { - if (message.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GIMBAL_MANAGER_INFORMATION) - { - var gmi = (MAVLink.mavlink_gimbal_manager_information_t)message.data; + var gmi = (MAVLink.mavlink_gimbal_manager_information_t)message.data; - ManagerInfo[gmi.gimbal_device_id] = gmi; - if (!ManagerInfo.ContainsKey(0) || gmi.gimbal_device_id <= ManagerInfo[0].gimbal_device_id) - { - ManagerInfo[0] = gmi; - } + ManagerInfo[gmi.gimbal_device_id] = gmi; + if (!ManagerInfo.ContainsKey(0) || gmi.gimbal_device_id <= ManagerInfo[0].gimbal_device_id) + { + ManagerInfo[0] = gmi; } + } - if (message.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GIMBAL_MANAGER_STATUS) + if (message.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GIMBAL_MANAGER_STATUS) + { + var gms = (MAVLink.mavlink_gimbal_manager_status_t)message.data; + ManagerStatus[gms.gimbal_device_id] = gms; + if (!ManagerStatus.ContainsKey(0) || gms.gimbal_device_id <= ManagerStatus[0].gimbal_device_id) { - var gms = (MAVLink.mavlink_gimbal_manager_status_t)message.data; - ManagerStatus[gms.gimbal_device_id] = gms; - if (!ManagerStatus.ContainsKey(0) || gms.gimbal_device_id <= ManagerStatus[0].gimbal_device_id) - { - ManagerStatus[0] = gms; - } + ManagerStatus[0] = gms; } + } - if (message.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GIMBAL_DEVICE_ATTITUDE_STATUS) + if (message.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GIMBAL_DEVICE_ATTITUDE_STATUS) + { + var gds = (MAVLink.mavlink_gimbal_device_attitude_status_t)message.data; + GimbalStatus[gds.gimbal_device_id] = gds; + if (!GimbalStatus.ContainsKey(0) || gds.gimbal_device_id <= GimbalStatus[0].gimbal_device_id) { - var gds = (MAVLink.mavlink_gimbal_device_attitude_status_t)message.data; - GimbalStatus[gds.gimbal_device_id] = gds; - if (!GimbalStatus.ContainsKey(0) || gds.gimbal_device_id <= GimbalStatus[0].gimbal_device_id) - { - GimbalStatus[0] = gds; - } + GimbalStatus[0] = gds; } - }; + } } public bool HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS flags, byte gimbal_device_id = 0) From 8132a872ac49c78ef4f2bbd3fc62354a418b5e61 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sun, 15 Sep 2024 16:28:22 +1000 Subject: [PATCH 23/45] Quaternion: fix earth_to_body Previously, this function did nothing. It was also trying to do the opposite of what it claimed to do. --- ExtLibs/Utilities/Quaternion.cs | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/ExtLibs/Utilities/Quaternion.cs b/ExtLibs/Utilities/Quaternion.cs index 09dc98c2e5..84a5ce8c3f 100644 --- a/ExtLibs/Utilities/Quaternion.cs +++ b/ExtLibs/Utilities/Quaternion.cs @@ -152,12 +152,18 @@ private double sqrtf(double v) return Math.Sqrt(v); } - // convert a vector from earth to body frame - public void earth_to_body(Vector3f v) + // convert a vector from body to earth frame + public Vector3f body_to_earth(Vector3f v) { Matrix3 m = new Matrix3(); rotation_matrix(m); - v = m * v; + return m * v; + } + + // convert a vector from earth to body frame + public Vector3f earth_to_body(Vector3f v) + { + return inverse().body_to_earth(v); } // create a quaternion from Euler angles From d716cf824ad931526066e69eb67f8f775f48b3cd Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sun, 15 Sep 2024 16:28:41 +1000 Subject: [PATCH 24/45] CameraProtocol: add CalculateImagePointVector --- ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs | 36 ++++++++++++++++++--- 1 file changed, 32 insertions(+), 4 deletions(-) diff --git a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs index a9261f8bcb..60c097685c 100644 --- a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs +++ b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs @@ -126,7 +126,7 @@ private async Task RequestCameraInformationAsync() { try { - if(parent?.parent != null) + if (parent?.parent != null) { // New-style request var resp = await parent.parent.doCommandAsync( @@ -202,7 +202,7 @@ public void ParseMessages(object sender, MAVLink.MAVLinkMessage message) /// Message frequency in messages per second. public void RequestMessageIntervals(int ratehz) { - if(parent?.parent == null) + if (parent?.parent == null) { return; } @@ -361,10 +361,15 @@ public Task SetZoomAsync(float zoom_level, MAVLink.CAMERA_ZOOM_TYPE zoom_type = ); } + /// + /// Calculate the lat/lon/alt-msl of a point in the image, given its x/y position in the image. + /// x position in the image, -1 to 1 (positive right) + /// y position in the image, -1 to 1 (positive down) + /// PointLatLngAlt with the calculated position, or null if the calculation failed public PointLatLngAlt CalculateImagePointLocation(double x, double y) { var imagePosition = new PointLatLngAlt(CameraFOVStatus.lat_image * 1e-7, CameraFOVStatus.lon_image * 1e-7, CameraFOVStatus.alt_image * 1e-3); - if(x == 0 && y == 0) + if (x == 0 && y == 0) { return imagePosition; } @@ -372,7 +377,7 @@ public PointLatLngAlt CalculateImagePointLocation(double x, double y) var camPosition = new PointLatLngAlt(CameraFOVStatus.lat_camera * 1e-7, CameraFOVStatus.lon_camera * 1e-7, CameraFOVStatus.alt_camera * 1e-3); var height = camPosition.Alt - imagePosition.Alt; - if(height < 0) + if (height < 0) { return null; } @@ -392,5 +397,28 @@ public PointLatLngAlt CalculateImagePointLocation(double x, double y) pos.Alt = imagePosition.Alt; return pos; } + + /// + /// Calculate the 3D unit vector of a pixel in the world frame, given its x/y position in the image. + /// + /// x position in the image, -1 to 1 (positive right) + /// y position in the image, -1 to 1 (positive down) + /// + public Vector3 CalculateImagePointVector(double x, double y) + { + var vector = new Vector3(1, 0, 0); // Body-frame vector pointing straight ahead + if (CameraFOVStatus.hfov != float.NaN && CameraFOVStatus.vfov != float.NaN && x != 0 && y != 0) + { + var hfov = CameraFOVStatus.hfov * Math.PI / 180; + var vfov = CameraFOVStatus.vfov * Math.PI / 180; + + vector.y = Math.Tan(x * hfov / 2); // x in the image is toward the right side of the plane (positive y in body frame) + vector.z = Math.Tan(y * vfov / 2); // y in the image is down (negative z in body frame) + vector.normalize(); + } + + var q = new Quaternion(CameraFOVStatus.q[0], CameraFOVStatus.q[1], CameraFOVStatus.q[2], CameraFOVStatus.q[3]); + return q.body_to_earth(vector); + } } } From d440f16261751544e4bd784e617461c17a86d1bc Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sun, 22 Sep 2024 14:13:57 +1000 Subject: [PATCH 25/45] Quaternion.cs: overhaul and document This library was originally copy-pasted from ArduPilot and ported to C#, but many mistakes were made in the process. For example, many of the methods in the C++ implementation take a reference variable as input, and the C# code was taking a regular input and outputting nothing. This commit overhauls the whole library, making it more C#-like and documents all methods. --- ExtLibs/Utilities/Quaternion.cs | 592 +++++++++++++++----------------- Swarm/Formation.cs | 3 +- 2 files changed, 278 insertions(+), 317 deletions(-) diff --git a/ExtLibs/Utilities/Quaternion.cs b/ExtLibs/Utilities/Quaternion.cs index 84a5ce8c3f..a8b9eac9d5 100644 --- a/ExtLibs/Utilities/Quaternion.cs +++ b/ExtLibs/Utilities/Quaternion.cs @@ -8,94 +8,37 @@ namespace MissionPlanner.Utilities { public class Quaternion { - private const double M_PI = Math.PI; - private const double M_2PI = (M_PI * 2); - public double q1, q2, q3, q4; - // constructor creates a quaternion equivalent - // to roll=0, pitch=0, yaw=0 + /// + /// Create a quaternion that represents no rotation + /// public Quaternion() { q1 = 1; q2 = q3 = q4 = 0; } - // setting constructor - public Quaternion(double _q1, double _q2, double _q3, double _q4) - { - q1 = _q1; - q2 = _q2; - q3 = _q3; - q4 = _q4; - } - - - // check if any elements are NAN - bool is_nan() - { - return isnan(q1) || isnan(q2) || isnan(q3) || isnan(q4); - } - - private bool isnan(double q1) + /// + /// Create a quaternion with the specified values. The values are not required to be normalized, + /// but no normalization will happen automatically + /// + public Quaternion(double q1, double q2, double q3, double q4) { - return double.IsNaN(q1); - } - - // return the rotation matrix equivalent for this quaternion - public void rotation_matrix(Matrix3 m) - { - double q3q3 = q3 * q3; - double q3q4 = q3 * q4; - double q2q2 = q2 * q2; - double q2q3 = q2 * q3; - double q2q4 = q2 * q4; - double q1q2 = q1 * q2; - double q1q3 = q1 * q3; - double q1q4 = q1 * q4; - double q4q4 = q4 * q4; - - m.a.x = 1.0f - 2.0f * (q3q3 + q4q4); - m.a.y = 2.0f * (q2q3 - q1q4); - m.a.z = 2.0f * (q2q4 + q1q3); - m.b.x = 2.0f * (q2q3 + q1q4); - m.b.y = 1.0f - 2.0f * (q2q2 + q4q4); - m.b.z = 2.0f * (q3q4 - q1q2); - m.c.x = 2.0f * (q2q4 - q1q3); - m.c.y = 2.0f * (q3q4 + q1q2); - m.c.z = 1.0f - 2.0f * (q2q2 + q3q3); + this.q1 = q1; + this.q2 = q2; + this.q3 = q3; + this.q4 = q4; } - // return the rotation matrix equivalent for this quaternion after normalization - public void rotation_matrix_norm(Matrix3 m) - { - double q1q1 = q1 * q1; - double q1q2 = q1 * q2; - double q1q3 = q1 * q3; - double q1q4 = q1 * q4; - double q2q2 = q2 * q2; - double q2q3 = q2 * q3; - double q2q4 = q2 * q4; - double q3q3 = q3 * q3; - double q3q4 = q3 * q4; - double q4q4 = q4 * q4; - double invs = 1.0f / (q1q1 + q2q2 + q3q3 + q4q4); - - m.a.x = (q2q2 - q3q3 - q4q4 + q1q1) * invs; - m.a.y = 2.0f * (q2q3 - q1q4) * invs; - m.a.z = 2.0f * (q2q4 + q1q3) * invs; - m.b.x = 2.0f * (q2q3 + q1q4) * invs; - m.b.y = (-q2q2 + q3q3 - q4q4 + q1q1) * invs; - m.b.z = 2.0f * (q3q4 - q1q2) * invs; - m.c.x = 2.0f * (q2q4 - q1q3) * invs; - m.c.y = 2.0f * (q3q4 + q1q2) * invs; - m.c.z = (-q2q2 - q3q3 + q4q4 + q1q1) * invs; - } - - // return the rotation matrix equivalent for this quaternion - // Thanks to Martin John Baker - // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm - public void from_rotation_matrix(Matrix3 m) + /// + /// Create a quaternion equivalent to the supplied matrix + /// Thanks to Martin John Baker + /// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm + /// + /// Rotation matrix. Must be a valid rotation matrix (orthogonal and with determinant 1) + /// + public static Quaternion from_rotation_matrix(Matrix3 m) { double m00 = m.a.x; double m11 = m.b.y; @@ -106,306 +49,349 @@ public void from_rotation_matrix(Matrix3 m) double m02 = m.a.z; double m21 = m.c.y; double m12 = m.b.z; - ref double qw = ref q1; - ref double qx = ref q2; - ref double qy = ref q3; - ref double qz = ref q4; double tr = m00 + m11 + m22; if (tr > 0) { - double S = sqrtf(tr + 1) * 2; - qw = 0.25f * S; - qx = (m21 - m12) / S; - qy = (m02 - m20) / S; - qz = (m10 - m01) / S; + double S = Math.Sqrt(tr + 1) * 2; + return new Quaternion( + 0.25 * S, + (m21 - m12) / S, + (m02 - m20) / S, + (m10 - m01) / S + ); } else if ((m00 > m11) && (m00 > m22)) { - double S = sqrtf(1.0f + m00 - m11 - m22) * 2.0f; - qw = (m21 - m12) / S; - qx = 0.25f * S; - qy = (m01 + m10) / S; - qz = (m02 + m20) / S; + double S = Math.Sqrt(1.0 + m00 - m11 - m22) * 2.0; + return new Quaternion( + (m21 - m12) / S, + 0.25 * S, + (m01 + m10) / S, + (m02 + m20) / S + ); } else if (m11 > m22) { - double S = sqrtf(1.0f + m11 - m00 - m22) * 2.0f; - qw = (m02 - m20) / S; - qx = (m01 + m10) / S; - qy = 0.25f * S; - qz = (m12 + m21) / S; + double S = Math.Sqrt(1.0 + m11 - m00 - m22) * 2.0; + return new Quaternion( + (m02 - m20) / S, + (m01 + m10) / S, + 0.25 * S, + (m12 + m21) / S + ); } else { - double S = sqrtf(1.0f + m22 - m00 - m11) * 2.0f; - qw = (m10 - m01) / S; - qx = (m02 + m20) / S; - qy = (m12 + m21) / S; - qz = 0.25f * S; + double S = Math.Sqrt(1.0 + m22 - m00 - m11) * 2.0; + return new Quaternion( + (m10 - m01) / S, + (m02 + m20) / S, + (m12 + m21) / S, + 0.25 * S + ); } } - private double sqrtf(double v) - { - return Math.Sqrt(v); - } - - // convert a vector from body to earth frame - public Vector3f body_to_earth(Vector3f v) + /// + /// create a quaternion from Euler angles + /// + /// Roll angle in radians + /// Pitch angle in radians + /// Yaw angle in radians + /// + public static Quaternion from_euler(double roll, double pitch, double yaw) + { + double cr2 = Math.Cos(roll * 0.5); + double cp2 = Math.Cos(pitch * 0.5); + double cy2 = Math.Cos(yaw * 0.5); + double sr2 = Math.Sin(roll * 0.5); + double sp2 = Math.Sin(pitch * 0.5); + double sy2 = Math.Sin(yaw * 0.5); + + return new Quaternion( + cr2 * cp2 * cy2 + sr2 * sp2 * sy2, + sr2 * cp2 * cy2 - cr2 * sp2 * sy2, + cr2 * sp2 * cy2 + sr2 * cp2 * sy2, + cr2 * cp2 * sy2 - sr2 * sp2 * cy2 + ); + } + + /// + /// Create a quaternion from Euler angles in 3-1-2 order (common joint order for gimbals) + /// + /// Roll angle in radians + /// Pitch angle in radians + /// Yaw angle in radians + /// + public static Quaternion from_euler312(double roll, double pitch, double yaw) { Matrix3 m = new Matrix3(); - rotation_matrix(m); - return m * v; - } - - // convert a vector from earth to body frame - public Vector3f earth_to_body(Vector3f v) - { - return inverse().body_to_earth(v); - } + m.from_euler312(roll, pitch, yaw); - // create a quaternion from Euler angles - public void from_euler(double roll, double pitch, double yaw) - { - double cr2 = cosf(roll * 0.5f); - double cp2 = cosf(pitch * 0.5f); - double cy2 = cosf(yaw * 0.5f); - double sr2 = sinf(roll * 0.5f); - double sp2 = sinf(pitch * 0.5f); - double sy2 = sinf(yaw * 0.5f); - - q1 = cr2 * cp2 * cy2 + sr2 * sp2 * sy2; - q2 = sr2 * cp2 * cy2 - cr2 * sp2 * sy2; - q3 = cr2 * sp2 * cy2 + sr2 * cp2 * sy2; - q4 = cr2 * cp2 * sy2 - sr2 * sp2 * cy2; + return from_rotation_matrix(m); } - private double sinf(double v) + /// + /// Create a quaternion that represents a rotation about a vector axis (right-hand rule) + /// + /// Axis about which to rotate. This must be a unit vector, or the resulting quaternion will be wrong. + /// Angle in radians + /// + public static Quaternion from_axis_angle(Vector3 axis, double theta) { - return Math.Sin(v); + if (theta == 0) + { + return new Quaternion(); + } + + double st2 = Math.Sin(theta / 2.0); + + return new Quaternion( + Math.Cos(theta / 2.0), + axis.x * st2, + axis.y * st2, + axis.z * st2 + ); } - private double cosf(double v) + /// + /// Create a quaternion that represents a rotation about a vector axis (right-hand rule) + /// + /// Axis about which to rotate, with a length that represents the angle in radians + /// + public static Quaternion from_axis_angle(Vector3 v) { - return Math.Cos(v); + double theta = v.length(); + if (theta == 0) + { + return new Quaternion(); + } + + return from_axis_angle(v / theta, theta); } - // create a quaternion from Euler angles - public void from_vector312(double roll, double pitch, double yaw) + /// + /// Check if any element of the quaternion is NaN + /// + /// + public bool is_nan() { - Matrix3 m = new Matrix3(); - m.from_euler312(roll, pitch, yaw); - - from_rotation_matrix(m); + return double.IsNaN(q1) || double.IsNaN(q2) || double.IsNaN(q3) || double.IsNaN(q4); } - public void from_axis_angle(Vector3f v) + /// + /// Return the equivalent rotation matrix for this quaternion + /// + /// + public Matrix3 rotation_matrix() { - double theta = v.length(); - if (is_zero(theta)) - { - q1 = 1.0f; - q2 = q3 = q4 = 0.0f; - return; - } + double q3q3 = q3 * q3; + double q3q4 = q3 * q4; + double q2q2 = q2 * q2; + double q2q3 = q2 * q3; + double q2q4 = q2 * q4; + double q1q2 = q1 * q2; + double q1q3 = q1 * q3; + double q1q4 = q1 * q4; + double q4q4 = q4 * q4; + + var m = new Matrix3(); + m.a.x = 1.0 - 2.0 * (q3q3 + q4q4); + m.a.y = 2.0 * (q2q3 - q1q4); + m.a.z = 2.0 * (q2q4 + q1q3); + m.b.x = 2.0 * (q2q3 + q1q4); + m.b.y = 1.0 - 2.0 * (q2q2 + q4q4); + m.b.z = 2.0 * (q3q4 - q1q2); + m.c.x = 2.0 * (q2q4 - q1q3); + m.c.y = 2.0 * (q3q4 + q1q2); + m.c.z = 1.0 - 2.0 * (q2q2 + q3q3); - v /= theta; - from_axis_angle(v, theta); + return m; } - public void from_axis_angle(Vector3f axis, double theta) + /// + /// Return the equivalent rotation matrix for a normalized version of this quaternion. + /// (this does not modify this quaternion) + /// + /// + public Matrix3 rotation_matrix_norm() { - // axis must be a unit vector as there is no check for length - if (is_zero(theta)) - { - q1 = 1.0f; - q2 = q3 = q4 = 0.0f; - return; - } + double q1q1 = q1 * q1; + double q1q2 = q1 * q2; + double q1q3 = q1 * q3; + double q1q4 = q1 * q4; + double q2q2 = q2 * q2; + double q2q3 = q2 * q3; + double q2q4 = q2 * q4; + double q3q3 = q3 * q3; + double q3q4 = q3 * q4; + double q4q4 = q4 * q4; + double invs = 1.0 / (q1q1 + q2q2 + q3q3 + q4q4); - double st2 = sinf(theta / 2.0f); + var m = new Matrix3(); + m.a.x = (q2q2 - q3q3 - q4q4 + q1q1) * invs; + m.a.y = 2.0 * (q2q3 - q1q4) * invs; + m.a.z = 2.0 * (q2q4 + q1q3) * invs; + m.b.x = 2.0 * (q2q3 + q1q4) * invs; + m.b.y = (-q2q2 + q3q3 - q4q4 + q1q1) * invs; + m.b.z = 2.0 * (q3q4 - q1q2) * invs; + m.c.x = 2.0 * (q2q4 - q1q3) * invs; + m.c.y = 2.0 * (q3q4 + q1q2) * invs; + m.c.z = (-q2q2 - q3q3 + q4q4 + q1q1) * invs; - q1 = cosf(theta / 2.0f); - q2 = axis.x * st2; - q3 = axis.y * st2; - q4 = axis.z * st2; + return m; } - private bool is_zero(double theta) + /// + /// Convert a vector from body to earth frame + /// (assuming this is a normalized quaternion that represents the rotation from earth to body) + /// + /// + /// + public Vector3 body_to_earth(Vector3 v) { - return theta == 0; + return rotation_matrix() * v; } - public Quaternion rotate(Vector3f v) + /// + /// Convert a vector from earth to body frame + /// (assuming this is a normalized quaternion that represents the rotation from earth to body) + /// + /// + /// + public Vector3 earth_to_body(Vector3 v) { - Quaternion r = new Quaternion(); - r.from_axis_angle(v); - return r; + return conjugate().body_to_earth(v); } - public void to_axis_angle(Vector3f v) + /// + /// Convert this quaternion to an axis-angle representation + /// + /// Vector of axis with a length of rotation angle in radians + public Vector3 to_axis_angle() { - double l = sqrt(sq(q2) + sq(q3) + sq(q4)); - v = new Vector3(q2, q3, q4); - if (!is_zero(l)) + var v = new Vector3(q2, q3, q4); + var l = v.length(); + if (l != 0) { - v /= l; - v *= wrap_PI(2.0f * atan2f(l, q1)); + v /= l; // normalize + v *= wrap_PI(2.0 * Math.Atan2(l, q1)); } + return v; } + /// + /// Wrap an angle to the range -PI to PI + /// + /// + /// double wrap_PI(double radian) { var res = wrap_2PI(radian); - if (res > M_PI) + if (res > Math.PI) { - res -= M_2PI; + res -= (Math.PI * 2); } return res; } + /// + /// Wrap an angle to the range 0 to 2PI + /// + /// + /// double wrap_2PI(double radian) { - var res = radian % M_2PI; + var res = radian % (Math.PI * 2); if (res < 0) { - res += M_2PI; + res += (Math.PI * 2); } return res; } - - private double atan2f(double l, double q1) + + /// + /// Square a number + /// + /// + /// + private double sq(double n) { - return Math.Atan2(l, q1); + return n * n; } - private double sqrt(double v) - { - return Math.Sqrt(v); - } - - private double sq(double q2) - { - return q2 * q2; - } - - public void from_axis_angle_fast(Vector3f v) - { - double theta = v.length(); - if (is_zero(theta)) - { - q1 = 1.0f; - q2 = q3 = q4 = 0.0f; - return; - } - - v /= theta; - from_axis_angle_fast(v, theta); - } - - public void from_axis_angle_fast(Vector3f axis, double theta) - { - double t2 = theta / 2.0f; - double sqt2 = sq(t2); - double st2 = t2 - sqt2 * t2 / 6.0f; - - q1 = 1.0f - (sqt2 / 2.0f) + sq(sqt2) / 24.0f; - q2 = axis.x * st2; - q3 = axis.y * st2; - q4 = axis.z * st2; - } - - public void rotate_fast(Vector3f v) - { - double theta = v.length(); - if (is_zero(theta)) - { - return; - } - - double t2 = theta / 2.0f; - double sqt2 = sq(t2); - double st2 = t2 - sqt2 * t2 / 6.0f; - st2 /= theta; - - //"rotation quaternion" - double w2 = 1.0f - (sqt2 / 2.0f) + sq(sqt2) / 24.0f; - double x2 = v.x * st2; - double y2 = v.y * st2; - double z2 = v.z * st2; - - //copy our quaternion - double w1 = q1; - double x1 = q2; - double y1 = q3; - double z1 = q4; - - //do the multiply into our quaternion - q1 = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2; - q2 = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2; - q3 = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2; - q4 = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2; - } - - // get euler roll angle + /// + /// Get euler roll angle in radians + /// + /// public double get_euler_roll() { - return (atan2f(2.0f * (q1 * q2 + q3 * q4), 1.0f - 2.0f * (q2 * q2 + q3 * q3))); + return (Math.Atan2(2.0 * (q1 * q2 + q3 * q4), 1.0 - 2.0 * (q2 * q2 + q3 * q3))); } - // get euler pitch angle + /// + /// Get euler pitch angle in radians + /// + /// public double get_euler_pitch() { - return safe_asin(2.0f * (q1 * q3 - q4 * q2)); - } - - private double safe_asin(double v) - { - return Math.Asin(v); + return Math.Asin(2.0 * (q1 * q3 - q4 * q2)); } - // get euler yaw angle + /// + /// Get euler yaw angle in radians + /// + /// public double get_euler_yaw() { - return atan2f(2.0f * (q1 * q4 + q2 * q3), 1.0f - 2.0f * (q3 * q3 + q4 * q4)); + return Math.Atan2(2.0 * (q1 * q4 + q2 * q3), 1.0 - 2.0 * (q3 * q3 + q4 * q4)); } - // create eulers from a quaternion - public void to_euler(ref double roll,ref double pitch,ref double yaw) + /// + /// Output euler angles for this quaternion in radians + /// + /// + /// + /// + public void to_euler(out double roll, out double pitch, out double yaw) { roll = get_euler_roll(); pitch = get_euler_pitch(); yaw = get_euler_yaw(); } - // create eulers from a quaternion - public Vector3f to_vector312() - { - Matrix3 m = new Matrix3(); - rotation_matrix(m); - return m.to_euler312(); - } - + /// + /// Get the magnitude of this quaternion + /// + /// public double length() { - return sqrtf(sq(q1) + sq(q2) + sq(q3) + sq(q4)); + return Math.Sqrt(sq(q1) + sq(q2) + sq(q3) + sq(q4)); } - public Quaternion inverse() + /// + /// Get the conjugate of this quaternion + /// (for unit quaternions, this is the same as the inverse) + /// + /// + public Quaternion conjugate() { return new Quaternion(q1, -q2, -q3, -q4); } + /// + /// Normalize this quaternion + /// public void normalize() { double quatMag = length(); - if (!is_zero(quatMag)) + if (quatMag != 0) { - double quatMagInv = 1.0f / quatMag; + double quatMagInv = 1.0 / quatMag; q1 *= quatMagInv; q2 *= quatMagInv; q3 *= quatMagInv; @@ -413,45 +399,21 @@ public void normalize() } } - public static Quaternion operator *(Quaternion self, Quaternion v) - { - Quaternion ret = new Quaternion(); - double w1 = self.q1; - double x1 = self.q2; - double y1 = self.q3; - double z1 = self.q4; - - double w2 = v.q1; - double x2 = v.q2; - double y2 = v.q3; - double z2 = v.q4; - - ret.q1 = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2; - ret.q2 = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2; - ret.q3 = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2; - ret.q4 = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2; - - return ret; - } - - public static Quaternion operator /(Quaternion self, Quaternion v) + /// + /// Quaternion multiplication w * v + /// (for rotations, this is equivalent to applying v and then w) + /// + /// + /// + /// + public static Quaternion operator *(Quaternion w, Quaternion v) { - Quaternion ret = new Quaternion(); - double quat0 = self.q1; - double quat1 = self.q2; - double quat2 = self.q3; - double quat3 = self.q4; - - double rquat0 = v.q1; - double rquat1 = v.q2; - double rquat2 = v.q3; - double rquat3 = v.q4; - - ret.q1 = (rquat0 * quat0 + rquat1 * quat1 + rquat2 * quat2 + rquat3 * quat3); - ret.q2 = (rquat0 * quat1 - rquat1 * quat0 - rquat2 * quat3 + rquat3 * quat2); - ret.q3 = (rquat0 * quat2 + rquat1 * quat3 - rquat2 * quat0 - rquat3 * quat1); - ret.q4 = (rquat0 * quat3 - rquat1 * quat2 + rquat2 * quat1 - rquat3 * quat0); - return ret; + return new Quaternion( + w.q1 * v.q1 - w.q2 * v.q2 - w.q3 * v.q3 - w.q4 * v.q4, + w.q1 * v.q2 + w.q2 * v.q1 + w.q3 * v.q4 - w.q4 * v.q3, + w.q1 * v.q3 - w.q2 * v.q4 + w.q3 * v.q1 + w.q4 * v.q2, + w.q1 * v.q4 + w.q2 * v.q3 - w.q3 * v.q2 + w.q4 * v.q1 + ); } } -} \ No newline at end of file +} diff --git a/Swarm/Formation.cs b/Swarm/Formation.cs index 2628291323..1a183ff4d3 100644 --- a/Swarm/Formation.cs +++ b/Swarm/Formation.cs @@ -269,8 +269,7 @@ public override void SendCommand() att_target.thrust = (float)MathHelper.constrain(thrustp.get_pid(), 0.1, 1); } - Quaternion q = new Quaternion(); - q.from_vector312(newroll * MathHelper.deg2rad, newpitch * MathHelper.deg2rad, yawerror * MathHelper.deg2rad); + Quaternion q = Quaternion.from_euler312(newroll * MathHelper.deg2rad, newpitch * MathHelper.deg2rad, yawerror * MathHelper.deg2rad); att_target.q = new float[4]; att_target.q[0] = (float)q.q1; From 85337a05a0a52f7d52b14a5782c953a682530a97 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sun, 22 Sep 2024 22:27:19 +1000 Subject: [PATCH 26/45] GimbalManagerProtocol: add Get/Set attitude quat --- .../Mavlink/GimbalManagerProtocol.cs | 97 +++++++++++++++++-- 1 file changed, 90 insertions(+), 7 deletions(-) diff --git a/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs b/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs index 2e68b79d0d..c48193bd2c 100644 --- a/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs +++ b/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs @@ -1,10 +1,14 @@ +using System; using System.Collections.Concurrent; using System.Threading.Tasks; +using MissionPlanner.Utilities; namespace MissionPlanner.ArduPilot.Mavlink { public class GimbalManagerProtocol { + CurrentState cs; + // Stores the last GIMBAL_MANAGER_INFORMATION message for each gimbal device/component ID. // This index will be 1-6, or MAVLink component IDs 154, 171-175. // Index 0 is used to store the message of the first (lowest) gimbal ID. @@ -25,9 +29,10 @@ public class GimbalManagerProtocol private readonly MAVLinkInterface mavint; - public GimbalManagerProtocol(MAVLinkInterface mavint) + public GimbalManagerProtocol(MAVLinkInterface mavint, CurrentState cs) { this.mavint = mavint; + this.cs = cs; } private bool first_discover = true; @@ -88,6 +93,49 @@ public bool HasAllCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS flags, byte gimbal return ManagerInfo.TryGetValue(gimbal_device_id, out var info) && ((info.cap_flags & (uint)flags) == (uint)flags); } + public bool HasStatusFlag(MAVLink.GIMBAL_DEVICE_FLAGS flags, byte gimbal_device_id = 0) + { + return ManagerStatus.TryGetValue(gimbal_device_id, out var status) && ((status.flags & (uint)flags) != 0); + } + + public bool YawInVehicleFrame(byte gimbal_device_id = 0) + { + bool yaw_in_earth_frame = HasStatusFlag(MAVLink.GIMBAL_DEVICE_FLAGS.YAW_IN_EARTH_FRAME, gimbal_device_id); + bool yaw_in_vehicle_frame = HasStatusFlag(MAVLink.GIMBAL_DEVICE_FLAGS.YAW_IN_VEHICLE_FRAME, gimbal_device_id); + + // Some older protocols don't set YAW_IN_EARTH_FRAME or YAW_IN_VEHICLE_FRAME flags, + // with those, we have to infer it from whether YAW_LOCK is set. + if (!yaw_in_earth_frame && !yaw_in_vehicle_frame) + { + bool yaw_lock = HasStatusFlag(MAVLink.GIMBAL_DEVICE_FLAGS.YAW_LOCK, gimbal_device_id); + yaw_in_vehicle_frame = !yaw_lock; + } + + return yaw_in_vehicle_frame; + } + + /// + /// Get the reported attitude of the gimbal. Yaw always reported relative to the earth frame. + /// + /// Device ID of the gimbal. 0 means all gimbals + /// + public Quaternion GetAttitude(byte gimbal_device_id = 0) + { + if (!GimbalStatus.TryGetValue(gimbal_device_id, out var status)) + { + return null; + } + + var q = new Quaternion(status.q[0], status.q[1], status.q[2], status.q[3]); + + if (YawInVehicleFrame(gimbal_device_id)) + { + q = Quaternion.from_euler(0, 0, cs.yaw * MathHelper.deg2rad) * q; + } + + return q; + } + public Task RetractAsync(byte gimbal_device_id = 0) { if (!HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_RETRACT)) @@ -147,13 +195,48 @@ public Task SetRCYawLockAsync(bool yaw_lock, byte gimbal_device_id = 0) gimbal_device_id); } - public Task SetAnglesCommandAsync(float pitch, float yaw, bool yaw_in_earth_frame, byte gimbal_device_id = 0) + /// + /// Set the attitude of the gimbal with a quaternion. Yaw always reported relative to the earth frame. + /// + /// Gimbal attitude quaternion + /// True if the gimbal should continue to point in this orientation. False if it should follow the yaw of the vehicle. + /// Device ID of the gimbal. 0 means all gimbals + /// + public Task SetAttitudeAsync(Quaternion q, bool yaw_lock, byte gimbal_device_id = 0) + { + var pitch = q.get_euler_pitch() * MathHelper.rad2deg; + var yaw = q.get_euler_yaw() * MathHelper.rad2deg; + + if (!yaw_lock) + { + yaw -= cs.yaw; + } + + Console.WriteLine("SetAttitudeAsync: pitch={0}, yaw={1}, yaw_lock={2}", pitch, yaw < 0 ? yaw + 360 : yaw, yaw_lock); + return SetAnglesCommandAsync(pitch, yaw, yaw_lock, gimbal_device_id); + //return Task.FromResult(true); + } + + private double wrap_180(double angle) + { + while (angle > 180) + { + angle -= 360; + } + while (angle < -180) + { + angle += 360; + } + return angle; + } + + public Task SetAnglesCommandAsync(double pitch, double yaw, bool yaw_lock, byte gimbal_device_id = 0) { if (!HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.CAN_POINT_LOCATION_LOCAL) || (pitch != 0 && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_PITCH_AXIS)) || (yaw != 0 && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_YAW_AXIS)) || - (yaw != 0 && yaw_in_earth_frame && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_YAW_LOCK)) || - (yaw != 0 && !yaw_in_earth_frame && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_YAW_FOLLOW))) + (yaw != 0 && yaw_lock && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_YAW_LOCK)) || + (yaw != 0 && !yaw_lock && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_YAW_FOLLOW))) { return Task.FromResult(false); } @@ -162,11 +245,11 @@ public Task SetAnglesCommandAsync(float pitch, float yaw, bool yaw_in_eart (byte)mavint.sysidcurrent, (byte)mavint.compidcurrent, MAVLink.MAV_CMD.DO_GIMBAL_MANAGER_PITCHYAW, - pitch, - yaw, + (float)wrap_180(pitch), + (float)wrap_180(yaw), float.NaN, // pitch rate float.NaN, // yaw rate - yaw_in_earth_frame ? (float)MAVLink.GIMBAL_MANAGER_FLAGS.YAW_LOCK : 0, // flags + yaw_lock ? (float)MAVLink.GIMBAL_MANAGER_FLAGS.YAW_LOCK : 0, // flags 0, // unused gimbal_device_id); } From b49fec968a4c0825fc02362698fa244c20c9043a Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sun, 22 Sep 2024 22:28:13 +1000 Subject: [PATCH 27/45] CameraProtocol: add pixel to rotation calculation --- ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs | 56 ++++++++++++++++++--- 1 file changed, 50 insertions(+), 6 deletions(-) diff --git a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs index 60c097685c..cbef3e0cf7 100644 --- a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs +++ b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs @@ -398,27 +398,71 @@ public PointLatLngAlt CalculateImagePointLocation(double x, double y) return pos; } + /// - /// Calculate the 3D unit vector of a pixel in the world frame, given its x/y position in the image. + /// Calculate the 3D unit vector of a pixel in the camera frame, given its x/y position in the image. /// /// x position in the image, -1 to 1 (positive right) /// y position in the image, -1 to 1 (positive down) /// - public Vector3 CalculateImagePointVector(double x, double y) + private Vector3 CalculateImagePointVectorCameraFrame(double x, double y) { - var vector = new Vector3(1, 0, 0); // Body-frame vector pointing straight ahead + var vector = new Vector3(1, 0, 0); // Camera-frame vector pointing straight ahead if (CameraFOVStatus.hfov != float.NaN && CameraFOVStatus.vfov != float.NaN && x != 0 && y != 0) { var hfov = CameraFOVStatus.hfov * Math.PI / 180; var vfov = CameraFOVStatus.vfov * Math.PI / 180; - vector.y = Math.Tan(x * hfov / 2); // x in the image is toward the right side of the plane (positive y in body frame) - vector.z = Math.Tan(y * vfov / 2); // y in the image is down (negative z in body frame) + vector.y = Math.Tan(x * hfov / 2); // x in the image is toward the right side of the plane (positive y in camera frame) + vector.z = Math.Tan(y * vfov / 2); // y in the image is down (z in camera frame) vector.normalize(); } + return vector; + } + + /// + /// Calculate the 3D unit vector of a pixel in the world frame, given its x/y position in the image. + /// + /// x position in the image, -1 to 1 (positive right) + /// y position in the image, -1 to 1 (positive down) + /// + public Vector3 CalculateImagePointVector(double x, double y) + { + if (CameraFOVStatus.q == null) + { + return new Vector3(1); + } + var v = CalculateImagePointVectorCameraFrame(x, y); var q = new Quaternion(CameraFOVStatus.q[0], CameraFOVStatus.q[1], CameraFOVStatus.q[2], CameraFOVStatus.q[3]); - return q.body_to_earth(vector); + return q.body_to_earth(v); + } + + /// + /// Calculate a rotation quaternion that will rotate the camera to point at a pixel in the image. + /// + /// x position in the image, -1 to 1 (positive right) + /// y position in the image, -1 to 1 (positive down) + /// + public Quaternion CalculateImagePointRotation(double x, double y) + { + var v1 = CalculateImagePointVectorCameraFrame(0, 0); + var v2 = CalculateImagePointVectorCameraFrame(x, y); + + if (v1 == -v2) + { + return Quaternion.from_axis_angle(new Vector3(0, 0, 1), Math.PI); // 180 degree rotation around z axis + } + + // The axis of rotation is the cross product of the two vectors + var axis = v1 % v2; + if(axis.length() == 0) + { + return new Quaternion(); + } + axis.normalize(); + + return Quaternion.from_axis_angle(axis, Math.Acos(v1 * v2)); } } } From fc73b29f2e8c4ef9f1c21fc9bd845418fe082f71 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Mon, 23 Sep 2024 09:02:06 +1000 Subject: [PATCH 28/45] GimbalVideoControl: add click-to-pan --- Controls/GimbalVideoControl.cs | 37 ++++++++++++++++--- ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs | 2 +- 2 files changed, 33 insertions(+), 6 deletions(-) diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs index e152288590..5da513b9a3 100644 --- a/Controls/GimbalVideoControl.cs +++ b/Controls/GimbalVideoControl.cs @@ -491,11 +491,38 @@ private void VideoBox_Click(object sender, EventArgs e) { MouseEventArgs me = (MouseEventArgs)e; var point = getMousePosition(me.X, me.Y); - if (!point.HasValue) { return; } + if (!point.HasValue) + { + return; + } - var loc = selectedCamera?.CalculateImagePointLocation(point.Value.x, point.Value.y); - if (loc != null) + // Check currently held modifier keys + if (Control.ModifierKeys == preferences.MoveCameraToMouseLocationModifier) { + var attitude = selectedGimbalManager?.GetAttitude(selectedGimbalID); + if (attitude == null) + { + return; + } + var q = selectedCamera?.CalculateImagePointRotation(point.Value.x, point.Value.y); + if (q == null) + { + return; + } + q = attitude * q; + Console.WriteLine("Attitude: {0:0.0} {1:0.0} {2:0.0}", attitude.get_euler_yaw() * MathHelper.rad2deg, attitude.get_euler_pitch() * MathHelper.rad2deg, attitude.get_euler_roll() * MathHelper.rad2deg); + Console.WriteLine("New: {0:0.0} {1:0.0} {2:0.0}", q.get_euler_yaw() * MathHelper.rad2deg, q.get_euler_pitch() * MathHelper.rad2deg, q.get_euler_roll() * MathHelper.rad2deg); + + selectedGimbalManager?.SetAttitudeAsync(q, yaw_lock, selectedGimbalID); + + } + else if (Control.ModifierKeys == preferences.MoveCameraPOIToMouseLocationModifier) + { + var loc = selectedCamera?.CalculateImagePointLocation(point.Value.x, point.Value.y); + if (loc == null) + { + return; + } selectedGimbalManager?.SetROILocationAsync(loc.Lat, loc.Lng, loc.Alt, frame: MAV_FRAME.GLOBAL); } } @@ -666,9 +693,9 @@ public GimbalControlPreferences() TrackObjectUnderMouse = MouseButton.Left; MoveCameraToMouseLocationModifier = Keys.None; - MoveCameraPOIToMouseLocationModifier = Keys.Shift; + MoveCameraPOIToMouseLocationModifier = Keys.Control; SlewCameraBasedOnMouseModifier = Keys.Alt; - TrackObjectUnderMouseModifier = Keys.Control; + TrackObjectUnderMouseModifier = Keys.Control | Keys.Alt; // Default speed settings SlewSpeedSlow = 1m; // deg/sec diff --git a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs index 3c7aa73af5..28acf12fbe 100644 --- a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs +++ b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs @@ -562,7 +562,7 @@ private void OnMAVDetected(object sender, (byte, byte) tuple) (tuple.Item2 >= (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER && tuple.Item2 <= (byte)MAV_COMPONENT.MAV_COMP_ID_ONBOARD_COMPUTER4)) { - MAVlist[tuple.Item1, tuple.Item2].GimbalManager = new GimbalManagerProtocol(this); + MAVlist[tuple.Item1, tuple.Item2].GimbalManager = new GimbalManagerProtocol(this, MAVlist[tuple.Item1, tuple.Item2].cs); Task.Run(async () => { try From 4418acee5d237c51d9bc3b01acc8a3e8c75a27ce Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sun, 29 Sep 2024 11:23:25 +1000 Subject: [PATCH 29/45] Controls: add Key and Click binding buttons --- ExtLibs/Controls/ClickBindingButton.cs | 208 +++++++++++++++ ExtLibs/Controls/KeyBindingButton.cs | 247 ++++++++++++++++++ .../Controls/MissionPlanner.Controls.csproj | 6 + 3 files changed, 461 insertions(+) create mode 100644 ExtLibs/Controls/ClickBindingButton.cs create mode 100644 ExtLibs/Controls/KeyBindingButton.cs diff --git a/ExtLibs/Controls/ClickBindingButton.cs b/ExtLibs/Controls/ClickBindingButton.cs new file mode 100644 index 0000000000..56aad2d547 --- /dev/null +++ b/ExtLibs/Controls/ClickBindingButton.cs @@ -0,0 +1,208 @@ +using System; +using System.ComponentModel; +using System.Drawing; +using System.Windows.Forms; + + +namespace MissionPlanner.Controls +{ + /// + /// Custom button control for binding a mouse click (with optional modifier keys) to a function. + /// + public class ClickBindingButton : MyButton + { + // Indicates if the button is currently waiting for an input + private bool isListening = false; + + // Stores the mouse button that is currently bound to this function + private MouseButtons _clickBinding = MouseButtons.None; + + // Stores the optional modifier keys that are bound to this function + private Keys _modifiers = Keys.None; + + // Backup of the outline color so we can paint it red when listening for a key + private Color _OutlineBackup; + + // The prompt text that is displayed when listening for input and nothing has been pressed yet + public string Prompt = "Click a mouse button... (Esc to cancel)"; + + /// + /// Event that is raised when the click binding is changed. + /// + public event EventHandler ClickBindingChanged; + + /// + /// The key/button combination that is bound to this function. + /// + [Category("Click Binding")] + [Description("The mouse button and modifier keys that are bound to this function.")] + public (Keys, MouseButtons) ClickBinding + { + get { return (_modifiers, _clickBinding); } + set + { + _modifiers = value.Item1; + _clickBinding = value.Item2; + Text = GetClickBindingString(_modifiers, _clickBinding); + ClickBindingChanged?.Invoke(this, EventArgs.Empty); + } + } + + public ClickBindingButton() + { + _BGGradTop = Color.FromArgb(0x94, 0xc1, 0x1f); + _BGGradBot = Color.FromArgb(0xcd, 0xe2, 0x96); + _TextColor = Color.FromArgb(0x40, 0x57, 0x04); + _Outline = Color.FromArgb(0x79, 0x94, 0x29); + _ColorNotEnabled = Color.FromArgb(73, 0x2b, 0x3a, 0x03); + _ColorMouseOver = Color.FromArgb(73, 0x2b, 0x3a, 0x03); + _ColorMouseDown = Color.FromArgb(150, 0x2b, 0x3a, 0x03); + } + + /// + /// Start listening for a mouse click, or bind the clicked button if already listening. + /// + /// + protected override void OnClick(EventArgs e) + { + if (!isListening) + { + // Start listening + _OutlineBackup = _Outline; + _Outline = Color.Red; + isListening = true; + Text = Prompt; + } + else + { + CancelClickAssignment(); + ClickBinding = (ModifierKeys, MouseButtons.Left); + } + + base.OnClick(e); + } + + protected override void OnMouseUp(MouseEventArgs mouseEvent) + { + if (isListening && mouseEvent.Button != MouseButtons.Left) // Left is handled by OnClick + { + CancelClickAssignment(); + ClickBinding = (ModifierKeys, mouseEvent.Button); + } + + base.OnMouseUp(mouseEvent); + } + + /// + /// Cancel click assignment. Prevents multiple buttons from listening at the same time. + /// + /// + protected override void OnLeave(EventArgs e) + { + if (isListening) + { + CancelClickAssignment(); + } + + base.OnLeave(e); + } + + /// + /// Update the displayed key combination + /// + /// + protected override void OnKeyDown(KeyEventArgs e) + { + if (isListening) + { + if (e.KeyCode == Keys.Escape) + { + CancelClickAssignment(); + return; + } + + _modifiers = ModifierKeys; + Text = GetModifiersString(_modifiers); + } + + base.OnKeyDown(e); + } + + /// + /// Update the displayed key combination when a modifier key is released. + /// + /// + protected override void OnKeyUp(KeyEventArgs e) + { + if (isListening) + { + if (e.Modifiers != Keys.None) + { + Text = GetModifiersString(e.KeyData); + } + else + { + Text = Prompt; + } + } + + base.OnKeyUp(e); + } + + /// + /// Cancel click assignment, restore outline color, and display the current key combination. + /// + private void CancelClickAssignment() + { + Text = GetClickBindingString(_modifiers, _clickBinding); + _Outline = _OutlineBackup; + isListening = false; + } + + /// + /// Get the string representation of the current key/button combination. + /// + /// Keys enum representing held modifier keys + /// MouseButtons enum representing the clicked mouse button + /// Key/button combination represented as a string + private string GetClickBindingString(Keys modifiers, MouseButtons clickButton) + { + if (clickButton == MouseButtons.None) + { + return "None"; + } + + string bindingString = GetModifiersString(modifiers); + + if (clickButton != MouseButtons.None) + { + bindingString += $"{clickButton} Click"; + } + + return bindingString; + } + + /// + /// Get a string representation of just the modifier keys in a key combination. + /// + /// Keys enum representing a key combination + /// Modifier keys represented as a string + private string GetModifiersString(Keys key) + { + string modifiers = ""; + if ((key & Keys.Shift) != 0) + { + modifiers += "Shift + "; + } + if ((key & Keys.Control) != 0) + { + modifiers += "Ctrl + "; + } + if ((key & Keys.Alt) != 0) + { + modifiers += "Alt + "; + } + return modifiers; + } + } +} diff --git a/ExtLibs/Controls/KeyBindingButton.cs b/ExtLibs/Controls/KeyBindingButton.cs new file mode 100644 index 0000000000..07ca44333a --- /dev/null +++ b/ExtLibs/Controls/KeyBindingButton.cs @@ -0,0 +1,247 @@ +using System; +using System.ComponentModel; +using System.Drawing; +using System.Windows.Forms; + + +namespace MissionPlanner.Controls +{ + /// + /// Custom button control for binding a key to a function. + /// + public class KeyBindingButton : MyButton + { + // Indicates if the button is currently waiting for a key input + private bool isListening = false; + + // Stores key combination that is currently bound to this function + private Keys _keyBinding = Keys.None; + + // Backup of the outline color so we can paint it red when listening for a key + private Color _OutlineBackup; + + // The prompt text that is displayed when listening for a key and nothing has been pressed yet + public string Prompt = "Press a key... (Esc to cancel)"; + + /// + /// Event that is raised when the key binding is changed. + /// + public event EventHandler KeyBindingChanged; + + /// + /// The key combination that is bound to this function. + /// + [Category("Key Binding")] + [Description("The key combination that is bound to this function.")] + public Keys KeyBinding + { + get { return _keyBinding; } + set + { + _keyBinding = value; + Text = GetKeyString(_keyBinding); + KeyBindingChanged?.Invoke(this, EventArgs.Empty); + } + } + + private bool _modifiersOnly = false; + /// + /// True if this button should only be used to assign a combination of modifier keys. + /// + [Category("Key Binding")] + [Description("Only allow modifier keys to be assigned.")] + public bool ModifiersOnly + { + get + { + return _modifiersOnly; + } + set + { + Prompt = value ? + "Press modifier keys + Enter..." : + "Press a key... (Esc to cancel)"; + _modifiersOnly = value; + } + } + + public KeyBindingButton() + { + _BGGradTop = Color.FromArgb(0x94, 0xc1, 0x1f); + _BGGradBot = Color.FromArgb(0xcd, 0xe2, 0x96); + _TextColor = Color.FromArgb(0x40, 0x57, 0x04); + _Outline = Color.FromArgb(0x79, 0x94, 0x29); + _ColorNotEnabled = Color.FromArgb(73, 0x2b, 0x3a, 0x03); + _ColorMouseOver = Color.FromArgb(73, 0x2b, 0x3a, 0x03); + _ColorMouseDown = Color.FromArgb(150, 0x2b, 0x3a, 0x03); + } + + /// + /// Start listening for a key press. + /// + /// + protected override void OnMouseClick(MouseEventArgs e) + { + if (!isListening) + { + // Make the outline red to indicate that we are listening for a key + _OutlineBackup = _Outline; + _Outline = Color.Red; + + isListening = true; + Text = Prompt; + } + + base.OnMouseClick(e); + } + + /// + /// Cancel key assignment. Prevents multiple buttons from listening at the same time. + /// + /// + protected override void OnLeave(EventArgs e) + { + if (isListening) + { + CancelKeyAssignment(); + } + + base.OnLeave(e); + } + + /// + /// Prevents arrow keys and space/enter from being ignored. + /// + /// + protected override void OnPreviewKeyDown(PreviewKeyDownEventArgs e) + { + e.IsInputKey = true; + base.OnPreviewKeyDown(e); + } + + /// + /// Update the displayed key combination, and stop listening if we have a valid key combination. + /// + /// + protected override void OnKeyDown(KeyEventArgs e) + { + if (isListening) + { + // Cancel key assignment if the user presses escape + if (e.KeyCode == Keys.Escape) + { + CancelKeyAssignment(); + return; + } + + // Display the modifier keys as they are pressed + Text = GetModifiersString(e.KeyData); + + // Modifier keys are not allowed as the main key + if (e.KeyCode == Keys.ShiftKey || e.KeyCode == Keys.ControlKey || e.KeyCode == Keys.Menu) + { + return; + } + + if (_modifiersOnly) + { + // If only modifier keys are allowed, we wait for the user to press Enter + if (e.KeyCode == Keys.Enter) + { + KeyBinding = e.Modifiers; + CancelKeyAssignment(); + } + return; + } + + // If any non-modifier key is pressed, assign it (and any modifiers) to the key binding + KeyBinding = e.KeyData; + CancelKeyAssignment(); + } + + base.OnKeyDown(e); + } + + /// + /// Update the displayed key combination when a modifier key is released. + /// + /// + protected override void OnKeyUp(KeyEventArgs e) + { + if (isListening) + { + if (e.Modifiers != Keys.None) + { + Text = GetModifiersString(e.KeyData); + } + else + { + // No keys are pressed, display the prompt text again + Text = Prompt; + } + } + + base.OnKeyUp(e); + } + + /// + /// Cancel key assignment, restore outline color, and display the current key combination. + /// + private void CancelKeyAssignment() + { + Text = GetKeyString(KeyBinding); + _Outline = _OutlineBackup; + isListening = false; + this.Invalidate(); + } + + /// + /// Get a string representation of a key combination. + /// + /// Keys enum representing a key combination + /// Keys represented as a string + private string GetKeyString(Keys key) + { + string modString = GetModifiersString(key); + + if (_modifiersOnly) + { + return modString; + } + + // Remove the modifiers to isolate the main key + key &= ~Keys.Shift; + key &= ~Keys.Control; + key &= ~Keys.Alt; + + if (key == Keys.None) + { + return "None"; + } + return modString + key.ToString(); + } + + /// + /// Get a string representation of just the modifier keys in a key combination. + /// + /// Keys enum representing a key combination + /// Modifier keys represented as a string + private string GetModifiersString(Keys key) + { + string modifiers = ""; + if ((key & Keys.Shift) != 0) + { + modifiers += "Shift + "; + } + if ((key & Keys.Control) != 0) + { + modifiers += "Ctrl + "; + } + if ((key & Keys.Alt) != 0) + { + modifiers += "Alt + "; + } + return modifiers; + } + } +} diff --git a/ExtLibs/Controls/MissionPlanner.Controls.csproj b/ExtLibs/Controls/MissionPlanner.Controls.csproj index ba2693f951..f6166554ee 100644 --- a/ExtLibs/Controls/MissionPlanner.Controls.csproj +++ b/ExtLibs/Controls/MissionPlanner.Controls.csproj @@ -44,6 +44,12 @@ True HUDT.resx + + Component + + + Component + True True From cace2de9de0f2c75c96a26eb122a0d74706790c1 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sun, 29 Sep 2024 16:19:19 +1000 Subject: [PATCH 30/45] Add GimbalControlSettingsForm --- .../GimbalControlSettingsForm.Designer.cs | 130 +++++++ Controls/GimbalControlSettingsForm.cs | 329 ++++++++++++++++++ Controls/GimbalControlSettingsForm.resx | 120 +++++++ Controls/GimbalVideoControl.Designer.cs | 39 ++- Controls/GimbalVideoControl.cs | 128 +------ 5 files changed, 613 insertions(+), 133 deletions(-) create mode 100644 Controls/GimbalControlSettingsForm.Designer.cs create mode 100644 Controls/GimbalControlSettingsForm.cs create mode 100644 Controls/GimbalControlSettingsForm.resx diff --git a/Controls/GimbalControlSettingsForm.Designer.cs b/Controls/GimbalControlSettingsForm.Designer.cs new file mode 100644 index 0000000000..8a88940627 --- /dev/null +++ b/Controls/GimbalControlSettingsForm.Designer.cs @@ -0,0 +1,130 @@ +namespace MissionPlanner.Controls +{ + partial class GimbalControlSettingsForm + { + /// + /// Required designer variable. + /// + private System.ComponentModel.IContainer components = null; + + /// + /// Clean up any resources being used. + /// + /// true if managed resources should be disposed; otherwise, false. + protected override void Dispose(bool disposing) + { + if (disposing && (components != null)) + { + components.Dispose(); + } + base.Dispose(disposing); + } + + #region Windows Form Designer generated code + + /// + /// Required method for Designer support - do not modify + /// the contents of this method with the code editor. + /// + private void InitializeComponent() + { + this.SettingsPanel = new System.Windows.Forms.Panel(); + this.SettingsTablePanel = new System.Windows.Forms.TableLayoutPanel(); + this.ButtonsPanel = new System.Windows.Forms.Panel(); + this.but_cancel = new MissionPlanner.Controls.MyButton(); + this.but_save = new MissionPlanner.Controls.MyButton(); + this.SettingsPanel.SuspendLayout(); + this.ButtonsPanel.SuspendLayout(); + this.SuspendLayout(); + // + // SettingsPanel + // + this.SettingsPanel.AutoScroll = true; + this.SettingsPanel.Controls.Add(this.SettingsTablePanel); + this.SettingsPanel.Dock = System.Windows.Forms.DockStyle.Fill; + this.SettingsPanel.Location = new System.Drawing.Point(0, 0); + this.SettingsPanel.Name = "SettingsPanel"; + this.SettingsPanel.Size = new System.Drawing.Size(533, 474); + this.SettingsPanel.TabIndex = 0; + // + // SettingsTablePanel + // + this.SettingsTablePanel.AutoSize = true; + this.SettingsTablePanel.AutoSizeMode = System.Windows.Forms.AutoSizeMode.GrowAndShrink; + this.SettingsTablePanel.ColumnCount = 3; + this.SettingsTablePanel.ColumnStyles.Add(new System.Windows.Forms.ColumnStyle()); + this.SettingsTablePanel.ColumnStyles.Add(new System.Windows.Forms.ColumnStyle(System.Windows.Forms.SizeType.Percent, 100F)); + this.SettingsTablePanel.ColumnStyles.Add(new System.Windows.Forms.ColumnStyle()); + this.SettingsTablePanel.Dock = System.Windows.Forms.DockStyle.Top; + this.SettingsTablePanel.Location = new System.Drawing.Point(0, 0); + this.SettingsTablePanel.Name = "SettingsTablePanel"; + this.SettingsTablePanel.RowCount = 6; + this.SettingsTablePanel.RowStyles.Add(new System.Windows.Forms.RowStyle(System.Windows.Forms.SizeType.Absolute, 20F)); + this.SettingsTablePanel.RowStyles.Add(new System.Windows.Forms.RowStyle(System.Windows.Forms.SizeType.Absolute, 20F)); + this.SettingsTablePanel.RowStyles.Add(new System.Windows.Forms.RowStyle(System.Windows.Forms.SizeType.Absolute, 20F)); + this.SettingsTablePanel.RowStyles.Add(new System.Windows.Forms.RowStyle(System.Windows.Forms.SizeType.Absolute, 20F)); + this.SettingsTablePanel.RowStyles.Add(new System.Windows.Forms.RowStyle(System.Windows.Forms.SizeType.Absolute, 20F)); + this.SettingsTablePanel.RowStyles.Add(new System.Windows.Forms.RowStyle(System.Windows.Forms.SizeType.Absolute, 20F)); + this.SettingsTablePanel.Size = new System.Drawing.Size(533, 120); + this.SettingsTablePanel.TabIndex = 0; + // + // ButtonsPanel + // + this.ButtonsPanel.Controls.Add(this.but_cancel); + this.ButtonsPanel.Controls.Add(this.but_save); + this.ButtonsPanel.Dock = System.Windows.Forms.DockStyle.Bottom; + this.ButtonsPanel.Location = new System.Drawing.Point(0, 474); + this.ButtonsPanel.Name = "ButtonsPanel"; + this.ButtonsPanel.Size = new System.Drawing.Size(533, 30); + this.ButtonsPanel.TabIndex = 1; + // + // but_cancel + // + this.but_cancel.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Right))); + this.but_cancel.Location = new System.Drawing.Point(455, 4); + this.but_cancel.Name = "but_cancel"; + this.but_cancel.Size = new System.Drawing.Size(75, 23); + this.but_cancel.TabIndex = 1; + this.but_cancel.Text = "Cancel"; + this.but_cancel.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); + this.but_cancel.UseVisualStyleBackColor = true; + this.but_cancel.Click += new System.EventHandler(this.but_cancel_Click); + // + // but_save + // + this.but_save.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Right))); + this.but_save.Location = new System.Drawing.Point(374, 4); + this.but_save.Name = "but_save"; + this.but_save.Size = new System.Drawing.Size(75, 23); + this.but_save.TabIndex = 0; + this.but_save.Text = "Save"; + this.but_save.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); + this.but_save.UseVisualStyleBackColor = true; + this.but_save.Click += new System.EventHandler(this.but_save_Click); + // + // GimbalControlSettingsForm + // + this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); + this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; + this.ClientSize = new System.Drawing.Size(533, 504); + this.Controls.Add(this.SettingsPanel); + this.Controls.Add(this.ButtonsPanel); + this.Margin = new System.Windows.Forms.Padding(2); + this.Name = "GimbalControlSettingsForm"; + this.Text = "GimbalVideoControlSettings"; + this.SettingsPanel.ResumeLayout(false); + this.SettingsPanel.PerformLayout(); + this.ButtonsPanel.ResumeLayout(false); + this.ResumeLayout(false); + + } + + #endregion + + private System.Windows.Forms.Panel SettingsPanel; + private System.Windows.Forms.Panel ButtonsPanel; + private MyButton but_cancel; + private MyButton but_save; + private System.Windows.Forms.TableLayoutPanel SettingsTablePanel; + } +} \ No newline at end of file diff --git a/Controls/GimbalControlSettingsForm.cs b/Controls/GimbalControlSettingsForm.cs new file mode 100644 index 0000000000..c6375d4906 --- /dev/null +++ b/Controls/GimbalControlSettingsForm.cs @@ -0,0 +1,329 @@ +using System; +using System.Collections.Generic; +using System.ComponentModel; +using System.Data; +using System.Drawing; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using System.Windows.Forms; +using MissionPlanner.Utilities; + +namespace MissionPlanner.Controls +{ + public partial class GimbalControlSettingsForm : Form + { + public GimbalControlSettings preferences; + + public GimbalControlSettingsForm(GimbalControlSettings preferences) + { + InitializeComponent(); + + ThemeManager.ApplyThemeTo(this); + + this.preferences = new GimbalControlSettings(preferences); + + LoadPreferences(); + } + + private void LoadPreferences() + { + var properties = preferences.GetType().GetProperties(); + + // Delete all rows in the table layout panel + SettingsTablePanel.Controls.Clear(); + SettingsTablePanel.RowStyles.Clear(); + int row = -1; + foreach (var property in properties) + { + var attributes = property.GetCustomAttributes(typeof(PreferencesAttribute), true); + if (attributes.Length == 0) + { + continue; + } + + row++; + SettingsTablePanel.RowStyles.Add(new RowStyle(SizeType.AutoSize)); + + var attribute = (PreferencesAttribute)attributes[0]; + + var label = new Label + { + Width = 200, + Text = attribute.LabelText, + TextAlign = ContentAlignment.MiddleLeft, + Anchor = AnchorStyles.Left, + }; + + SettingsTablePanel.Controls.Add(label, 0, row); + + switch (attribute.ControlType) + { + case ControlType.KeyBindingButton: + var keyBindingButton = new KeyBindingButton + { + KeyBinding = (Keys)property.GetValue(preferences), + Anchor = AnchorStyles.Left | AnchorStyles.Right, + }; + keyBindingButton.KeyBindingChanged += (sender, e) => + { + property.SetValue(preferences, keyBindingButton.KeyBinding); + }; + SettingsTablePanel.Controls.Add(keyBindingButton, 1, row); + var clearKeyButton = new MyButton + { + Text = "❌", + Width = keyBindingButton.Height, + Height = keyBindingButton.Height, + }; + clearKeyButton.Click += (sender, e) => + { + keyBindingButton.KeyBinding = Keys.None; + }; + SettingsTablePanel.Controls.Add(clearKeyButton, 2, row); + break; + case ControlType.ClickBindingButton: + var clickBindingButton = new ClickBindingButton + { + ClickBinding = ((Keys, MouseButtons))property.GetValue(preferences), + Anchor = AnchorStyles.Left | AnchorStyles.Right, + }; + clickBindingButton.ClickBindingChanged += (sender, e) => + { + property.SetValue(preferences, clickBindingButton.ClickBinding); + }; + SettingsTablePanel.Controls.Add(clickBindingButton, 1, row); + var clearClickButton = new MyButton + { + Text = "❌", + Width = clickBindingButton.Height, + Height = clickBindingButton.Height, + }; + clearClickButton.Click += (sender, e) => + { + clickBindingButton.ClickBinding = (Keys.None, MouseButtons.None); + }; + SettingsTablePanel.Controls.Add(clearClickButton, 2, row); + break; + case ControlType.ModifierBinding: + var modifierBinding = new KeyBindingButton + { + ModifiersOnly = true, + Anchor = AnchorStyles.Left | AnchorStyles.Right, + }; + modifierBinding.KeyBinding = (Keys)property.GetValue(preferences); + SettingsTablePanel.Controls.Add(modifierBinding, 1, row); + modifierBinding.KeyBindingChanged += (sender, e) => + { + property.SetValue(preferences, modifierBinding.KeyBinding); + }; + var clearModifierButton = new MyButton + { + Text = "❌", + Width = modifierBinding.Height, + Height = modifierBinding.Height, + }; + clearModifierButton.Click += (sender, e) => + { + modifierBinding.KeyBinding = Keys.None; + }; + SettingsTablePanel.Controls.Add(clearModifierButton, 2, row); + break; + case ControlType.DecimalUpDown: + var value = (decimal)property.GetValue(preferences); + var decimalUpDown = new NumericUpDown + { + Value = value, + Minimum = Math.Min((decimal)attribute.Min, value), + Maximum = Math.Max((decimal)attribute.Max, value), + Increment = (decimal)attribute.Increment, + DecimalPlaces = attribute.DecimalPlaces + }; + decimalUpDown.ValueChanged += (sender, e) => + { + property.SetValue(preferences, decimalUpDown.Value); + }; + SettingsTablePanel.Controls.Add(decimalUpDown, 1, row); + break; + case ControlType.Checkbox: + var checkbox = new CheckBox + { + Checked = (bool)property.GetValue(preferences) + }; + checkbox.CheckedChanged += (sender, e) => + { + property.SetValue(preferences, checkbox.Checked); + }; + SettingsTablePanel.Controls.Add(checkbox, 1, row); + break; + } + } + + // Recalculate column widths + SettingsTablePanel.Invalidate(); + } + + private void but_save_Click(object sender, EventArgs e) + { + this.DialogResult = DialogResult.OK; + this.Close(); + } + + private void but_cancel_Click(object sender, EventArgs e) + { + this.DialogResult = DialogResult.Cancel; + this.Close(); + } + } + + public enum ControlType + { + KeyBindingButton, + ClickBindingButton, + ModifierBinding, + DecimalUpDown, + Checkbox + } + + [AttributeUsage(AttributeTargets.Property)] + public class PreferencesAttribute : Attribute + { + public string LabelText { get; } + public ControlType ControlType { get; } + + // Additional properties for DecimalUpDown control + public double Min { get; set; } = 0; + public double Max { get; set; } = 100; + public double Increment { get; set; } = 1; + public int DecimalPlaces { get; set; } = 2; + + public PreferencesAttribute(string labelText, ControlType controlType) + { + LabelText = labelText; + ControlType = controlType; + } + } + + public class GimbalControlSettings + { + // Keybindings for various actions + [Preferences("Slew Left", ControlType.KeyBindingButton)] + public Keys SlewLeft { get; set; } + [Preferences("Slew Right", ControlType.KeyBindingButton)] + public Keys SlewRight { get; set; } + [Preferences("Slew Up", ControlType.KeyBindingButton)] + public Keys SlewUp { get; set; } + [Preferences("Slew Down", ControlType.KeyBindingButton)] + public Keys SlewDown { get; set; } + [Preferences("Zoom In", ControlType.KeyBindingButton)] + public Keys ZoomIn { get; set; } + [Preferences("Zoom Out", ControlType.KeyBindingButton)] + public Keys ZoomOut { get; set; } + + [Preferences("Slew Fast Modifier", ControlType.ModifierBinding)] + public Keys SlewFastModifier { get; set; } + [Preferences("Slew Slow Modifier", ControlType.ModifierBinding)] + public Keys SlewSlowModifier { get; set; } + + [Preferences("Take Picture", ControlType.KeyBindingButton)] + public Keys TakePicture { get; set; } + [Preferences("Toggle Recording", ControlType.KeyBindingButton)] + public Keys ToggleRecording { get; set; } + [Preferences("Start Recording", ControlType.KeyBindingButton)] + public Keys StartRecording { get; set; } + [Preferences("Stop Recording", ControlType.KeyBindingButton)] + public Keys StopRecording { get; set; } + + [Preferences("Toggle Lock/Follow", ControlType.KeyBindingButton)] + public Keys ToggleLockFollow { get; set; } + [Preferences("Set Lock", ControlType.KeyBindingButton)] + public Keys SetLock { get; set; } + [Preferences("Set Follow", ControlType.KeyBindingButton)] + public Keys SetFollow { get; set; } + [Preferences("Retract", ControlType.KeyBindingButton)] + public Keys Retract { get; set; } + [Preferences("Neutral", ControlType.KeyBindingButton)] + public Keys Neutral { get; set; } + [Preferences("Point Down", ControlType.KeyBindingButton)] + public Keys PointDown { get; set; } + [Preferences("Home", ControlType.KeyBindingButton)] + public Keys Home { get; set; } + + + [Preferences("Click Pan/Tilt", ControlType.ClickBindingButton)] + public (Keys, MouseButtons) MoveCameraToMouseLocation { get; set; } + [Preferences("Click Point-of-Interest", ControlType.ClickBindingButton)] + public (Keys, MouseButtons) MoveCameraPOIToMouseLocation { get; set; } + [Preferences("Click Track Object", ControlType.ClickBindingButton)] + public (Keys, MouseButtons) TrackObjectUnderMouse { get; set; } + + + // Speed settings + [Preferences("Slew Speed Slow (deg/s)", ControlType.DecimalUpDown, Min = 0.1, Max = 360, Increment = 1, DecimalPlaces = 1)] + public decimal SlewSpeedSlow { get; set; } + [Preferences("Slew Speed Normal (deg/s)", ControlType.DecimalUpDown, Min = 0.1, Max = 360, Increment = 1, DecimalPlaces = 1)] + public decimal SlewSpeedNormal { get; set; } + [Preferences("Slew Speed Fast (deg/s)", ControlType.DecimalUpDown, Min = 0.1, Max = 360, Increment = 1, DecimalPlaces = 1)] + public decimal SlewSpeedFast { get; set; } + + [Preferences("Zoom Speed (unitless)", ControlType.DecimalUpDown, Min = 0.01, Max = 1, Increment = 0.1, DecimalPlaces = 2)] + public decimal ZoomSpeed { get; set; } + [Preferences("Camera FOV (deg)", ControlType.DecimalUpDown, Min = 0.01, Max = 180, Increment = 1, DecimalPlaces = 2)] + public decimal CameraFOV { get; set; } + + // Boolean options + [Preferences("Use FOV Reported by Camera", ControlType.Checkbox)] + public bool UseFOVReportedByCamera { get; set; } + [Preferences("Default Locked Mode", ControlType.Checkbox)] + public bool DefaultLockedMode { get; set; } + + public GimbalControlSettings() + { + SlewLeft = Keys.A; + SlewRight = Keys.D; + SlewUp = Keys.W; + SlewDown = Keys.S; + ZoomIn = Keys.E; + ZoomOut = Keys.Q; + + SlewSlowModifier = Keys.Control; + SlewFastModifier = Keys.Shift; + + TakePicture = Keys.Alt | Keys.F; + ToggleRecording = Keys.Alt | Keys.R; + StartRecording = Keys.None; + StopRecording = Keys.None; + + ToggleLockFollow = Keys.L; + SetLock = Keys.None; + SetFollow = Keys.None; + Retract = Keys.None; + Neutral = Keys.N; + PointDown = Keys.None; + Home = Keys.H; + + MoveCameraToMouseLocation = (Keys.None, MouseButtons.Left); + MoveCameraPOIToMouseLocation = (Keys.Control, MouseButtons.Left); + TrackObjectUnderMouse = (Keys.Alt, MouseButtons.Left); + + // Default speed settings + SlewSpeedSlow = 1m; // deg/sec + SlewSpeedNormal = 5m; // deg/sec + SlewSpeedFast = 25m; // deg/sec + ZoomSpeed = 1.0m; // unitless [0, 1] + CameraFOV = 50.0m; // horizontal, degrees + + // Default boolean options + DefaultLockedMode = false; + UseFOVReportedByCamera = true; + } + + public GimbalControlSettings(GimbalControlSettings other) + { + foreach (var property in other.GetType().GetProperties()) + { + property.SetValue(this, property.GetValue(other)); + } + } + } +} diff --git a/Controls/GimbalControlSettingsForm.resx b/Controls/GimbalControlSettingsForm.resx new file mode 100644 index 0000000000..29dcb1b3a3 --- /dev/null +++ b/Controls/GimbalControlSettingsForm.resx @@ -0,0 +1,120 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + text/microsoft-resx + + + 2.0 + + + System.Resources.ResXResourceReader, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + \ No newline at end of file diff --git a/Controls/GimbalVideoControl.Designer.cs b/Controls/GimbalVideoControl.Designer.cs index dc830d8d99..21c7a3f8c9 100644 --- a/Controls/GimbalVideoControl.Designer.cs +++ b/Controls/GimbalVideoControl.Designer.cs @@ -28,11 +28,11 @@ private void InitializeComponent() this.toolStripSeparator1 = new System.Windows.Forms.ToolStripSeparator(); this.takePictureToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.startRecordingToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.stopRecordingToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.toolStripMenuItem2 = new System.Windows.Forms.ToolStripSeparator(); this.settingsToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.ControlInfoTooltip = new System.Windows.Forms.ToolTip(this.components); this.UITimer = new System.Windows.Forms.Timer(this.components); - this.stopRecordingToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); ((System.ComponentModel.ISupportInitialize)(this.VideoBox)).BeginInit(); this.VideoBoxContextMenu.SuspendLayout(); this.SuspendLayout(); @@ -77,59 +77,59 @@ private void InitializeComponent() // videoStreamToolStripMenuItem // this.videoStreamToolStripMenuItem.Name = "videoStreamToolStripMenuItem"; - this.videoStreamToolStripMenuItem.Size = new System.Drawing.Size(155, 22); + this.videoStreamToolStripMenuItem.Size = new System.Drawing.Size(180, 22); this.videoStreamToolStripMenuItem.Text = "Video Stream"; this.videoStreamToolStripMenuItem.Click += new System.EventHandler(this.videoStreamToolStripMenuItem_Click); // // toolStripMenuItem1 // this.toolStripMenuItem1.Name = "toolStripMenuItem1"; - this.toolStripMenuItem1.Size = new System.Drawing.Size(152, 6); + this.toolStripMenuItem1.Size = new System.Drawing.Size(177, 6); // // retractToolStripMenuItem // this.retractToolStripMenuItem.Name = "retractToolStripMenuItem"; - this.retractToolStripMenuItem.Size = new System.Drawing.Size(155, 22); + this.retractToolStripMenuItem.Size = new System.Drawing.Size(180, 22); this.retractToolStripMenuItem.Text = "Retract"; this.retractToolStripMenuItem.Click += new System.EventHandler(this.retractToolStripMenuItem_Click); // // neutralToolStripMenuItem // this.neutralToolStripMenuItem.Name = "neutralToolStripMenuItem"; - this.neutralToolStripMenuItem.Size = new System.Drawing.Size(155, 22); + this.neutralToolStripMenuItem.Size = new System.Drawing.Size(180, 22); this.neutralToolStripMenuItem.Text = "Neutral"; this.neutralToolStripMenuItem.Click += new System.EventHandler(this.neutralToolStripMenuItem_Click); // // pointDownToolStripMenuItem // this.pointDownToolStripMenuItem.Name = "pointDownToolStripMenuItem"; - this.pointDownToolStripMenuItem.Size = new System.Drawing.Size(155, 22); + this.pointDownToolStripMenuItem.Size = new System.Drawing.Size(180, 22); this.pointDownToolStripMenuItem.Text = "Point Down"; this.pointDownToolStripMenuItem.Click += new System.EventHandler(this.pointDownToolStripMenuItem_Click); // // pointHomeToolStripMenuItem // this.pointHomeToolStripMenuItem.Name = "pointHomeToolStripMenuItem"; - this.pointHomeToolStripMenuItem.Size = new System.Drawing.Size(155, 22); + this.pointHomeToolStripMenuItem.Size = new System.Drawing.Size(180, 22); this.pointHomeToolStripMenuItem.Text = "Point Home"; this.pointHomeToolStripMenuItem.Click += new System.EventHandler(this.pointHomeToolStripMenuItem_Click); // // yawLockToolStripMenuItem // this.yawLockToolStripMenuItem.Name = "yawLockToolStripMenuItem"; - this.yawLockToolStripMenuItem.Size = new System.Drawing.Size(155, 22); + this.yawLockToolStripMenuItem.Size = new System.Drawing.Size(180, 22); this.yawLockToolStripMenuItem.Text = "Yaw Lock"; this.yawLockToolStripMenuItem.Click += new System.EventHandler(this.yawLockToolStripMenuItem_Click); // // toolStripSeparator1 // this.toolStripSeparator1.Name = "toolStripSeparator1"; - this.toolStripSeparator1.Size = new System.Drawing.Size(152, 6); + this.toolStripSeparator1.Size = new System.Drawing.Size(177, 6); // // takePictureToolStripMenuItem // this.takePictureToolStripMenuItem.Name = "takePictureToolStripMenuItem"; - this.takePictureToolStripMenuItem.Size = new System.Drawing.Size(155, 22); + this.takePictureToolStripMenuItem.Size = new System.Drawing.Size(180, 22); this.takePictureToolStripMenuItem.Text = "Take Picture"; this.takePictureToolStripMenuItem.Click += new System.EventHandler(this.takePictureToolStripMenuItem_Click); // @@ -140,16 +140,24 @@ private void InitializeComponent() this.startRecordingToolStripMenuItem.Text = "Start Recording"; this.startRecordingToolStripMenuItem.Click += new System.EventHandler(this.startRecordingToolStripMenuItem_Click); // + // stopRecordingToolStripMenuItem + // + this.stopRecordingToolStripMenuItem.Name = "stopRecordingToolStripMenuItem"; + this.stopRecordingToolStripMenuItem.Size = new System.Drawing.Size(180, 22); + this.stopRecordingToolStripMenuItem.Text = "Stop Recording"; + this.stopRecordingToolStripMenuItem.Click += new System.EventHandler(this.stopRecordingToolStripMenuItem_Click); + // // toolStripMenuItem2 // this.toolStripMenuItem2.Name = "toolStripMenuItem2"; - this.toolStripMenuItem2.Size = new System.Drawing.Size(152, 6); + this.toolStripMenuItem2.Size = new System.Drawing.Size(177, 6); // // settingsToolStripMenuItem // this.settingsToolStripMenuItem.Name = "settingsToolStripMenuItem"; - this.settingsToolStripMenuItem.Size = new System.Drawing.Size(155, 22); + this.settingsToolStripMenuItem.Size = new System.Drawing.Size(180, 22); this.settingsToolStripMenuItem.Text = "Settings"; + this.settingsToolStripMenuItem.Click += new System.EventHandler(this.settingsToolStripMenuItem_Click); // // UITimer // @@ -157,13 +165,6 @@ private void InitializeComponent() this.UITimer.Interval = 500; this.UITimer.Tick += new System.EventHandler(this.UITimer_Tick); // - // stopRecordingToolStripMenuItem - // - this.stopRecordingToolStripMenuItem.Name = "stopRecordingToolStripMenuItem"; - this.stopRecordingToolStripMenuItem.Size = new System.Drawing.Size(180, 22); - this.stopRecordingToolStripMenuItem.Text = "Stop Recording"; - this.stopRecordingToolStripMenuItem.Click += new System.EventHandler(this.stopRecordingToolStripMenuItem_Click); - // // GimbalVideoControl // this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs index 5da513b9a3..de5d9e12b5 100644 --- a/Controls/GimbalVideoControl.cs +++ b/Controls/GimbalVideoControl.cs @@ -12,7 +12,6 @@ using System.Windows.Forms; using MissionPlanner.Utilities; using SkiaSharp; -using OpenTK.Input; using log4net; using System.Collections.Generic; using static MAVLink; @@ -29,7 +28,7 @@ public partial class GimbalVideoControl : UserControl, IMessageFilter // logger private static readonly ILog log = LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType); - private GimbalControlPreferences preferences = new GimbalControlPreferences(); + private GimbalControlSettings preferences = new GimbalControlSettings(); private readonly GStreamer _stream = new GStreamer(); @@ -111,12 +110,13 @@ public GimbalVideoControl() private void loadPreferences() { + preferences = new GimbalControlSettings(); var json = Settings.Instance["GimbalControlPreferences", ""]; if (json != "") { try { - preferences = Newtonsoft.Json.JsonConvert.DeserializeObject(json); + preferences = Newtonsoft.Json.JsonConvert.DeserializeObject(json); } catch (Exception ex) { @@ -174,16 +174,6 @@ private void RenderFrame(object sender, MPBitmap image) PixelFormat.Format32bppPArgb, image.LockBits(Rectangle.Empty, null, SKColorType.Bgra8888).Scan0); - // Overlay crosshairs - if (Control.ModifierKeys == preferences.SlewCameraBasedOnMouseModifier) - { - using (var g = Graphics.FromImage(VideoBox.Image)) - { - g.DrawLine(Pens.Red, image.Width / 2, 0, image.Width / 2, image.Height); - g.DrawLine(Pens.Red, 0, image.Height / 2, image.Width, image.Height / 2); - } - } - old?.Dispose(); } catch (Exception ex) @@ -496,8 +486,8 @@ private void VideoBox_Click(object sender, EventArgs e) return; } - // Check currently held modifier keys - if (Control.ModifierKeys == preferences.MoveCameraToMouseLocationModifier) + // Check the key/button combination to determine the action + if ((Control.ModifierKeys, me.Button) == preferences.MoveCameraToMouseLocation) { var attitude = selectedGimbalManager?.GetAttitude(selectedGimbalID); if (attitude == null) @@ -516,7 +506,7 @@ private void VideoBox_Click(object sender, EventArgs e) selectedGimbalManager?.SetAttitudeAsync(q, yaw_lock, selectedGimbalID); } - else if (Control.ModifierKeys == preferences.MoveCameraPOIToMouseLocationModifier) + else if ((Control.ModifierKeys, me.Button) == preferences.MoveCameraPOIToMouseLocation) { var loc = selectedCamera?.CalculateImagePointLocation(point.Value.x, point.Value.y); if (loc == null) @@ -610,105 +600,15 @@ private void stopRecordingToolStripMenuItem_Click(object sender, EventArgs e) { SetRecording(false); } - } - public class GimbalControlPreferences - { - // Keybindings for various actions - public Keys SlewLeft { get; set; } - public Keys SlewRight { get; set; } - public Keys SlewUp { get; set; } - public Keys SlewDown { get; set; } - public Keys ZoomIn { get; set; } - public Keys ZoomOut { get; set; } - - public Keys SlewFastModifier { get; set; } - public Keys SlewSlowModifier { get; set; } - - public Keys TakePicture { get; set; } - public Keys ToggleRecording { get; set; } - public Keys StartRecording { get; set; } - public Keys StopRecording { get; set; } - - public Keys ToggleLockFollow { get; set; } - public Keys SetLock { get; set; } - public Keys SetFollow { get; set; } - public Keys Retract { get; set; } - public Keys Neutral { get; set; } - public Keys PointDown { get; set; } - public Keys Home { get; set; } - - - public MouseButton MoveCameraToMouseLocation { get; set; } - public MouseButton MoveCameraPOIToMouseLocation { get; set; } - public MouseButton SlewCameraBasedOnMouse { get; set; } - public MouseButton TrackObjectUnderMouse { get; set; } - - public Keys MoveCameraToMouseLocationModifier { get; set; } - public Keys MoveCameraPOIToMouseLocationModifier { get; set; } - public Keys SlewCameraBasedOnMouseModifier { get; set; } - public Keys TrackObjectUnderMouseModifier { get; set; } - - // Speed settings - public decimal SlewSpeedSlow { get; set; } - public decimal SlewSpeedNormal { get; set; } - public decimal SlewSpeedFast { get; set; } - public decimal ZoomSpeed { get; set; } - public decimal CameraFOV { get; set; } - - // Boolean options - public bool UseScrollForZoom { get; set; } - public bool DefaultLockedMode { get; set; } - public bool UseFOVReportedByCamera { get; set; } - public bool ShowCameraControls { get; set; } - - public GimbalControlPreferences() - { - SlewLeft = Keys.A; - SlewRight = Keys.D; - SlewUp = Keys.W; - SlewDown = Keys.S; - ZoomIn = Keys.E; - ZoomOut = Keys.Q; - - SlewSlowModifier = Keys.Control; - SlewFastModifier = Keys.Shift; - - TakePicture = Keys.Alt | Keys.F; - ToggleRecording = Keys.Alt | Keys.R; - StartRecording = Keys.None; - StopRecording = Keys.None; - - ToggleLockFollow = Keys.L; - SetLock = Keys.None; - SetFollow = Keys.None; - Retract = Keys.None; - Neutral = Keys.N; - PointDown = Keys.None; - Home = Keys.H; - - MoveCameraToMouseLocation = MouseButton.Left; - MoveCameraPOIToMouseLocation = MouseButton.Left; - SlewCameraBasedOnMouse = MouseButton.Left; - TrackObjectUnderMouse = MouseButton.Left; - - MoveCameraToMouseLocationModifier = Keys.None; - MoveCameraPOIToMouseLocationModifier = Keys.Control; - SlewCameraBasedOnMouseModifier = Keys.Alt; - TrackObjectUnderMouseModifier = Keys.Control | Keys.Alt; - - // Default speed settings - SlewSpeedSlow = 1m; // deg/sec - SlewSpeedNormal = 5m; // deg/sec - SlewSpeedFast = 25m; // deg/sec - ZoomSpeed = 1.0m; // unitless [0, 1] - CameraFOV = 50.0m; // horizontal, degrees - - // Default boolean options - UseScrollForZoom = true; - DefaultLockedMode = false; - UseFOVReportedByCamera = true; - ShowCameraControls = true; + private void settingsToolStripMenuItem_Click(object sender, EventArgs e) + { + var form = new GimbalControlSettingsForm(preferences); + if (form.ShowDialog() == DialogResult.OK) + { + Settings.Instance["GimbalControlPreferences"] = Newtonsoft.Json.JsonConvert.SerializeObject(form.preferences); + loadPreferences(); + } } } } From 03d8af3efb6941842b82c927c445233d0681da24 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sun, 29 Sep 2024 16:41:54 +1000 Subject: [PATCH 31/45] GimbalControlSettings: implement FOV settings --- Controls/GimbalControlSettingsForm.cs | 9 ++-- Controls/GimbalVideoControl.cs | 7 +++ ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs | 49 ++++++++++++++++++--- 3 files changed, 57 insertions(+), 8 deletions(-) diff --git a/Controls/GimbalControlSettingsForm.cs b/Controls/GimbalControlSettingsForm.cs index c6375d4906..85419c8056 100644 --- a/Controls/GimbalControlSettingsForm.cs +++ b/Controls/GimbalControlSettingsForm.cs @@ -268,8 +268,10 @@ public class GimbalControlSettings [Preferences("Zoom Speed (unitless)", ControlType.DecimalUpDown, Min = 0.01, Max = 1, Increment = 0.1, DecimalPlaces = 2)] public decimal ZoomSpeed { get; set; } - [Preferences("Camera FOV (deg)", ControlType.DecimalUpDown, Min = 0.01, Max = 180, Increment = 1, DecimalPlaces = 2)] - public decimal CameraFOV { get; set; } + [Preferences("Camera Horizontal FOV (deg)", ControlType.DecimalUpDown, Min = 0.01, Max = 180, Increment = 1, DecimalPlaces = 2)] + public decimal CameraHFOV { get; set; } + [Preferences("Camera Vertical FOV (deg)", ControlType.DecimalUpDown, Min = 0.01, Max = 180, Increment = 1, DecimalPlaces = 2)] + public decimal CameraVFOV { get; set; } // Boolean options [Preferences("Use FOV Reported by Camera", ControlType.Checkbox)] @@ -311,7 +313,8 @@ public GimbalControlSettings() SlewSpeedNormal = 5m; // deg/sec SlewSpeedFast = 25m; // deg/sec ZoomSpeed = 1.0m; // unitless [0, 1] - CameraFOV = 50.0m; // horizontal, degrees + CameraHFOV = 40.0m; // horizontal, degrees + CameraVFOV = 30.0m; // vertical, degrees // Default boolean options DefaultLockedMode = false; diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs index de5d9e12b5..4dff3b27d3 100644 --- a/Controls/GimbalVideoControl.cs +++ b/Controls/GimbalVideoControl.cs @@ -557,6 +557,13 @@ private void UITimer_Tick(object sender, EventArgs e) takePictureToolStripMenuItem.Enabled = selectedCamera?.CanCaptureImage ?? false; startRecordingToolStripMenuItem.Enabled = selectedCamera?.CanCaptureVideo ?? false; stopRecordingToolStripMenuItem.Enabled = selectedCamera?.CanCaptureVideo ?? false; + + if (selectedCamera != null) + { + selectedCamera.HFOV = (float)preferences.CameraHFOV; + selectedCamera.VFOV = (float)preferences.CameraVFOV; + selectedCamera.UseFOVStatus = preferences.UseFOVReportedByCamera; + } } private void yawLockToolStripMenuItem_Click(object sender, EventArgs e) diff --git a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs index cbef3e0cf7..29f99c20f0 100644 --- a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs +++ b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs @@ -106,6 +106,45 @@ public bool CanCaptureImage } } + public bool UseFOVStatus { get; set; } = true; + + public float _hfov = float.NaN; + /// + /// Horizontal field of view of the camera, in degrees. Uses the latest received value from the camera if available and `UseFOVStatus` is true. + /// + public float HFOV + { + get + { + if (!UseFOVStatus || CameraFOVStatus.hfov == float.NaN) + { + return _hfov; + } + return CameraFOVStatus.hfov; + } + set + { + _hfov = value; + } + } + + public float _vfov = float.NaN; + public float VFOV + { + get + { + if (!UseFOVStatus || CameraFOVStatus.vfov == float.NaN) + { + return _vfov; + } + return CameraFOVStatus.vfov; + } + set + { + _vfov = value; + } + } + /// /// Initializes the camera protocol by setting up message parsing and requesting initial camera information. /// @@ -384,12 +423,12 @@ public PointLatLngAlt CalculateImagePointLocation(double x, double y) var dist = camPosition.GetDistance(imagePosition); var down_elevation = Math.Atan2(height, dist); // zero means pointing level, pi/2 is straight down - down_elevation += y / 2 * CameraFOVStatus.vfov * Math.PI / 180; + down_elevation += y / 2 * VFOV * Math.PI / 180; down_elevation = Math.Max(0.0001, down_elevation); var out_distance = height * Math.Cos(down_elevation) / Math.Sin(down_elevation); out_distance = Math.Min(out_distance, 1e5); - var side_angle = x / 2 * CameraFOVStatus.hfov * Math.PI / 180; + var side_angle = x / 2 * HFOV * Math.PI / 180; var side_distance = Math.Sqrt(out_distance * out_distance + height * height) * Math.Tan(side_angle); var bearing = camPosition.GetBearing(imagePosition); @@ -408,10 +447,10 @@ public PointLatLngAlt CalculateImagePointLocation(double x, double y) private Vector3 CalculateImagePointVectorCameraFrame(double x, double y) { var vector = new Vector3(1, 0, 0); // Camera-frame vector pointing straight ahead - if (CameraFOVStatus.hfov != float.NaN && CameraFOVStatus.vfov != float.NaN && x != 0 && y != 0) + if (HFOV != float.NaN && VFOV != float.NaN && x != 0 && y != 0) { - var hfov = CameraFOVStatus.hfov * Math.PI / 180; - var vfov = CameraFOVStatus.vfov * Math.PI / 180; + var hfov = HFOV * Math.PI / 180; + var vfov = VFOV * Math.PI / 180; vector.y = Math.Tan(x * hfov / 2); // x in the image is toward the right side of the plane (positive y in camera frame) vector.z = Math.Tan(y * vfov / 2); // y in the image is down (z in camera frame) From 7f2ee4523cf94ca098ff4666a6af8e16b550c47c Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sun, 29 Sep 2024 16:42:38 +1000 Subject: [PATCH 32/45] GimbalControlSettings: implement default yaw lock setting --- Controls/GimbalControlSettingsForm.cs | 2 +- Controls/GimbalVideoControl.cs | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/Controls/GimbalControlSettingsForm.cs b/Controls/GimbalControlSettingsForm.cs index 85419c8056..e80b431acb 100644 --- a/Controls/GimbalControlSettingsForm.cs +++ b/Controls/GimbalControlSettingsForm.cs @@ -317,8 +317,8 @@ public GimbalControlSettings() CameraVFOV = 30.0m; // vertical, degrees // Default boolean options - DefaultLockedMode = false; UseFOVReportedByCamera = true; + DefaultLockedMode = false; } public GimbalControlSettings(GimbalControlSettings other) diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs index 4dff3b27d3..3e4f73bf78 100644 --- a/Controls/GimbalVideoControl.cs +++ b/Controls/GimbalVideoControl.cs @@ -101,6 +101,8 @@ public GimbalVideoControl() loadPreferences(); + yaw_lock = preferences.DefaultLockedMode; + // Register the global key handler Application.AddMessageFilter(this); From 84f30815097897ef1ca697be8a1d9b8fa19640da Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sun, 13 Oct 2024 18:51:13 +1100 Subject: [PATCH 33/45] CameraProtocol: add image tracking --- ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs | 65 +++++++++++++++++++++ 1 file changed, 65 insertions(+) diff --git a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs index 29f99c20f0..601400ed07 100644 --- a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs +++ b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs @@ -400,6 +400,71 @@ public Task SetZoomAsync(float zoom_level, MAVLink.CAMERA_ZOOM_TYPE zoom_type = ); } + /// + /// Command the camera to track a point in the image. + /// + /// x position in the image, -1 to 1 (positive right) + /// y position in the image, -1 to 1 (positive down) + /// + public Task SetTrackingPointAsync(float x, float y) + { + // Check capabilities. + if ((CameraInformation.flags & (int)MAVLink.CAMERA_CAP_FLAGS.HAS_TRACKING_POINT) == 0) + { + return Task.FromResult(false); + } + // Map -1:1 to 0:1 + x = (x + 1) / 2; + y = (y + 1) / 2; + return parent.parent.doCommandAsync( + parent.sysid, parent.compid, + MAVLink.MAV_CMD.CAMERA_TRACK_POINT, + x, y, + 0, 0, 0, 0, 0 + ); + } + + + /// + /// Command the camera to track a rectangle in the image. + /// + /// x position of one corner of the rectangle, -1 to 1 (positive right) + /// y position of one corner of the rectangle, -1 to 1 (positive down) + /// x position of the other corner of the rectangle, -1 to 1 (positive right) + /// y position of the other corner of the rectangle, -1 to 1 (positive down) + /// + public Task SetTrackingRectangleAsync(float x1, float y1, float x2, float y2) + { + // Check capabilities. + if ((CameraInformation.flags & (int)MAVLink.CAMERA_CAP_FLAGS.HAS_TRACKING_RECTANGLE) == 0) + { + return Task.FromResult(false); + } + + // Map -1:1 to 0:1 + x1 = (x1 + 1) / 2; + y1 = (y1 + 1) / 2; + x2 = (x2 + 1) / 2; + y2 = (y2 + 1) / 2; + + // Ensure x1 < x2 and y1 < y2 + if (x1 > x2) + { + (x2, x1) = (x1, x2); + } + if (y1 > y2) + { + (y2, y1) = (y1, y2); + } + + return parent.parent.doCommandAsync( + parent.sysid, parent.compid, + MAVLink.MAV_CMD.CAMERA_TRACK_RECTANGLE, + x1, y1, x2, y2, + 0, 0, 0 + ); + } + /// /// Calculate the lat/lon/alt-msl of a point in the image, given its x/y position in the image. /// x position in the image, -1 to 1 (positive right) From e5c5f1db8e4ab6c9f8496472192091d302ce739f Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sun, 13 Oct 2024 18:51:55 +1100 Subject: [PATCH 34/45] GimbalVideoControl: add image tracking --- Controls/GimbalVideoControl.cs | 45 ++++++++++++++++++++++++++++------ 1 file changed, 37 insertions(+), 8 deletions(-) diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs index 3e4f73bf78..a890a3d8e7 100644 --- a/Controls/GimbalVideoControl.cs +++ b/Controls/GimbalVideoControl.cs @@ -451,6 +451,8 @@ private void Home() } private DateTime lastMouseMove = DateTime.MinValue; + private (double x, double y)? dragStartPoint = null; + private (double x, double y)? dragEndPoint = null; private void VideoBox_MouseMove(object sender, MouseEventArgs e) { if(DateTime.Now > lastMouseMove.AddMilliseconds(100)) @@ -472,11 +474,27 @@ private void VideoBox_MouseMove(object sender, MouseEventArgs e) ); } } + + if ((Control.ModifierKeys, e.Button) == preferences.TrackObjectUnderMouse) + { + if(dragStartPoint == null) + { + dragStartPoint = getMousePosition(e.X, e.Y); + } + dragEndPoint = getMousePosition(e.X, e.Y); + } + else + { + dragStartPoint = null; + dragEndPoint = null; + } } private void VideoBox_MouseLeave(object sender, EventArgs e) { mouseMapMarker.Markers.Clear(); + dragStartPoint = null; + dragEndPoint = null; } private void VideoBox_Click(object sender, EventArgs e) @@ -517,6 +535,23 @@ private void VideoBox_Click(object sender, EventArgs e) } selectedGimbalManager?.SetROILocationAsync(loc.Lat, loc.Lng, loc.Alt, frame: MAV_FRAME.GLOBAL); } + else if ((Control.ModifierKeys, me.Button) == preferences.TrackObjectUnderMouse) + { + var x = (float)point.Value.x; + var y = (float)point.Value.y; + if (dragStartPoint.HasValue) + { + var start_x = (float)dragStartPoint.Value.x; + var start_y = (float)dragStartPoint.Value.y; + selectedCamera?.SetTrackingRectangleAsync( + start_x, start_y, x, y + ); + } + else + { + selectedCamera?.SetTrackingPointAsync(x, y); + } + } } private (double x, double y)? getMousePosition(int x, int y) @@ -533,19 +568,13 @@ private void VideoBox_Click(object sender, EventArgs e) { x -= (VideoBox.Width - imageWidth) / 2; x *= VideoBox.Width / imageWidth; - if (x < 0 || x >= imageWidth) - { - return null; - } + x = Math.Max(0, Math.Min(VideoBox.Width, x)); } if (imageHeight < VideoBox.Height) { y -= (VideoBox.Height - imageHeight) / 2; y *= VideoBox.Height / imageHeight; - if (y < 0 || y >= imageHeight) - { - return null; - } + y = Math.Max(0, Math.Min(VideoBox.Height, y)); } return (2 * x / (double)imageWidth - 1, 2 * y / (double)imageHeight - 1); From f9c1c815fff5bbe1b8476a599af905195e476a08 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sun, 13 Oct 2024 18:52:38 +1100 Subject: [PATCH 35/45] CameraProtocol: add tracking feedback --- ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs | 26 +++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs index 601400ed07..8146c35d17 100644 --- a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs +++ b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs @@ -29,6 +29,7 @@ public class CameraProtocol public MAVLink.mavlink_camera_settings_t CameraSettings { get; private set; } public MAVLink.mavlink_camera_capture_status_t CameraCaptureStatus { get; private set; } public MAVLink.mavlink_camera_fov_status_t CameraFOVStatus { get; private set; } + public MAVLink.mavlink_camera_tracking_image_status_t CameraTrackingImageStatus { get; private set; } public static ConcurrentDictionary<(byte, byte, byte), MAVLink.mavlink_video_stream_information_t> VideoStreams { get; private set; } = new ConcurrentDictionary<(byte, byte, byte), MAVLink.mavlink_video_stream_information_t>(); @@ -231,6 +232,9 @@ public void ParseMessages(object sender, MAVLink.MAVLinkMessage message) case MAVLink.MAVLINK_MSG_ID.CAMERA_FOV_STATUS: CameraFOVStatus = (MAVLink.mavlink_camera_fov_status_t)message.data; break; + case MAVLink.MAVLINK_MSG_ID.CAMERA_TRACKING_IMAGE_STATUS: + CameraTrackingImageStatus = (MAVLink.mavlink_camera_tracking_image_status_t)message.data; + break; } } @@ -298,6 +302,28 @@ await parent.parent.doCommandAsync( } } + public void RequestTrackingMessageInterval(int ratehz) + { + if (parent?.parent == null) + { + return; + } + + float interval_us = (float)(1e6 / ratehz); + + Task.Run(async () => + { + await parent.parent.doCommandAsync( + parent.sysid, parent.compid, + MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL, + (float)MAVLink.MAVLINK_MSG_ID.CAMERA_TRACKING_IMAGE_STATUS, + interval_us, + 0, 0, 0, 0, 0, + false // Don't wait for response + ).ConfigureAwait(false); + }); + } + /// /// Sends command to capture one image. /// From e26b323503cc7d516d9e88e130088d2994d8fdf1 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sun, 13 Oct 2024 18:53:22 +1100 Subject: [PATCH 36/45] GimbalVideoControl: draw tracking feedback --- Controls/GimbalVideoControl.cs | 53 +++++++++++++++++++++++++++++++++- 1 file changed, 52 insertions(+), 1 deletion(-) diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs index a890a3d8e7..5bb5e6609b 100644 --- a/Controls/GimbalVideoControl.cs +++ b/Controls/GimbalVideoControl.cs @@ -174,7 +174,57 @@ private void RenderFrame(object sender, MPBitmap image) VideoBox.Image = new Bitmap( image.Width, image.Height, 4 * image.Width, PixelFormat.Format32bppPArgb, - image.LockBits(Rectangle.Empty, null, SKColorType.Bgra8888).Scan0); + image.LockBits(Rectangle.Empty, null, SKColorType.Bgra8888).Scan0 + ); + + // Overlay tracking info + var tracking_status = selectedCamera?.CameraTrackingImageStatus ?? new mavlink_camera_tracking_image_status_t(); + if (dragStartPoint.HasValue && dragEndPoint.HasValue) + { + var start = dragStartPoint.Value; + var end = dragEndPoint.Value; + using (var g = Graphics.FromImage(VideoBox.Image)) + { + var x = (float)(Math.Min(start.x, end.x) + 1) * VideoBox.Image.Width / 2; + var y = (float)(Math.Min(start.y, end.y) + 1) * VideoBox.Image.Height / 2; + var w = (float)Math.Abs(start.x - end.x) * VideoBox.Image.Width / 2; + var h = (float)Math.Abs(start.y - end.y) * VideoBox.Image.Height / 2; + g.DrawRectangle(Pens.Red, x, y, w, h); + } + } + else if (tracking_status.tracking_status == (byte)MAVLink.CAMERA_TRACKING_STATUS_FLAGS.ACTIVE && + (tracking_status.target_data & (byte)MAVLink.CAMERA_TRACKING_TARGET_DATA.RENDERED) == 0 && // Don't render if the target is already rendered + (tracking_status.target_data & (byte)MAVLink.CAMERA_TRACKING_TARGET_DATA.IN_STATUS) != 0) // Only render if this status message contains the target data + { + if (tracking_status.tracking_mode == (byte)MAVLink.CAMERA_TRACKING_MODE.POINT && + !float.IsNaN(tracking_status.point_x) && + !float.IsNaN(tracking_status.point_y)) + { + var x = tracking_status.point_x * VideoBox.Image.Width; + var y = tracking_status.point_y * VideoBox.Image.Height; + var size = float.IsNaN(tracking_status.radius) ? 10 : tracking_status.radius; + using (var g = Graphics.FromImage(VideoBox.Image)) + { + g.DrawEllipse(Pens.Red, (int)x - size / 2, (int)y - size / 2, size, size); + } + } + else if (tracking_status.tracking_mode == (byte)MAVLink.CAMERA_TRACKING_MODE.RECTANGLE && + !float.IsNaN(tracking_status.rec_top_x) && + !float.IsNaN(tracking_status.rec_top_y) && + !float.IsNaN(tracking_status.rec_bottom_x) && + !float.IsNaN(tracking_status.rec_bottom_y)) + { + var x = (float)(Math.Min(tracking_status.rec_top_x, tracking_status.rec_bottom_x)) * VideoBox.Image.Width; + var y = (float)(Math.Min(tracking_status.rec_top_y, tracking_status.rec_bottom_y)) * VideoBox.Image.Height; + var w = (float)Math.Abs(tracking_status.rec_top_x - tracking_status.rec_bottom_x) * VideoBox.Image.Width; + var h = (float)Math.Abs(tracking_status.rec_top_y - tracking_status.rec_bottom_y) * VideoBox.Image.Height; + using (var g = Graphics.FromImage(VideoBox.Image)) + { + g.DrawRectangle(Pens.Red, x, y, w, h); + } + } + } + old?.Dispose(); } @@ -537,6 +587,7 @@ private void VideoBox_Click(object sender, EventArgs e) } else if ((Control.ModifierKeys, me.Button) == preferences.TrackObjectUnderMouse) { + selectedCamera?.RequestTrackingMessageInterval(5); var x = (float)point.Value.x; var y = (float)point.Value.y; if (dragStartPoint.HasValue) From 9b44846d2fe5cddd7f14582251b53753012d00b2 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Mon, 21 Oct 2024 22:20:11 +1100 Subject: [PATCH 37/45] FlightData: add GimbalVideoControl to map --- Controls/GimbalVideoControl.Designer.cs | 7 +- Controls/GimbalVideoControl.cs | 15 +- GCSViews/FlightData.Designer.cs | 353 +++++++++++++-------- GCSViews/FlightData.cs | 174 ++++++++++- GCSViews/FlightData.resx | 398 +++++++++++++----------- 5 files changed, 623 insertions(+), 324 deletions(-) diff --git a/Controls/GimbalVideoControl.Designer.cs b/Controls/GimbalVideoControl.Designer.cs index 21c7a3f8c9..a98e601445 100644 --- a/Controls/GimbalVideoControl.Designer.cs +++ b/Controls/GimbalVideoControl.Designer.cs @@ -72,7 +72,7 @@ private void InitializeComponent() this.toolStripMenuItem2, this.settingsToolStripMenuItem}); this.VideoBoxContextMenu.Name = "VideoBoxContextMenu"; - this.VideoBoxContextMenu.Size = new System.Drawing.Size(181, 264); + this.VideoBoxContextMenu.Size = new System.Drawing.Size(156, 242); // // videoStreamToolStripMenuItem // @@ -180,10 +180,7 @@ private void InitializeComponent() } #endregion - - private System.Windows.Forms.PictureBox VideoBox; private System.Windows.Forms.ToolTip ControlInfoTooltip; - private System.Windows.Forms.ContextMenuStrip VideoBoxContextMenu; private System.Windows.Forms.ToolStripMenuItem videoStreamToolStripMenuItem; private System.Windows.Forms.ToolStripMenuItem retractToolStripMenuItem; private System.Windows.Forms.ToolStripMenuItem neutralToolStripMenuItem; @@ -198,5 +195,7 @@ private void InitializeComponent() private System.Windows.Forms.ToolStripMenuItem takePictureToolStripMenuItem; private System.Windows.Forms.ToolStripMenuItem startRecordingToolStripMenuItem; private System.Windows.Forms.ToolStripMenuItem stopRecordingToolStripMenuItem; + public System.Windows.Forms.ContextMenuStrip VideoBoxContextMenu; + public System.Windows.Forms.PictureBox VideoBox; } } diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs index 5bb5e6609b..297ddfa5b8 100644 --- a/Controls/GimbalVideoControl.cs +++ b/Controls/GimbalVideoControl.cs @@ -238,9 +238,7 @@ protected override void Dispose(bool disposing) { if (disposing) { - _stream.OnNewImage -= RenderFrame; - _stream.Stop(); - + Stop(); if (components != null) { components.Dispose(); @@ -249,6 +247,12 @@ protected override void Dispose(bool disposing) base.Dispose(disposing); } + public void Stop() + { + _stream.OnNewImage -= RenderFrame; + _stream.Stop(); + } + private void videoStreamToolStripMenuItem_Click(object sender, EventArgs e) { GStreamer.GstLaunch = GStreamer.LookForGstreamer(); @@ -276,7 +280,7 @@ private void videoStreamToolStripMenuItem_Click(object sender, EventArgs e) public bool PreFilterMessage(ref Message m) { // Don't hog the keyboard when this control doesn't have focus - if (!ContainsFocus) + if (!(Parent?.ContainsFocus ?? false)) { if(heldKeys.Count > 0) { @@ -549,6 +553,9 @@ private void VideoBox_MouseLeave(object sender, EventArgs e) private void VideoBox_Click(object sender, EventArgs e) { + // Focus the control when clicked + VideoBox.Focus(); + MouseEventArgs me = (MouseEventArgs)e; var point = getMousePosition(me.X, me.Y); if (!point.HasValue) diff --git a/GCSViews/FlightData.Designer.cs b/GCSViews/FlightData.Designer.cs index 557eb60268..3eeea28fc8 100644 --- a/GCSViews/FlightData.Designer.cs +++ b/GCSViews/FlightData.Designer.cs @@ -30,10 +30,10 @@ private void InitializeComponent() this.setBatteryCellCountToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.showIconsToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.bindingSourceHud = new System.Windows.Forms.BindingSource(this.components); - this.tabControlactions = new System.Windows.Forms.TabControl(); this.contextMenuStripactionstab = new System.Windows.Forms.ContextMenuStrip(this.components); this.customizeToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.multiLineToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.tabControlactions = new System.Windows.Forms.TabControl(); this.tabQuick = new System.Windows.Forms.TabPage(); this.tableLayoutPanelQuick = new System.Windows.Forms.TableLayoutPanel(); this.quickView6 = new MissionPlanner.Controls.QuickView(); @@ -211,8 +211,16 @@ private void InitializeComponent() this.setHomeHereToolStripMenuItem1 = new System.Windows.Forms.ToolStripMenuItem(); this.takeOffToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.onOffCameraOverlapToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.jumpToTagToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.gimbalVideoToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.gimbalVideoFullSizedToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.gimbalVideoMiniToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.gimbalVideoPopOutToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.label1 = new System.Windows.Forms.Label(); this.but_disablejoystick = new MissionPlanner.Controls.MyButton(); + this.Zoomlevel = new System.Windows.Forms.NumericUpDown(); this.distanceBar1 = new MissionPlanner.Controls.DistanceBar(); + this.TRK_zoom = new MissionPlanner.Controls.MyTrackBar(); this.windDir1 = new MissionPlanner.Controls.WindDir(); this.bindingSource1 = new System.Windows.Forms.BindingSource(this.components); this.label6 = new System.Windows.Forms.Label(); @@ -222,11 +230,8 @@ private void InitializeComponent() this.lbl_hdop = new MissionPlanner.Controls.MyLabel(); this.lbl_sats = new MissionPlanner.Controls.MyLabel(); this.gMapControl1 = new MissionPlanner.Controls.myGMAP(); - this.TRK_zoom = new MissionPlanner.Controls.MyTrackBar(); this.panel1 = new System.Windows.Forms.Panel(); this.coords1 = new MissionPlanner.Controls.Coords(); - this.Zoomlevel = new System.Windows.Forms.NumericUpDown(); - this.label1 = new System.Windows.Forms.Label(); this.CHK_autopan = new System.Windows.Forms.CheckBox(); this.CB_tuning = new System.Windows.Forms.CheckBox(); this.ZedGraphTimer = new System.Windows.Forms.Timer(this.components); @@ -235,7 +240,6 @@ private void InitializeComponent() this.scriptChecker = new System.Windows.Forms.Timer(this.components); this.Messagetabtimer = new System.Windows.Forms.Timer(this.components); this.bindingSourceStatusTab = new System.Windows.Forms.BindingSource(this.components); - this.jumpToTagToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); ((System.ComponentModel.ISupportInitialize)(this.MainH)).BeginInit(); this.MainH.Panel1.SuspendLayout(); this.MainH.Panel2.SuspendLayout(); @@ -246,8 +250,8 @@ private void InitializeComponent() this.SubMainLeft.SuspendLayout(); this.contextMenuStripHud.SuspendLayout(); ((System.ComponentModel.ISupportInitialize)(this.bindingSourceHud)).BeginInit(); - this.tabControlactions.SuspendLayout(); this.contextMenuStripactionstab.SuspendLayout(); + this.tabControlactions.SuspendLayout(); this.tabQuick.SuspendLayout(); this.tableLayoutPanelQuick.SuspendLayout(); this.contextMenuStripQuickView.SuspendLayout(); @@ -286,10 +290,10 @@ private void InitializeComponent() this.splitContainer1.Panel2.SuspendLayout(); this.splitContainer1.SuspendLayout(); this.contextMenuStripMap.SuspendLayout(); - ((System.ComponentModel.ISupportInitialize)(this.bindingSource1)).BeginInit(); + ((System.ComponentModel.ISupportInitialize)(this.Zoomlevel)).BeginInit(); ((System.ComponentModel.ISupportInitialize)(this.TRK_zoom)).BeginInit(); + ((System.ComponentModel.ISupportInitialize)(this.bindingSource1)).BeginInit(); this.panel1.SuspendLayout(); - ((System.ComponentModel.ISupportInitialize)(this.Zoomlevel)).BeginInit(); ((System.ComponentModel.ISupportInitialize)(this.bindingSourceStatusTab)).BeginInit(); this.SuspendLayout(); // @@ -334,6 +338,7 @@ private void InitializeComponent() this.hud1.batterycellcount = 4; this.hud1.batterylevel = 0F; this.hud1.batterylevel2 = 0F; + this.hud1.batteryon2 = true; this.hud1.batteryremaining = 0F; this.hud1.batteryremaining2 = 0F; this.hud1.bgimage = null; @@ -344,52 +349,52 @@ private void InitializeComponent() this.hud1.critSSA = 30F; this.hud1.current = 0F; this.hud1.current2 = 0F; - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("airspeed", this.bindingSourceHud, "airspeed", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("alt", this.bindingSourceHud, "alt", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("load", this.bindingSourceHud, "load", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("batterylevel", this.bindingSourceHud, "battery_voltage", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("batteryremaining", this.bindingSourceHud, "battery_remaining", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("connected", this.bindingSourceHud, "connected", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("current", this.bindingSourceHud, "current", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("batterylevel2", this.bindingSourceHud, "battery_voltage2", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("batteryremaining2", this.bindingSourceHud, "battery_remaining2", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("current2", this.bindingSourceHud, "current2", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("datetime", this.bindingSourceHud, "datetime", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("disttowp", this.bindingSourceHud, "wp_dist", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("ekfstatus", this.bindingSourceHud, "ekfstatus", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("failsafe", this.bindingSourceHud, "failsafe", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("gpsfix", this.bindingSourceHud, "gpsstatus", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("gpsfix2", this.bindingSourceHud, "gpsstatus2", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("gpshdop", this.bindingSourceHud, "gpshdop", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("gpshdop2", this.bindingSourceHud, "gpshdop2", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("groundalt", this.bindingSourceHud, "HomeAlt", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("groundcourse", this.bindingSourceHud, "groundcourse", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("groundspeed", this.bindingSourceHud, "groundspeed", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("heading", this.bindingSourceHud, "yaw", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("linkqualitygcs", this.bindingSourceHud, "linkqualitygcs", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("message", this.bindingSourceHud, "messageHigh", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("messageSeverity", this.bindingSourceHud, "messageHighSeverity", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("mode", this.bindingSourceHud, "mode", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("navpitch", this.bindingSourceHud, "nav_pitch", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("navroll", this.bindingSourceHud, "nav_roll", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("pitch", this.bindingSourceHud, "pitch", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("prearmstatus", this.bindingSourceHud, "prearmstatus", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("roll", this.bindingSourceHud, "roll", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("safetyactive", this.bindingSourceHud, "safetyactive", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("status", this.bindingSourceHud, "armed", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("targetalt", this.bindingSourceHud, "targetalt", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("targetheading", this.bindingSourceHud, "nav_bearing", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("targetspeed", this.bindingSourceHud, "targetairspeed", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("turnrate", this.bindingSourceHud, "turnrate", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("verticalspeed", this.bindingSourceHud, "verticalspeed", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("vibex", this.bindingSourceHud, "vibex", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("vibey", this.bindingSourceHud, "vibey", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("vibez", this.bindingSourceHud, "vibez", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("wpno", this.bindingSourceHud, "wpno", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("xtrack_error", this.bindingSourceHud, "xtrack_error", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("AOA", this.bindingSourceHud, "AOA", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("SSA", this.bindingSourceHud, "SSA", false)); - this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("critAOA", this.bindingSourceHud, "crit_AOA", false)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("airspeed", this.bindingSourceHud, "airspeed", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("alt", this.bindingSourceHud, "alt", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("load", this.bindingSourceHud, "load", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("batterylevel", this.bindingSourceHud, "battery_voltage", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("batteryremaining", this.bindingSourceHud, "battery_remaining", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("connected", this.bindingSourceHud, "connected", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("current", this.bindingSourceHud, "current", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("batterylevel2", this.bindingSourceHud, "battery_voltage2", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("batteryremaining2", this.bindingSourceHud, "battery_remaining2", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("current2", this.bindingSourceHud, "current2", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("datetime", this.bindingSourceHud, "datetime", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("disttowp", this.bindingSourceHud, "wp_dist", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("ekfstatus", this.bindingSourceHud, "ekfstatus", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("failsafe", this.bindingSourceHud, "failsafe", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("gpsfix", this.bindingSourceHud, "gpsstatus", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("gpsfix2", this.bindingSourceHud, "gpsstatus2", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("gpshdop", this.bindingSourceHud, "gpshdop", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("gpshdop2", this.bindingSourceHud, "gpshdop2", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("groundalt", this.bindingSourceHud, "HomeAlt", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("groundcourse", this.bindingSourceHud, "groundcourse", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("groundspeed", this.bindingSourceHud, "groundspeed", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("heading", this.bindingSourceHud, "yaw", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("linkqualitygcs", this.bindingSourceHud, "linkqualitygcs", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("message", this.bindingSourceHud, "messageHigh", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("messageSeverity", this.bindingSourceHud, "messageHighSeverity", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("mode", this.bindingSourceHud, "mode", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("navpitch", this.bindingSourceHud, "nav_pitch", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("navroll", this.bindingSourceHud, "nav_roll", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("pitch", this.bindingSourceHud, "pitch", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("prearmstatus", this.bindingSourceHud, "prearmstatus", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("roll", this.bindingSourceHud, "roll", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("safetyactive", this.bindingSourceHud, "safetyactive", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("status", this.bindingSourceHud, "armed", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("targetalt", this.bindingSourceHud, "targetalt", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("targetheading", this.bindingSourceHud, "nav_bearing", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("targetspeed", this.bindingSourceHud, "targetairspeed", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("turnrate", this.bindingSourceHud, "turnrate", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("verticalspeed", this.bindingSourceHud, "verticalspeed", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("vibex", this.bindingSourceHud, "vibex", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("vibey", this.bindingSourceHud, "vibey", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("vibez", this.bindingSourceHud, "vibez", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("wpno", this.bindingSourceHud, "wpno", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("xtrack_error", this.bindingSourceHud, "xtrack_error", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("AOA", this.bindingSourceHud, "AOA", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("SSA", this.bindingSourceHud, "SSA", true)); + this.hud1.DataBindings.Add(new System.Windows.Forms.Binding("critAOA", this.bindingSourceHud, "crit_AOA", true)); this.hud1.datetime = new System.DateTime(((long)(0))); this.hud1.displayAOASSA = false; this.hud1.displayCellVoltage = false; @@ -409,6 +414,7 @@ private void InitializeComponent() this.hud1.heading = 0F; this.hud1.hudcolor = System.Drawing.Color.LightGray; this.hud1.linkqualitygcs = 0F; + this.hud1.load = 0F; this.hud1.lowairspeed = false; this.hud1.lowgroundspeed = false; this.hud1.lowvoltagealert = false; @@ -422,6 +428,7 @@ private void InitializeComponent() this.hud1.prearmstatus = false; this.hud1.roll = 0F; this.hud1.Russian = false; + this.hud1.safetyactive = false; this.hud1.skyColor1 = System.Drawing.Color.Blue; this.hud1.skyColor2 = System.Drawing.Color.LightBlue; this.hud1.speedunit = null; @@ -561,6 +568,26 @@ private void InitializeComponent() // this.bindingSourceHud.DataSource = typeof(MissionPlanner.CurrentState); // + // contextMenuStripactionstab + // + this.contextMenuStripactionstab.Items.AddRange(new System.Windows.Forms.ToolStripItem[] { + this.customizeToolStripMenuItem, + this.multiLineToolStripMenuItem}); + this.contextMenuStripactionstab.Name = "contextMenuStripactionstab"; + resources.ApplyResources(this.contextMenuStripactionstab, "contextMenuStripactionstab"); + // + // customizeToolStripMenuItem + // + this.customizeToolStripMenuItem.Name = "customizeToolStripMenuItem"; + resources.ApplyResources(this.customizeToolStripMenuItem, "customizeToolStripMenuItem"); + this.customizeToolStripMenuItem.Click += new System.EventHandler(this.customizeToolStripMenuItem_Click); + // + // multiLineToolStripMenuItem + // + this.multiLineToolStripMenuItem.Name = "multiLineToolStripMenuItem"; + resources.ApplyResources(this.multiLineToolStripMenuItem, "multiLineToolStripMenuItem"); + this.multiLineToolStripMenuItem.Click += new System.EventHandler(this.multiLineToolStripMenuItem_Click); + // // tabControlactions // this.tabControlactions.ContextMenuStrip = this.contextMenuStripactionstab; @@ -584,26 +611,6 @@ private void InitializeComponent() this.tabControlactions.DrawItem += new System.Windows.Forms.DrawItemEventHandler(this.tabControl1_DrawItem); this.tabControlactions.SelectedIndexChanged += new System.EventHandler(this.tabControl1_SelectedIndexChanged); // - // contextMenuStripactionstab - // - this.contextMenuStripactionstab.Items.AddRange(new System.Windows.Forms.ToolStripItem[] { - this.customizeToolStripMenuItem, - this.multiLineToolStripMenuItem}); - this.contextMenuStripactionstab.Name = "contextMenuStripactionstab"; - resources.ApplyResources(this.contextMenuStripactionstab, "contextMenuStripactionstab"); - // - // customizeToolStripMenuItem - // - this.customizeToolStripMenuItem.Name = "customizeToolStripMenuItem"; - resources.ApplyResources(this.customizeToolStripMenuItem, "customizeToolStripMenuItem"); - this.customizeToolStripMenuItem.Click += new System.EventHandler(this.customizeToolStripMenuItem_Click); - // - // multiLineToolStripMenuItem - // - this.multiLineToolStripMenuItem.Name = "multiLineToolStripMenuItem"; - resources.ApplyResources(this.multiLineToolStripMenuItem, "multiLineToolStripMenuItem"); - this.multiLineToolStripMenuItem.Click += new System.EventHandler(this.multiLineToolStripMenuItem_Click); - // // tabQuick // resources.ApplyResources(this.tabQuick, "tabQuick"); @@ -768,6 +775,7 @@ private void InitializeComponent() this.BUT_SendMSG.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_SendMSG, "BUT_SendMSG"); this.BUT_SendMSG.Name = "BUT_SendMSG"; + this.BUT_SendMSG.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.toolTip1.SetToolTip(this.BUT_SendMSG, resources.GetString("BUT_SendMSG.ToolTip")); this.BUT_SendMSG.UseVisualStyleBackColor = true; this.BUT_SendMSG.Click += new System.EventHandler(this.BUT_SendMSG_Click); @@ -779,6 +787,7 @@ private void InitializeComponent() this.BUT_abortland.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_abortland, "BUT_abortland"); this.BUT_abortland.Name = "BUT_abortland"; + this.BUT_abortland.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.toolTip1.SetToolTip(this.BUT_abortland, resources.GetString("BUT_abortland.ToolTip")); this.BUT_abortland.UseVisualStyleBackColor = true; this.BUT_abortland.Click += new System.EventHandler(this.BUT_abortland_Click); @@ -818,6 +827,7 @@ private void InitializeComponent() this.BUT_clear_track.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_clear_track, "BUT_clear_track"); this.BUT_clear_track.Name = "BUT_clear_track"; + this.BUT_clear_track.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.toolTip1.SetToolTip(this.BUT_clear_track, resources.GetString("BUT_clear_track.ToolTip")); this.BUT_clear_track.UseVisualStyleBackColor = true; this.BUT_clear_track.Click += new System.EventHandler(this.BUT_clear_track_Click); @@ -837,6 +847,7 @@ private void InitializeComponent() this.BUTactiondo.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUTactiondo, "BUTactiondo"); this.BUTactiondo.Name = "BUTactiondo"; + this.BUTactiondo.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.toolTip1.SetToolTip(this.BUTactiondo, resources.GetString("BUTactiondo.ToolTip")); this.BUTactiondo.UseVisualStyleBackColor = true; this.BUTactiondo.Click += new System.EventHandler(this.BUTactiondo_Click); @@ -848,6 +859,7 @@ private void InitializeComponent() this.BUT_resumemis.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_resumemis, "BUT_resumemis"); this.BUT_resumemis.Name = "BUT_resumemis"; + this.BUT_resumemis.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_resumemis.UseVisualStyleBackColor = true; this.BUT_resumemis.Click += new System.EventHandler(this.BUT_resumemis_Click); // @@ -926,6 +938,7 @@ private void InitializeComponent() this.BUT_ARM.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_ARM, "BUT_ARM"); this.BUT_ARM.Name = "BUT_ARM"; + this.BUT_ARM.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.toolTip1.SetToolTip(this.BUT_ARM, resources.GetString("BUT_ARM.ToolTip")); this.BUT_ARM.UseVisualStyleBackColor = true; this.BUT_ARM.Click += new System.EventHandler(this.BUT_ARM_Click); @@ -937,6 +950,7 @@ private void InitializeComponent() this.BUT_mountmode.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_mountmode, "BUT_mountmode"); this.BUT_mountmode.Name = "BUT_mountmode"; + this.BUT_mountmode.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.toolTip1.SetToolTip(this.BUT_mountmode, resources.GetString("BUT_mountmode.ToolTip")); this.BUT_mountmode.UseVisualStyleBackColor = true; this.BUT_mountmode.Click += new System.EventHandler(this.BUT_mountmode_Click); @@ -948,6 +962,7 @@ private void InitializeComponent() this.BUT_joystick.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_joystick, "BUT_joystick"); this.BUT_joystick.Name = "BUT_joystick"; + this.BUT_joystick.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.toolTip1.SetToolTip(this.BUT_joystick, resources.GetString("BUT_joystick.ToolTip")); this.BUT_joystick.UseVisualStyleBackColor = true; this.BUT_joystick.Click += new System.EventHandler(this.BUT_joystick_Click); @@ -959,6 +974,7 @@ private void InitializeComponent() this.BUT_RAWSensor.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_RAWSensor, "BUT_RAWSensor"); this.BUT_RAWSensor.Name = "BUT_RAWSensor"; + this.BUT_RAWSensor.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.toolTip1.SetToolTip(this.BUT_RAWSensor, resources.GetString("BUT_RAWSensor.ToolTip")); this.BUT_RAWSensor.UseVisualStyleBackColor = true; this.BUT_RAWSensor.Click += new System.EventHandler(this.BUT_RAWSensor_Click); @@ -970,6 +986,7 @@ private void InitializeComponent() this.BUT_Homealt.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_Homealt, "BUT_Homealt"); this.BUT_Homealt.Name = "BUT_Homealt"; + this.BUT_Homealt.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.toolTip1.SetToolTip(this.BUT_Homealt, resources.GetString("BUT_Homealt.ToolTip")); this.BUT_Homealt.UseVisualStyleBackColor = true; this.BUT_Homealt.Click += new System.EventHandler(this.BUT_Homealt_Click); @@ -981,6 +998,7 @@ private void InitializeComponent() this.BUTrestartmission.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUTrestartmission, "BUTrestartmission"); this.BUTrestartmission.Name = "BUTrestartmission"; + this.BUTrestartmission.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.toolTip1.SetToolTip(this.BUTrestartmission, resources.GetString("BUTrestartmission.ToolTip")); this.BUTrestartmission.UseVisualStyleBackColor = true; this.BUTrestartmission.Click += new System.EventHandler(this.BUTrestartmission_Click); @@ -1000,6 +1018,7 @@ private void InitializeComponent() this.BUT_quickrtl.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_quickrtl, "BUT_quickrtl"); this.BUT_quickrtl.Name = "BUT_quickrtl"; + this.BUT_quickrtl.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.toolTip1.SetToolTip(this.BUT_quickrtl, resources.GetString("BUT_quickrtl.ToolTip")); this.BUT_quickrtl.UseVisualStyleBackColor = true; this.BUT_quickrtl.Click += new System.EventHandler(this.BUT_quickrtl_Click); @@ -1011,6 +1030,7 @@ private void InitializeComponent() this.BUT_quickmanual.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_quickmanual, "BUT_quickmanual"); this.BUT_quickmanual.Name = "BUT_quickmanual"; + this.BUT_quickmanual.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.toolTip1.SetToolTip(this.BUT_quickmanual, resources.GetString("BUT_quickmanual.ToolTip")); this.BUT_quickmanual.UseVisualStyleBackColor = true; this.BUT_quickmanual.Click += new System.EventHandler(this.BUT_quickmanual_Click); @@ -1022,6 +1042,7 @@ private void InitializeComponent() this.BUT_setwp.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_setwp, "BUT_setwp"); this.BUT_setwp.Name = "BUT_setwp"; + this.BUT_setwp.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.toolTip1.SetToolTip(this.BUT_setwp, resources.GetString("BUT_setwp.ToolTip")); this.BUT_setwp.UseVisualStyleBackColor = true; this.BUT_setwp.Click += new System.EventHandler(this.BUT_setwp_Click); @@ -1042,6 +1063,7 @@ private void InitializeComponent() this.BUT_quickauto.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_quickauto, "BUT_quickauto"); this.BUT_quickauto.Name = "BUT_quickauto"; + this.BUT_quickauto.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.toolTip1.SetToolTip(this.BUT_quickauto, resources.GetString("BUT_quickauto.ToolTip")); this.BUT_quickauto.UseVisualStyleBackColor = true; this.BUT_quickauto.Click += new System.EventHandler(this.BUT_quickauto_Click); @@ -1053,6 +1075,7 @@ private void InitializeComponent() this.BUT_setmode.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_setmode, "BUT_setmode"); this.BUT_setmode.Name = "BUT_setmode"; + this.BUT_setmode.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.toolTip1.SetToolTip(this.BUT_setmode, resources.GetString("BUT_setmode.ToolTip")); this.BUT_setmode.UseVisualStyleBackColor = true; this.BUT_setmode.Click += new System.EventHandler(this.BUT_setmode_Click); @@ -1085,6 +1108,7 @@ private void InitializeComponent() this.myButton1.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.myButton1, "myButton1"); this.myButton1.Name = "myButton1"; + this.myButton1.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.toolTip1.SetToolTip(this.myButton1, resources.GetString("myButton1.ToolTip")); this.myButton1.UseVisualStyleBackColor = true; this.myButton1.Click += new System.EventHandler(this.BUT_quickmanual_Click); @@ -1096,6 +1120,7 @@ private void InitializeComponent() this.myButton2.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.myButton2, "myButton2"); this.myButton2.Name = "myButton2"; + this.myButton2.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.toolTip1.SetToolTip(this.myButton2, resources.GetString("myButton2.ToolTip")); this.myButton2.UseVisualStyleBackColor = true; this.myButton2.Click += new System.EventHandler(this.BUT_quickrtl_Click); @@ -1107,6 +1132,7 @@ private void InitializeComponent() this.myButton3.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.myButton3, "myButton3"); this.myButton3.Name = "myButton3"; + this.myButton3.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.toolTip1.SetToolTip(this.myButton3, resources.GetString("myButton3.ToolTip")); this.myButton3.UseVisualStyleBackColor = true; this.myButton3.Click += new System.EventHandler(this.BUT_quickauto_Click); @@ -2012,6 +2038,7 @@ private void InitializeComponent() this.BUT_edit_selected.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_edit_selected, "BUT_edit_selected"); this.BUT_edit_selected.Name = "BUT_edit_selected"; + this.BUT_edit_selected.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_edit_selected.UseVisualStyleBackColor = true; this.BUT_edit_selected.Click += new System.EventHandler(this.BUT_edit_selected_Click); // @@ -2027,6 +2054,7 @@ private void InitializeComponent() this.BUT_run_script.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_run_script, "BUT_run_script"); this.BUT_run_script.Name = "BUT_run_script"; + this.BUT_run_script.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_run_script.UseVisualStyleBackColor = true; this.BUT_run_script.Click += new System.EventHandler(this.BUT_run_script_Click); // @@ -2037,6 +2065,7 @@ private void InitializeComponent() this.BUT_abort_script.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_abort_script, "BUT_abort_script"); this.BUT_abort_script.Name = "BUT_abort_script"; + this.BUT_abort_script.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_abort_script.UseVisualStyleBackColor = true; this.BUT_abort_script.Click += new System.EventHandler(this.BUT_abort_script_Click); // @@ -2052,6 +2081,7 @@ private void InitializeComponent() this.BUT_select_script.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_select_script, "BUT_select_script"); this.BUT_select_script.Name = "BUT_select_script"; + this.BUT_select_script.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_select_script.UseVisualStyleBackColor = true; this.BUT_select_script.Click += new System.EventHandler(this.BUT_select_script_Click); // @@ -2070,8 +2100,9 @@ private void InitializeComponent() // resources.ApplyResources(this.BUT_GimbalVideo, "BUT_GimbalVideo"); this.BUT_GimbalVideo.Name = "BUT_GimbalVideo"; + this.BUT_GimbalVideo.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_GimbalVideo.UseVisualStyleBackColor = true; - this.BUT_GimbalVideo.Click += new System.EventHandler(this.BUT_GimbalVideo_Click); + this.BUT_GimbalVideo.Click += new System.EventHandler(this.gimbalVideoPopOutToolStripMenuItem_Click); // // groupBoxRoll // @@ -2129,6 +2160,7 @@ private void InitializeComponent() // resources.ApplyResources(this.BUT_resetGimbalPos, "BUT_resetGimbalPos"); this.BUT_resetGimbalPos.Name = "BUT_resetGimbalPos"; + this.BUT_resetGimbalPos.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_resetGimbalPos.UseVisualStyleBackColor = true; this.BUT_resetGimbalPos.Click += new System.EventHandler(this.BUT_resetGimbalPos_Click); // @@ -2203,6 +2235,7 @@ private void InitializeComponent() resources.ApplyResources(this.BUT_speed10, "BUT_speed10"); this.BUT_speed10.Name = "BUT_speed10"; this.BUT_speed10.Tag = "10"; + this.BUT_speed10.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_speed10.UseVisualStyleBackColor = true; this.BUT_speed10.Click += new System.EventHandler(this.BUT_speed1_Click); // @@ -2214,6 +2247,7 @@ private void InitializeComponent() resources.ApplyResources(this.BUT_speed5, "BUT_speed5"); this.BUT_speed5.Name = "BUT_speed5"; this.BUT_speed5.Tag = "5"; + this.BUT_speed5.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_speed5.UseVisualStyleBackColor = true; this.BUT_speed5.Click += new System.EventHandler(this.BUT_speed1_Click); // @@ -2225,6 +2259,7 @@ private void InitializeComponent() resources.ApplyResources(this.BUT_speed2, "BUT_speed2"); this.BUT_speed2.Name = "BUT_speed2"; this.BUT_speed2.Tag = "2"; + this.BUT_speed2.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_speed2.UseVisualStyleBackColor = true; this.BUT_speed2.Click += new System.EventHandler(this.BUT_speed1_Click); // @@ -2236,6 +2271,7 @@ private void InitializeComponent() resources.ApplyResources(this.BUT_speed1, "BUT_speed1"); this.BUT_speed1.Name = "BUT_speed1"; this.BUT_speed1.Tag = "1"; + this.BUT_speed1.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_speed1.UseVisualStyleBackColor = true; this.BUT_speed1.Click += new System.EventHandler(this.BUT_speed1_Click); // @@ -2247,6 +2283,7 @@ private void InitializeComponent() resources.ApplyResources(this.BUT_speed1_2, "BUT_speed1_2"); this.BUT_speed1_2.Name = "BUT_speed1_2"; this.BUT_speed1_2.Tag = "0.5"; + this.BUT_speed1_2.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_speed1_2.UseVisualStyleBackColor = true; this.BUT_speed1_2.Click += new System.EventHandler(this.BUT_speed1_Click); // @@ -2258,6 +2295,7 @@ private void InitializeComponent() resources.ApplyResources(this.BUT_speed1_4, "BUT_speed1_4"); this.BUT_speed1_4.Name = "BUT_speed1_4"; this.BUT_speed1_4.Tag = "0.25"; + this.BUT_speed1_4.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_speed1_4.UseVisualStyleBackColor = true; this.BUT_speed1_4.Click += new System.EventHandler(this.BUT_speed1_Click); // @@ -2269,6 +2307,7 @@ private void InitializeComponent() resources.ApplyResources(this.BUT_speed1_10, "BUT_speed1_10"); this.BUT_speed1_10.Name = "BUT_speed1_10"; this.BUT_speed1_10.Tag = "0.1"; + this.BUT_speed1_10.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_speed1_10.UseVisualStyleBackColor = true; this.BUT_speed1_10.Click += new System.EventHandler(this.BUT_speed1_Click); // @@ -2279,6 +2318,7 @@ private void InitializeComponent() this.BUT_loadtelem.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_loadtelem, "BUT_loadtelem"); this.BUT_loadtelem.Name = "BUT_loadtelem"; + this.BUT_loadtelem.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_loadtelem.UseVisualStyleBackColor = true; this.BUT_loadtelem.Click += new System.EventHandler(this.BUT_loadtelem_Click); // @@ -2305,6 +2345,7 @@ private void InitializeComponent() this.BUT_log2kml.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_log2kml, "BUT_log2kml"); this.BUT_log2kml.Name = "BUT_log2kml"; + this.BUT_log2kml.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_log2kml.UseVisualStyleBackColor = true; this.BUT_log2kml.Click += new System.EventHandler(this.BUT_log2kml_Click); // @@ -2315,6 +2356,7 @@ private void InitializeComponent() this.BUT_playlog.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_playlog, "BUT_playlog"); this.BUT_playlog.Name = "BUT_playlog"; + this.BUT_playlog.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_playlog.UseVisualStyleBackColor = true; this.BUT_playlog.Click += new System.EventHandler(this.BUT_playlog_Click); // @@ -2352,6 +2394,7 @@ private void InitializeComponent() this.BUT_DFMavlink.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_DFMavlink, "BUT_DFMavlink"); this.BUT_DFMavlink.Name = "BUT_DFMavlink"; + this.BUT_DFMavlink.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_DFMavlink.UseVisualStyleBackColor = true; this.BUT_DFMavlink.Click += new System.EventHandler(this.BUT_DFMavlink_Click); // @@ -2359,6 +2402,7 @@ private void InitializeComponent() // resources.ApplyResources(this.BUT_georefimage, "BUT_georefimage"); this.BUT_georefimage.Name = "BUT_georefimage"; + this.BUT_georefimage.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_georefimage.Click += new System.EventHandler(this.BUT_georefimage_Click); // // BUT_logbrowse @@ -2368,6 +2412,7 @@ private void InitializeComponent() this.BUT_logbrowse.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_logbrowse, "BUT_logbrowse"); this.BUT_logbrowse.Name = "BUT_logbrowse"; + this.BUT_logbrowse.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_logbrowse.UseVisualStyleBackColor = true; this.BUT_logbrowse.Click += new System.EventHandler(this.BUT_logbrowse_Click); // @@ -2378,6 +2423,7 @@ private void InitializeComponent() this.BUT_matlab.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_matlab, "BUT_matlab"); this.BUT_matlab.Name = "BUT_matlab"; + this.BUT_matlab.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_matlab.UseVisualStyleBackColor = true; this.BUT_matlab.Click += new System.EventHandler(this.BUT_matlab_Click); // @@ -2388,6 +2434,7 @@ private void InitializeComponent() this.but_bintolog.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.but_bintolog, "but_bintolog"); this.but_bintolog.Name = "but_bintolog"; + this.but_bintolog.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.but_bintolog.UseVisualStyleBackColor = true; this.but_bintolog.Click += new System.EventHandler(this.but_bintolog_Click); // @@ -2398,6 +2445,7 @@ private void InitializeComponent() this.but_dflogtokml.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.but_dflogtokml, "but_dflogtokml"); this.but_dflogtokml.Name = "but_dflogtokml"; + this.but_dflogtokml.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.but_dflogtokml.UseVisualStyleBackColor = true; this.but_dflogtokml.Click += new System.EventHandler(this.but_dflogtokml_Click); // @@ -2408,6 +2456,7 @@ private void InitializeComponent() this.BUT_loganalysis.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.BUT_loganalysis, "BUT_loganalysis"); this.BUT_loganalysis.Name = "BUT_loganalysis"; + this.BUT_loganalysis.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_loganalysis.UseVisualStyleBackColor = true; this.BUT_loganalysis.Click += new System.EventHandler(this.BUT_loganalysis_Click); // @@ -2448,6 +2497,7 @@ private void InitializeComponent() this.splitContainer1.Panel2.Controls.Add(this.lbl_hdop); this.splitContainer1.Panel2.Controls.Add(this.lbl_sats); this.splitContainer1.Panel2.Controls.Add(this.gMapControl1); + this.splitContainer1.Panel2.Resize += new System.EventHandler(this.splitContainer1_Panel2_Resize); // // zg1 // @@ -2476,7 +2526,8 @@ private void InitializeComponent() this.setHomeHereToolStripMenuItem, this.takeOffToolStripMenuItem, this.onOffCameraOverlapToolStripMenuItem, - this.jumpToTagToolStripMenuItem}); + this.jumpToTagToolStripMenuItem, + this.gimbalVideoToolStripMenuItem}); this.contextMenuStripMap.Name = "contextMenuStrip1"; resources.ApplyResources(this.contextMenuStripMap, "contextMenuStripMap"); // @@ -2590,6 +2641,44 @@ private void InitializeComponent() resources.ApplyResources(this.onOffCameraOverlapToolStripMenuItem, "onOffCameraOverlapToolStripMenuItem"); this.onOffCameraOverlapToolStripMenuItem.Click += new System.EventHandler(this.onOffCameraOverlapToolStripMenuItem_Click); // + // jumpToTagToolStripMenuItem + // + this.jumpToTagToolStripMenuItem.Name = "jumpToTagToolStripMenuItem"; + resources.ApplyResources(this.jumpToTagToolStripMenuItem, "jumpToTagToolStripMenuItem"); + this.jumpToTagToolStripMenuItem.Click += new System.EventHandler(this.jumpToTagToolStripMenuItem_Click); + // + // gimbalVideoToolStripMenuItem + // + this.gimbalVideoToolStripMenuItem.DropDownItems.AddRange(new System.Windows.Forms.ToolStripItem[] { + this.gimbalVideoFullSizedToolStripMenuItem, + this.gimbalVideoMiniToolStripMenuItem, + this.gimbalVideoPopOutToolStripMenuItem}); + this.gimbalVideoToolStripMenuItem.Name = "gimbalVideoToolStripMenuItem"; + resources.ApplyResources(this.gimbalVideoToolStripMenuItem, "gimbalVideoToolStripMenuItem"); + // + // gimbalVideoFullSizedToolStripMenuItem + // + this.gimbalVideoFullSizedToolStripMenuItem.Name = "gimbalVideoFullSizedToolStripMenuItem"; + resources.ApplyResources(this.gimbalVideoFullSizedToolStripMenuItem, "gimbalVideoFullSizedToolStripMenuItem"); + this.gimbalVideoFullSizedToolStripMenuItem.Click += new System.EventHandler(this.gimbalVideoFullSizedToolStripMenuItem_Click); + // + // gimbalVideoMiniToolStripMenuItem + // + this.gimbalVideoMiniToolStripMenuItem.Name = "gimbalVideoMiniToolStripMenuItem"; + resources.ApplyResources(this.gimbalVideoMiniToolStripMenuItem, "gimbalVideoMiniToolStripMenuItem"); + this.gimbalVideoMiniToolStripMenuItem.Click += new System.EventHandler(this.gimbalVideoMiniToolStripMenuItem_Click); + // + // gimbalVideoPopOutToolStripMenuItem + // + this.gimbalVideoPopOutToolStripMenuItem.Name = "gimbalVideoPopOutToolStripMenuItem"; + resources.ApplyResources(this.gimbalVideoPopOutToolStripMenuItem, "gimbalVideoPopOutToolStripMenuItem"); + this.gimbalVideoPopOutToolStripMenuItem.Click += new System.EventHandler(this.gimbalVideoPopOutToolStripMenuItem_Click); + // + // label1 + // + resources.ApplyResources(this.label1, "label1"); + this.label1.Name = "label1"; + // // but_disablejoystick // this.but_disablejoystick.ColorMouseDown = System.Drawing.Color.Empty; @@ -2597,9 +2686,38 @@ private void InitializeComponent() this.but_disablejoystick.ColorNotEnabled = System.Drawing.Color.Empty; resources.ApplyResources(this.but_disablejoystick, "but_disablejoystick"); this.but_disablejoystick.Name = "but_disablejoystick"; + this.but_disablejoystick.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.but_disablejoystick.UseVisualStyleBackColor = true; this.but_disablejoystick.Click += new System.EventHandler(this.but_disablejoystick_Click); // + // Zoomlevel + // + resources.ApplyResources(this.Zoomlevel, "Zoomlevel"); + this.Zoomlevel.DecimalPlaces = 1; + this.Zoomlevel.Increment = new decimal(new int[] { + 5, + 0, + 0, + 65536}); + this.Zoomlevel.Maximum = new decimal(new int[] { + 18, + 0, + 0, + 0}); + this.Zoomlevel.Minimum = new decimal(new int[] { + 1, + 0, + 0, + 0}); + this.Zoomlevel.Name = "Zoomlevel"; + this.toolTip1.SetToolTip(this.Zoomlevel, resources.GetString("Zoomlevel.ToolTip")); + this.Zoomlevel.Value = new decimal(new int[] { + 10, + 0, + 0, + 0}); + this.Zoomlevel.ValueChanged += new System.EventHandler(this.Zoomlevel_ValueChanged); + // // distanceBar1 // resources.ApplyResources(this.distanceBar1, "distanceBar1"); @@ -2608,11 +2726,24 @@ private void InitializeComponent() this.distanceBar1.totaldist = 100F; this.distanceBar1.traveleddist = 0F; // + // TRK_zoom + // + resources.ApplyResources(this.TRK_zoom, "TRK_zoom"); + this.TRK_zoom.LargeChange = 1F; + this.TRK_zoom.Maximum = 24F; + this.TRK_zoom.Minimum = 1F; + this.TRK_zoom.Name = "TRK_zoom"; + this.TRK_zoom.SmallChange = 1F; + this.TRK_zoom.TickFrequency = 1F; + this.TRK_zoom.TickStyle = System.Windows.Forms.TickStyle.TopLeft; + this.TRK_zoom.Value = 1F; + this.TRK_zoom.Scroll += new System.EventHandler(this.TRK_zoom_Scroll); + // // windDir1 // this.windDir1.BackColor = System.Drawing.Color.Transparent; - this.windDir1.DataBindings.Add(new System.Windows.Forms.Binding("Direction", this.bindingSource1, "wind_dir", false, System.Windows.Forms.DataSourceUpdateMode.Never)); - this.windDir1.DataBindings.Add(new System.Windows.Forms.Binding("Speed", this.bindingSource1, "wind_vel", false, System.Windows.Forms.DataSourceUpdateMode.Never)); + this.windDir1.DataBindings.Add(new System.Windows.Forms.Binding("Direction", this.bindingSource1, "wind_dir", true, System.Windows.Forms.DataSourceUpdateMode.Never)); + this.windDir1.DataBindings.Add(new System.Windows.Forms.Binding("Speed", this.bindingSource1, "wind_vel", true, System.Windows.Forms.DataSourceUpdateMode.Never)); this.windDir1.Direction = 180D; resources.ApplyResources(this.windDir1, "windDir1"); this.windDir1.Name = "windDir1"; @@ -2699,19 +2830,6 @@ private void InitializeComponent() this.gMapControl1.MouseMove += new System.Windows.Forms.MouseEventHandler(this.gMapControl1_MouseMove); this.gMapControl1.MouseUp += new System.Windows.Forms.MouseEventHandler(this.gMapControl1_MouseUp); // - // TRK_zoom - // - resources.ApplyResources(this.TRK_zoom, "TRK_zoom"); - this.TRK_zoom.LargeChange = 1F; - this.TRK_zoom.Maximum = 24F; - this.TRK_zoom.Minimum = 1F; - this.TRK_zoom.Name = "TRK_zoom"; - this.TRK_zoom.SmallChange = 1F; - this.TRK_zoom.TickFrequency = 1F; - this.TRK_zoom.TickStyle = System.Windows.Forms.TickStyle.TopLeft; - this.TRK_zoom.Value = 1F; - this.TRK_zoom.Scroll += new System.EventHandler(this.TRK_zoom_Scroll); - // // panel1 // resources.ApplyResources(this.panel1, "panel1"); @@ -2725,48 +2843,15 @@ private void InitializeComponent() this.coords1.Alt = 0D; this.coords1.AltSource = ""; this.coords1.AltUnit = "m"; - this.coords1.DataBindings.Add(new System.Windows.Forms.Binding("Alt", this.bindingSource1, "alt", false)); - this.coords1.DataBindings.Add(new System.Windows.Forms.Binding("Lat", this.bindingSource1, "lat", false)); - this.coords1.DataBindings.Add(new System.Windows.Forms.Binding("Lng", this.bindingSource1, "lng", false)); + this.coords1.DataBindings.Add(new System.Windows.Forms.Binding("Alt", this.bindingSource1, "alt", true)); + this.coords1.DataBindings.Add(new System.Windows.Forms.Binding("Lat", this.bindingSource1, "lat", true)); + this.coords1.DataBindings.Add(new System.Windows.Forms.Binding("Lng", this.bindingSource1, "lng", true)); this.coords1.Lat = 0D; this.coords1.Lng = 0D; resources.ApplyResources(this.coords1, "coords1"); this.coords1.Name = "coords1"; this.coords1.Vertical = false; // - // Zoomlevel - // - resources.ApplyResources(this.Zoomlevel, "Zoomlevel"); - this.Zoomlevel.DecimalPlaces = 1; - this.Zoomlevel.Increment = new decimal(new int[] { - 5, - 0, - 0, - 65536}); - this.Zoomlevel.Maximum = new decimal(new int[] { - 18, - 0, - 0, - 0}); - this.Zoomlevel.Minimum = new decimal(new int[] { - 1, - 0, - 0, - 0}); - this.Zoomlevel.Name = "Zoomlevel"; - this.toolTip1.SetToolTip(this.Zoomlevel, resources.GetString("Zoomlevel.ToolTip")); - this.Zoomlevel.Value = new decimal(new int[] { - 10, - 0, - 0, - 0}); - this.Zoomlevel.ValueChanged += new System.EventHandler(this.Zoomlevel_ValueChanged); - // - // label1 - // - resources.ApplyResources(this.label1, "label1"); - this.label1.Name = "label1"; - // // CHK_autopan // resources.ApplyResources(this.CHK_autopan, "CHK_autopan"); @@ -2811,12 +2896,6 @@ private void InitializeComponent() // this.bindingSourceStatusTab.DataSource = typeof(MissionPlanner.CurrentState); // - // jumpToTagToolStripMenuItem - // - this.jumpToTagToolStripMenuItem.Name = "jumpToTagToolStripMenuItem"; - resources.ApplyResources(this.jumpToTagToolStripMenuItem, "jumpToTagToolStripMenuItem"); - this.jumpToTagToolStripMenuItem.Click += new System.EventHandler(this.jumpToTagToolStripMenuItem_Click); - // // FlightData // this.Controls.Add(this.MainH); @@ -2837,8 +2916,8 @@ private void InitializeComponent() this.SubMainLeft.ResumeLayout(false); this.contextMenuStripHud.ResumeLayout(false); ((System.ComponentModel.ISupportInitialize)(this.bindingSourceHud)).EndInit(); - this.tabControlactions.ResumeLayout(false); this.contextMenuStripactionstab.ResumeLayout(false); + this.tabControlactions.ResumeLayout(false); this.tabQuick.ResumeLayout(false); this.tableLayoutPanelQuick.ResumeLayout(false); this.contextMenuStripQuickView.ResumeLayout(false); @@ -2888,9 +2967,9 @@ private void InitializeComponent() ((System.ComponentModel.ISupportInitialize)(this.splitContainer1)).EndInit(); this.splitContainer1.ResumeLayout(false); this.contextMenuStripMap.ResumeLayout(false); - ((System.ComponentModel.ISupportInitialize)(this.bindingSource1)).EndInit(); ((System.ComponentModel.ISupportInitialize)(this.Zoomlevel)).EndInit(); ((System.ComponentModel.ISupportInitialize)(this.TRK_zoom)).EndInit(); + ((System.ComponentModel.ISupportInitialize)(this.bindingSource1)).EndInit(); this.panel1.ResumeLayout(false); this.panel1.PerformLayout(); ((System.ComponentModel.ISupportInitialize)(this.bindingSourceStatusTab)).EndInit(); @@ -3127,5 +3206,9 @@ private void InitializeComponent() private Controls.RelayOptions relayOptions14; private Controls.RelayOptions relayOptions15; private Controls.RelayOptions relayOptions16; + private ToolStripMenuItem gimbalVideoToolStripMenuItem; + private ToolStripMenuItem gimbalVideoFullSizedToolStripMenuItem; + private ToolStripMenuItem gimbalVideoMiniToolStripMenuItem; + private ToolStripMenuItem gimbalVideoPopOutToolStripMenuItem; } } diff --git a/GCSViews/FlightData.cs b/GCSViews/FlightData.cs index 3e23b1bf5c..e35cf591e2 100644 --- a/GCSViews/FlightData.cs +++ b/GCSViews/FlightData.cs @@ -1860,6 +1860,9 @@ private void CB_tuning_CheckedChanged(object sender, EventArgs e) ZedGraphTimer.Stop(); zg1.Visible = false; } + + // Fire the splitContainer1_Panel2_Resize event + splitContainer1_Panel2_Resize(null, null); } private void CheckAndBindPreFlightData() @@ -6464,19 +6467,178 @@ private void jumpToTagToolStripMenuItem_Click(object sender, EventArgs e) } } - private void BUT_GimbalVideo_Click(object sender, EventArgs e) + ToolStripMenuItem gimbalVideoShowMiniMap = new ToolStripMenuItem("Mini map"); + ToolStripMenuItem gimbalVideoSwapPosition = new ToolStripMenuItem("Swap with map"); + ToolStripMenuItem gimbalVideoClose = new ToolStripMenuItem("Close"); + GimbalVideoControl _gimbalVideoControl; + GimbalVideoControl gimbalVideoControl + { + get + { + // Check if we need to construct a gimbalVideoControl + if (_gimbalVideoControl == null || _gimbalVideoControl.IsDisposed) + { + _gimbalVideoControl = new GimbalVideoControl(); + _gimbalVideoControl.Dock = DockStyle.Fill; + + // Add option to show/hide minimap + gimbalVideoShowMiniMap.CheckOnClick = true; + gimbalVideoShowMiniMap.Checked = true; + gimbalVideoShowMiniMap.CheckedChanged += (s, ev) => + { + gMapControl1.Visible = gimbalVideoShowMiniMap.Checked; + gimbalVideoSwapPosition.Visible = gimbalVideoShowMiniMap.Checked; + }; + gimbalVideoSwapPosition.Click += (s, ev) => + { + if (gimbalVideoControl.Dock == DockStyle.None) + { + gimbalVideoFullSizedToolStripMenuItem_Click(null, null); + } + else + { + gimbalVideoMiniToolStripMenuItem_Click(null, null); + } + }; + gimbalVideoClose.Click += (s, ev) => + { + gimbalVideoMiniToolStripMenuItem_Click(null, null); + gimbalVideoControl.Visible = false; + gimbalVideoControl.Stop(); + gimbalVideoControl.Dispose(); + }; + + _gimbalVideoControl.VideoBoxContextMenu.Items.Add(gimbalVideoShowMiniMap); + _gimbalVideoControl.VideoBoxContextMenu.Items.Add(gimbalVideoSwapPosition); + _gimbalVideoControl.VideoBoxContextMenu.Items.Add(gimbalVideoClose); + } + + return _gimbalVideoControl; + } + } + + // Resize the mini video or mini map when the container is resized + private void splitContainer1_Panel2_Resize(object sender, EventArgs e) { + bool miniVideo = splitContainer1.Panel2.Contains(_gimbalVideoControl) + && _gimbalVideoControl?.Dock == DockStyle.None + && _gimbalVideoControl.Visible; + bool miniMap = gMapControl1.Dock == DockStyle.None && gMapControl1.Visible; + if (miniVideo) + { + var width = (int)(splitContainer1.Panel2.Width * 0.3); + var height = (int)(splitContainer1.Panel2.Height * 0.3); + var aspectRatio = _gimbalVideoControl.VideoBox.Image.Width / (double)_gimbalVideoControl.VideoBox.Image.Height; + (width, height) = ( + Math.Min(width, (int)(height * aspectRatio)), + Math.Min(height, (int)(width / aspectRatio)) + ); + var x = splitContainer1.Panel2.Width - width - TRK_zoom.Width; + var y = splitContainer1.Panel2.Height - height; + _gimbalVideoControl.Location = new Point(x, y); + _gimbalVideoControl.Size = new Size(width, height); + } + else if (miniMap) + { + var width = (int)(splitContainer1.Panel2.Width * 0.3); + var height = (int)(splitContainer1.Panel2.Height * 0.3); + var x = splitContainer1.Panel2.Width - width; + var y = splitContainer1.Panel2.Height - height; + gMapControl1.Location = new Point(x, y); + gMapControl1.Size = new Size(width, height); + } + + Invalidate(); + } + + private void gimbalVideoFullSizedToolStripMenuItem_Click(object sender, EventArgs e) + { + // If the gimbal video is in its own window, close it + var containingForm = gimbalVideoControl.Parent as Form; + + // Fill the panel with the gimbal video control + splitContainer1.Panel2.Controls.Add(gimbalVideoControl); + gimbalVideoControl.Dock = DockStyle.Fill; + gimbalVideoControl.BringToFront(); // Place on top of all map overlay controls + gimbalVideoControl.Visible = true; + + // Add the map panel to the mini map panel + gMapControl1.Dock = DockStyle.None; + gMapControl1.BringToFront(); + gMapControl1.Visible = gimbalVideoShowMiniMap.Checked; + + // Call resize to correctly position the mini map + splitContainer1_Panel2_Resize(null, null); + + // Reconfigure context menu controls + gimbalVideoShowMiniMap.Visible = true; + gimbalVideoSwapPosition.Visible = gimbalVideoShowMiniMap.Checked; + gimbalVideoClose.Visible = true; + + containingForm?.Close(); + } + + private void gimbalVideoMiniToolStripMenuItem_Click(object sender, EventArgs e) + { + // If the gimbal video is in its own window, close it + var containingForm = gimbalVideoControl.Parent as Form; + + // Fill the panel with the map + gMapControl1.Dock = DockStyle.Fill; + gMapControl1.Visible = true; + gMapControl1.SendToBack(); // Behind the map overlay controls + + // Add the gimbal video control to the mini video panel + splitContainer1.Panel2.Controls.Add(gimbalVideoControl); + gimbalVideoControl.Dock = DockStyle.None; + gimbalVideoControl.BringToFront(); + gimbalVideoControl.Visible = true; + + // Call resize to correctly position the mini video + splitContainer1_Panel2_Resize(null, null); + + // Reconfigure context menu controls + gimbalVideoShowMiniMap.Visible = false; + gimbalVideoSwapPosition.Visible = true; + gimbalVideoClose.Visible = true; + + containingForm?.Close(); + } + + private void gimbalVideoPopOutToolStripMenuItem_Click(object sender, EventArgs e) + { + // See if the gimbal video is already in its own window + if (gimbalVideoControl.Parent is Form) + { + // Remove from the form and dispose the form + // (in case the form has ended up off screen or something) + var ParentForm = gimbalVideoControl.Parent as Form; + ParentForm.Controls.Remove(gimbalVideoControl); + ParentForm.Close(); + } + + // Restore the map to full sized if necessary + gMapControl1.Dock = DockStyle.Fill; + gMapControl1.SendToBack(); + gMapControl1.Visible = true; + var form = new Form() { Text = "Gimbal Control", Size = new Size(600, 400), StartPosition = FormStartPosition.CenterParent }; - form.Controls.Add(new GimbalVideoControl() - { - Dock = DockStyle.Fill - }); - form.Show(); + form.Controls.Add(gimbalVideoControl); + gimbalVideoControl.Dock = DockStyle.Fill; + gimbalVideoControl.Visible = true; + + // Reconfigure context menu controls + gimbalVideoShowMiniMap.Visible = false; + gimbalVideoSwapPosition.Visible = false; + gimbalVideoClose.Visible = false; + + // Pass `this` to keep the pop-out always on top + form.Show(this); } } } diff --git a/GCSViews/FlightData.resx b/GCSViews/FlightData.resx index d709f2d951..f8dddae42c 100644 --- a/GCSViews/FlightData.resx +++ b/GCSViews/FlightData.resx @@ -694,7 +694,7 @@ modifyandSetLoiterRad - MissionPlanner.Controls.ModifyandSet, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.ModifyandSet, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null tableLayoutPanel1 @@ -853,7 +853,7 @@ modifyandSetAlt - MissionPlanner.Controls.ModifyandSet, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.ModifyandSet, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null tableLayoutPanel1 @@ -889,7 +889,7 @@ modifyandSetSpeed - MissionPlanner.Controls.ModifyandSet, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.ModifyandSet, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null tableLayoutPanel1 @@ -1582,7 +1582,7 @@ checkListControl1 - MissionPlanner.Controls.PreFlight.CheckListControl, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.PreFlight.CheckListControl, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null tabPagePreFlight @@ -3071,7 +3071,7 @@ servoOptions1 - MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3092,7 +3092,7 @@ servoOptions2 - MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3113,7 +3113,7 @@ servoOptions3 - MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3134,7 +3134,7 @@ servoOptions4 - MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3155,7 +3155,7 @@ servoOptions5 - MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3164,7 +3164,7 @@ 4 - 3, 168 + 324, 3 315, 27 @@ -3176,7 +3176,7 @@ servoOptions6 - MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3185,7 +3185,7 @@ 5 - 3, 201 + 324, 36 315, 27 @@ -3197,7 +3197,7 @@ servoOptions7 - MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3206,7 +3206,7 @@ 6 - 3, 234 + 324, 69 295, 24 @@ -3218,7 +3218,7 @@ servoOptions8 - MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3227,7 +3227,7 @@ 7 - 3, 264 + 324, 99 295, 24 @@ -3239,7 +3239,7 @@ servoOptions9 - MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3248,7 +3248,7 @@ 8 - 3, 294 + 324, 129 295, 24 @@ -3260,7 +3260,7 @@ servoOptions10 - MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3269,7 +3269,7 @@ 9 - 3, 324 + 645, 3 295, 22 @@ -3281,7 +3281,7 @@ servoOptions11 - MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3290,7 +3290,7 @@ 10 - 3, 352 + 645, 31 295, 22 @@ -3302,7 +3302,7 @@ servoOptions12 - MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.ServoOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3311,7 +3311,7 @@ 11 - 3, 380 + 645, 59 198, 22 @@ -3323,7 +3323,7 @@ relayOptions1 - MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3332,7 +3332,7 @@ 12 - 3, 408 + 645, 87 198, 22 @@ -3344,7 +3344,7 @@ relayOptions2 - MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3353,7 +3353,7 @@ 13 - 3, 436 + 645, 115 198, 22 @@ -3365,7 +3365,7 @@ relayOptions3 - MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3374,7 +3374,7 @@ 14 - 3, 464 + 645, 143 198, 22 @@ -3386,7 +3386,7 @@ relayOptions4 - MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3395,7 +3395,7 @@ 15 - 3, 492 + 946, 3 295, 22 @@ -3407,7 +3407,7 @@ relayOptions5 - MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3416,7 +3416,7 @@ 16 - 3, 520 + 946, 31 295, 22 @@ -3428,7 +3428,7 @@ relayOptions6 - MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3437,7 +3437,7 @@ 17 - 3, 548 + 946, 59 295, 22 @@ -3449,7 +3449,7 @@ relayOptions7 - MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.8888.28482, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3458,7 +3458,7 @@ 18 - 3, 576 + 946, 87 295, 22 @@ -3470,7 +3470,7 @@ relayOptions8 - MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.8888.28482, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3479,7 +3479,7 @@ 19 - 3, 604 + 946, 115 295, 22 @@ -3491,7 +3491,7 @@ relayOptions9 - MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.8888.28482, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3500,7 +3500,7 @@ 20 - 3, 632 + 946, 143 295, 22 @@ -3512,7 +3512,7 @@ relayOptions10 - MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.8888.28482, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3521,7 +3521,7 @@ 21 - 3, 660 + 1247, 3 295, 22 @@ -3533,7 +3533,7 @@ relayOptions11 - MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.8888.28482, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3542,7 +3542,7 @@ 22 - 3, 688 + 1247, 31 295, 22 @@ -3554,7 +3554,7 @@ relayOptions12 - MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.8888.28482, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3563,7 +3563,7 @@ 23 - 3, 716 + 1247, 59 295, 22 @@ -3575,7 +3575,7 @@ relayOptions13 - MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.8888.28482, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3584,7 +3584,7 @@ 24 - 3, 744 + 1247, 87 295, 22 @@ -3596,7 +3596,7 @@ relayOptions14 - MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.8888.28482, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3605,7 +3605,7 @@ 25 - 3, 772 + 1247, 115 295, 22 @@ -3617,7 +3617,7 @@ relayOptions15 - MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.8888.28482, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3626,7 +3626,7 @@ 26 - 3, 800 + 1247, 143 295, 22 @@ -3638,7 +3638,7 @@ relayOptions16 - MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.8888.28482, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.RelayOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanelServos @@ -3716,7 +3716,7 @@ auxOptions1 - MissionPlanner.Controls.AuxOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.AuxOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanel1 @@ -3737,7 +3737,7 @@ auxOptions2 - MissionPlanner.Controls.AuxOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.AuxOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanel1 @@ -3758,7 +3758,7 @@ auxOptions3 - MissionPlanner.Controls.AuxOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.AuxOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanel1 @@ -3779,7 +3779,7 @@ auxOptions4 - MissionPlanner.Controls.AuxOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.AuxOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanel1 @@ -3800,7 +3800,7 @@ auxOptions5 - MissionPlanner.Controls.AuxOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.AuxOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanel1 @@ -3821,7 +3821,7 @@ auxOptions6 - MissionPlanner.Controls.AuxOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.AuxOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanel1 @@ -3842,7 +3842,7 @@ auxOptions7 - MissionPlanner.Controls.AuxOptions, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.AuxOptions, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null flowLayoutPanel1 @@ -5455,6 +5455,30 @@ Jump To Tag + + 123, 22 + + + Full Sized + + + 123, 22 + + + Mini + + + 123, 22 + + + Pop Out + + + 187, 22 + + + Gimbal Video + 188, 290 @@ -5464,6 +5488,39 @@ System.Windows.Forms.ContextMenuStrip, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + Top, Bottom, Right + + + True + + + NoControl + + + 555, 3 + + + 34, 13 + + + 70 + + + Zoom + + + label1 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + splitContainer1.Panel2 + + + 0 + NoControl @@ -5492,7 +5549,34 @@ splitContainer1.Panel2 - 0 + 1 + + + Top, Right + + + 551, 22 + + + 46, 20 + + + 69 + + + Change Zoom Level + + + Zoomlevel + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + splitContainer1.Panel2 + + + 2 Top, Left, Right @@ -5510,13 +5594,43 @@ distanceBar1 - MissionPlanner.Controls.DistanceBar, MissionPlanner, Version=1.3.8313.24739, Culture=neutral, PublicKeyToken=null + MissionPlanner.Controls.DistanceBar, MissionPlanner, Version=1.3.9060.39140, Culture=neutral, PublicKeyToken=null splitContainer1.Panel2 - 1 + 3 + + + Top, Bottom, Right + + + NoControl + + + 550, 40 + + + Vertical + + + 45, 157 + + + 72 + + + TRK_zoom + + + MissionPlanner.Controls.MyTrackBar, MissionPlanner.Controls, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + + + splitContainer1.Panel2 + + + 4 307, 17 @@ -5543,7 +5657,7 @@ splitContainer1.Panel2 - 2 + 5 Bottom, Left @@ -5576,7 +5690,7 @@ splitContainer1.Panel2 - 3 + 6 Bottom, Left @@ -5609,7 +5723,7 @@ splitContainer1.Panel2 - 4 + 7 Bottom, Left @@ -5642,7 +5756,7 @@ splitContainer1.Panel2 - 5 + 8 Bottom, Left @@ -5675,7 +5789,7 @@ splitContainer1.Panel2 - 6 + 9 Bottom, Left @@ -5708,7 +5822,7 @@ splitContainer1.Panel2 - 7 + 10 Bottom, Left @@ -5741,10 +5855,10 @@ splitContainer1.Panel2 - 8 + 11 - - Top, Bottom, Left, Right + + Fill 0, 0 @@ -5753,7 +5867,7 @@ 0, 0, 0, 0 - 550, 197 + 595, 197 0 @@ -5768,37 +5882,7 @@ splitContainer1.Panel2 - 9 - - - Top, Bottom, Right - - - NoControl - - - 550, 40 - - - Vertical - - - 45, 157 - - - 72 - - - TRK_zoom - - - MissionPlanner.Controls.MyTrackBar, MissionPlanner.Controls, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null - - - splitContainer1.Panel2 - - - 10 + 12 splitContainer1.Panel2 @@ -5857,66 +5941,6 @@ 0 - - Top, Right - - - 551, 22 - - - 46, 20 - - - 69 - - - Change Zoom Level - - - Zoomlevel - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - splitContainer1.Panel2 - - - 1 - - - Top, Bottom, Right - - - True - - - NoControl - - - 555, 3 - - - 34, 13 - - - 70 - - - Zoom - - - label1 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - splitContainer1.Panel2 - - - 2 - Bottom, Left @@ -5951,7 +5975,7 @@ panel1 - 3 + 1 Bottom, Left @@ -5987,7 +6011,7 @@ panel1 - 4 + 2 Fill @@ -6101,7 +6125,7 @@ True - 98 + 48 0, 0, 0, 0 @@ -6349,6 +6373,36 @@ System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + jumpToTagToolStripMenuItem + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + gimbalVideoToolStripMenuItem + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + gimbalVideoFullSizedToolStripMenuItem + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + gimbalVideoMiniToolStripMenuItem + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + gimbalVideoPopOutToolStripMenuItem + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + bindingSource1 @@ -6391,12 +6445,6 @@ System.Windows.Forms.BindingSource, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - jumpToTagToolStripMenuItem - - - System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - FlightData From b8d2d5f887d1bd6ce0e8e7fb1c59f45696ed314b Mon Sep 17 00:00:00 2001 From: Bob Long Date: Mon, 16 Dec 2024 23:33:31 +1100 Subject: [PATCH 38/45] CameraProtocol: add GStreamerPipeline generator --- Controls/VideoStreamSelector.cs | 19 +++---- ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs | 60 +++++++++++++++++++++ 2 files changed, 66 insertions(+), 13 deletions(-) diff --git a/Controls/VideoStreamSelector.cs b/Controls/VideoStreamSelector.cs index 4de49fe465..6a7464bb91 100644 --- a/Controls/VideoStreamSelector.cs +++ b/Controls/VideoStreamSelector.cs @@ -8,6 +8,7 @@ using System.Text; using System.Threading.Tasks; using System.Windows.Forms; +using static MAVLink; namespace MissionPlanner.Controls { @@ -20,15 +21,13 @@ public VideoStreamSelector() InitializeComponent(); // Populate the combobox with the available video streams - // use the "name" as the display value and the "url" as the value cmb_detectedstreams.DisplayMember = "Key"; cmb_detectedstreams.ValueMember = "Value"; cmb_detectedstreams.DataSource = CameraProtocol.VideoStreams.Values.Select ( - x => new KeyValuePair + x => new KeyValuePair ( - System.Text.Encoding.UTF8.GetString(x.name).Split('\0')[0], - System.Text.Encoding.UTF8.GetString(x.uri).Split('\0')[0] + System.Text.Encoding.UTF8.GetString(x.name).Split('\0')[0], x ) ).ToList(); @@ -48,15 +47,9 @@ private void but_launch_Click(object sender, EventArgs e) private void cmb_detectedstreams_SelectedIndexChanged(object sender, EventArgs e) { - var uri = cmb_detectedstreams.SelectedValue.ToString(); - if (uri.StartsWith("rtsp://")) - { - txt_gstreamraw.Text = $"rtspsrc location={uri} latency=41 udp-reconnect=1 timeout=0 do-retransmission=false ! application/x-rtp ! decodebin3 ! queue leaky=2 ! videoconvert ! video/x-raw,format=BGRA ! appsink name=outsink sync=false"; - } - else if (uri.StartsWith("gst://")) - { - txt_gstreamraw.Text = uri.Substring("gst://".Length); - } + if (cmb_detectedstreams.SelectedValue == null) + return; + txt_gstreamraw.Text = CameraProtocol.GStreamerPipeline((mavlink_video_stream_information_t)cmb_detectedstreams.SelectedValue); } } } diff --git a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs index 8146c35d17..c085cd41bd 100644 --- a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs +++ b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs @@ -2,6 +2,7 @@ using System.Collections.Concurrent; using System.Collections.Generic; using System.Reflection; +using System.Text.RegularExpressions; using System.Threading.Tasks; using Core.Geometry; using GeoAPI.DataStructures; @@ -33,6 +34,65 @@ public class CameraProtocol public static ConcurrentDictionary<(byte, byte, byte), MAVLink.mavlink_video_stream_information_t> VideoStreams { get; private set; } = new ConcurrentDictionary<(byte, byte, byte), MAVLink.mavlink_video_stream_information_t>(); + public static string GStreamerPipeline(MAVLink.mavlink_video_stream_information_t stream) + { + var type = (MAVLink.VIDEO_STREAM_TYPE)stream.type; + var uri = System.Text.Encoding.UTF8.GetString(stream.uri).Split('\0')[0]; + + // Allow a uri that starts with "gst://" to be used directly as a GStreamer pipeline + // (this is my personal hack to allow for custom pipelines for testing) + if (uri.StartsWith("gst://")) + { + return uri.Substring("gst://".Length); + } + + // For the UDP transports, extract the port number from the URI. The URI should be only the port number, + // but we will attempt to handle malformed ones like "udp://127.0.0.1:5600" as well. + int port = 0; + if (type == MAVLink.VIDEO_STREAM_TYPE.RTPUDP || type == MAVLink.VIDEO_STREAM_TYPE.MPEG_TS) + { + var match = Regex.Match(uri, ":(\\d+)"); // Match a colon followed by digits + if (match.Success) + { + port = int.Parse(match.Groups[1].Value); + if (port < 1 || port > 65535) + { + port = 0; + } + } + if (port == 0) + { + return ""; + } + } + + // Otherwise, correctly generate a pipeline based on the stream type + switch (type) + { + case MAVLink.VIDEO_STREAM_TYPE.RTSP: + uri = "rtsp://" + Regex.Replace(uri, "^.*://", ""); + return $"rtspsrc location={uri} latency=41 udp-reconnect=1 timeout=0 do-retransmission=false ! application/x-rtp ! decodebin3 ! queue leaky=2 ! videoconvert ! video/x-raw,format=BGRA ! appsink name=outsink sync=false"; + + case MAVLink.VIDEO_STREAM_TYPE.RTPUDP: + // Assume unknown encodings are H264 + string encoding_name = stream.encoding == (byte)MAVLink.VIDEO_STREAM_ENCODING.H265 ? "H265" : "H264"; + return $"udpsrc port={port} buffer-size=90000 ! application/x-rtp,media=(string)video,clock-rate=(int)90000,encoding-name=(string){encoding_name} ! decodebin3 ! queue max-size-buffers=1 leaky=2 ! videoconvert ! video/x-raw,format=BGRA ! appsink name=outsink sync=false"; + + case MAVLink.VIDEO_STREAM_TYPE.TCP_MPEG: + var match = Regex.Match(uri, @"^(?:.*://)?([^:/]+):(\d+)"); + if (match.Success) + { + return $"tcpclientsrc host={match.Groups[1].Value} port={match.Groups[2].Value} ! decodebin ! queue max-size-buffers=1 leaky=2 ! videoconvert ! video/x-raw,format=BGRA ! appsink name=outsink sync=false"; + } + return ""; + + case MAVLink.VIDEO_STREAM_TYPE.MPEG_TS: + return $"udpsrc port={port} buffer-size=90000 ! tsparse ! tsdemux ! decodebin ! queue max-size-buffers=1 leaky=2 ! videoconvert ! video/x-raw,format=BGRA ! appsink name=outsink sync=false"; + default: + return ""; + } + } + /// /// True if the camera has different modes, like image mode and video mode /// From 1325f6ae4b240b015e413f69c5c2780369ef5086 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Mon, 16 Dec 2024 23:45:31 +1100 Subject: [PATCH 39/45] GimbalVideoControl: autoconnect to first reported stream --- Controls/GimbalVideoControl.Designer.cs | 26 +++--- Controls/GimbalVideoControl.cs | 89 +++++++++++++++++++++ ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs | 13 ++- 3 files changed, 108 insertions(+), 20 deletions(-) diff --git a/Controls/GimbalVideoControl.Designer.cs b/Controls/GimbalVideoControl.Designer.cs index a98e601445..165b907435 100644 --- a/Controls/GimbalVideoControl.Designer.cs +++ b/Controls/GimbalVideoControl.Designer.cs @@ -77,85 +77,85 @@ private void InitializeComponent() // videoStreamToolStripMenuItem // this.videoStreamToolStripMenuItem.Name = "videoStreamToolStripMenuItem"; - this.videoStreamToolStripMenuItem.Size = new System.Drawing.Size(180, 22); + this.videoStreamToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.videoStreamToolStripMenuItem.Text = "Video Stream"; this.videoStreamToolStripMenuItem.Click += new System.EventHandler(this.videoStreamToolStripMenuItem_Click); // // toolStripMenuItem1 // this.toolStripMenuItem1.Name = "toolStripMenuItem1"; - this.toolStripMenuItem1.Size = new System.Drawing.Size(177, 6); + this.toolStripMenuItem1.Size = new System.Drawing.Size(152, 6); // // retractToolStripMenuItem // this.retractToolStripMenuItem.Name = "retractToolStripMenuItem"; - this.retractToolStripMenuItem.Size = new System.Drawing.Size(180, 22); + this.retractToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.retractToolStripMenuItem.Text = "Retract"; this.retractToolStripMenuItem.Click += new System.EventHandler(this.retractToolStripMenuItem_Click); // // neutralToolStripMenuItem // this.neutralToolStripMenuItem.Name = "neutralToolStripMenuItem"; - this.neutralToolStripMenuItem.Size = new System.Drawing.Size(180, 22); + this.neutralToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.neutralToolStripMenuItem.Text = "Neutral"; this.neutralToolStripMenuItem.Click += new System.EventHandler(this.neutralToolStripMenuItem_Click); // // pointDownToolStripMenuItem // this.pointDownToolStripMenuItem.Name = "pointDownToolStripMenuItem"; - this.pointDownToolStripMenuItem.Size = new System.Drawing.Size(180, 22); + this.pointDownToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.pointDownToolStripMenuItem.Text = "Point Down"; this.pointDownToolStripMenuItem.Click += new System.EventHandler(this.pointDownToolStripMenuItem_Click); // // pointHomeToolStripMenuItem // this.pointHomeToolStripMenuItem.Name = "pointHomeToolStripMenuItem"; - this.pointHomeToolStripMenuItem.Size = new System.Drawing.Size(180, 22); + this.pointHomeToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.pointHomeToolStripMenuItem.Text = "Point Home"; this.pointHomeToolStripMenuItem.Click += new System.EventHandler(this.pointHomeToolStripMenuItem_Click); // // yawLockToolStripMenuItem // this.yawLockToolStripMenuItem.Name = "yawLockToolStripMenuItem"; - this.yawLockToolStripMenuItem.Size = new System.Drawing.Size(180, 22); + this.yawLockToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.yawLockToolStripMenuItem.Text = "Yaw Lock"; this.yawLockToolStripMenuItem.Click += new System.EventHandler(this.yawLockToolStripMenuItem_Click); // // toolStripSeparator1 // this.toolStripSeparator1.Name = "toolStripSeparator1"; - this.toolStripSeparator1.Size = new System.Drawing.Size(177, 6); + this.toolStripSeparator1.Size = new System.Drawing.Size(152, 6); // // takePictureToolStripMenuItem // this.takePictureToolStripMenuItem.Name = "takePictureToolStripMenuItem"; - this.takePictureToolStripMenuItem.Size = new System.Drawing.Size(180, 22); + this.takePictureToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.takePictureToolStripMenuItem.Text = "Take Picture"; this.takePictureToolStripMenuItem.Click += new System.EventHandler(this.takePictureToolStripMenuItem_Click); // // startRecordingToolStripMenuItem // this.startRecordingToolStripMenuItem.Name = "startRecordingToolStripMenuItem"; - this.startRecordingToolStripMenuItem.Size = new System.Drawing.Size(180, 22); + this.startRecordingToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.startRecordingToolStripMenuItem.Text = "Start Recording"; this.startRecordingToolStripMenuItem.Click += new System.EventHandler(this.startRecordingToolStripMenuItem_Click); // // stopRecordingToolStripMenuItem // this.stopRecordingToolStripMenuItem.Name = "stopRecordingToolStripMenuItem"; - this.stopRecordingToolStripMenuItem.Size = new System.Drawing.Size(180, 22); + this.stopRecordingToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.stopRecordingToolStripMenuItem.Text = "Stop Recording"; this.stopRecordingToolStripMenuItem.Click += new System.EventHandler(this.stopRecordingToolStripMenuItem_Click); // // toolStripMenuItem2 // this.toolStripMenuItem2.Name = "toolStripMenuItem2"; - this.toolStripMenuItem2.Size = new System.Drawing.Size(177, 6); + this.toolStripMenuItem2.Size = new System.Drawing.Size(152, 6); // // settingsToolStripMenuItem // this.settingsToolStripMenuItem.Name = "settingsToolStripMenuItem"; - this.settingsToolStripMenuItem.Size = new System.Drawing.Size(180, 22); + this.settingsToolStripMenuItem.Size = new System.Drawing.Size(155, 22); this.settingsToolStripMenuItem.Text = "Settings"; this.settingsToolStripMenuItem.Click += new System.EventHandler(this.settingsToolStripMenuItem_Click); // diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs index 297ddfa5b8..826c5932f8 100644 --- a/Controls/GimbalVideoControl.cs +++ b/Controls/GimbalVideoControl.cs @@ -20,6 +20,7 @@ using MissionPlanner.ArduPilot.Mavlink; using GMap.NET.WindowsForms; using MouseEventArgs = System.Windows.Forms.MouseEventArgs; +using System.Linq; namespace MissionPlanner.Controls { @@ -93,6 +94,8 @@ private GimbalManagerProtocol selectedGimbalManager private byte selectedGimbalID = 0; private GMapOverlay mouseMapMarker; + + private readonly System.Timers.Timer AutoConnectTimer; public GimbalVideoControl() { InitializeComponent(); @@ -108,6 +111,60 @@ public GimbalVideoControl() mouseMapMarker = new GMapOverlay("MouseMarker"); MainV2.instance.FlightData.gMapControl1.Overlays.Add(mouseMapMarker); + + if (!initializeGStreamer()) + { + // No point in doing anything else if GStreamer isn't available + return; + } + + _stream.OnNewImage += RenderFrame; + + // Set up the auto-connect timer + AutoConnectTimer = new System.Timers.Timer() + { + Interval = 1000, + AutoReset = false + }; + AutoConnectTimer.Elapsed += AutoConnectTimerCallback; + AutoConnectTimer.Start(); + } + + private bool initializeGStreamer() + { + GStreamer.GstLaunch = GStreamer.LookForGstreamer(); + + if (!GStreamer.GstLaunchExists) + { + var result = CustomMessageBox.Show( + "This feature requires GStreamer. Would you like to download and install it now?", + "GStreamer not found", + MessageBoxButtons.YesNo, + CustomMessageBox.MessageBoxIcon.Question + ); + if (result != (int)DialogResult.Yes) + { + return false; + } + GStreamerUI.DownloadGStreamer(); + // Check success + if (!GStreamer.GstLaunchExists) + { + var message = "GStreamer was not found after installation. Please install it manually."; + if (GStreamer.NativeMethods.Backend == GStreamer.NativeMethods.BackendEnum.Windows) + { + message += "\n\nFor Windows, install the MinGW version from https://gstreamer.freedesktop.org/download/#windows"; + } + CustomMessageBox.Show( + message, + "GStreamer not found", + MessageBoxButtons.OK, + CustomMessageBox.MessageBoxIcon.Error + ); + return false; + } + } + return true; } private void loadPreferences() @@ -706,5 +763,37 @@ private void settingsToolStripMenuItem_Click(object sender, EventArgs e) loadPreferences(); } } + + private void AutoConnectTimerCallback(object sender, System.Timers.ElapsedEventArgs e) + { + if (CameraProtocol.VideoStreams.Count < 1) + { + Console.Write("Requesting camera information..."); + // We must not have any reported video streams. Try to request them + selectedCamera?.RequestCameraInformationAsync().Wait(); + Console.WriteLine(" done."); + // Come back later and see if any streams have been reported + AutoConnectTimer.Start(); + return; + } + + + string previous_stream = Settings.Instance["gimbal_video_stream", ""]; + // See if any of the streams are the last one used + foreach (var stream in CameraProtocol.VideoStreams.Values) + { + if (System.Text.Encoding.UTF8.GetString(stream.uri).Split('\0')[0] == previous_stream) + { + _stream.Start(CameraProtocol.GStreamerPipeline(stream)); + return; + } + } + + // If not, just use the first one + var first_stream = CameraProtocol.VideoStreams.First().Value; + Settings.Instance["gimbal_video_stream"] = System.Text.Encoding.UTF8.GetString(first_stream.uri).Split('\0')[0]; + _stream.Start(CameraProtocol.GStreamerPipeline(first_stream)); + return; + } } } diff --git a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs index c085cd41bd..89345631df 100644 --- a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs +++ b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs @@ -51,16 +51,15 @@ public static string GStreamerPipeline(MAVLink.mavlink_video_stream_information_ int port = 0; if (type == MAVLink.VIDEO_STREAM_TYPE.RTPUDP || type == MAVLink.VIDEO_STREAM_TYPE.MPEG_TS) { - var match = Regex.Match(uri, ":(\\d+)"); // Match a colon followed by digits - if (match.Success) + if (!int.TryParse(uri, out port)) { - port = int.Parse(match.Groups[1].Value); - if (port < 1 || port > 65535) + var match = Regex.Match(uri, ":(\\d+)"); // Match a colon followed by digits + if (match.Success) { - port = 0; + port = int.Parse(match.Groups[1].Value); } } - if (port == 0) + if (port < 1 || port > 65535) { return ""; } @@ -222,7 +221,7 @@ public Task StartID(MAVState mavState) /// /// Sends an asynchronous request to fetch camera information via. /// - private async Task RequestCameraInformationAsync() + public async Task RequestCameraInformationAsync() { try { From ae7257835792a359480a658a2d0d906121cd59d3 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Thu, 26 Dec 2024 20:57:21 +1100 Subject: [PATCH 40/45] GimbalVideoControl: initialize GStreamer --- Controls/GimbalVideoControl.cs | 2 -- 1 file changed, 2 deletions(-) diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs index 826c5932f8..5ab3465a65 100644 --- a/Controls/GimbalVideoControl.cs +++ b/Controls/GimbalVideoControl.cs @@ -100,8 +100,6 @@ public GimbalVideoControl() { InitializeComponent(); - _stream.OnNewImage += RenderFrame; - loadPreferences(); yaw_lock = preferences.DefaultLockedMode; From 65444f90030863c212ec43b721e0f3666b7250b3 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Thu, 26 Dec 2024 21:16:29 +1100 Subject: [PATCH 41/45] GimbalVideoControl: fix exceptions on disconnect --- Controls/GimbalVideoControl.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs index 5ab3465a65..7b536d7d25 100644 --- a/Controls/GimbalVideoControl.cs +++ b/Controls/GimbalVideoControl.cs @@ -221,7 +221,7 @@ private void RenderFrame(object sender, MPBitmap image) { VideoBox.Image?.Dispose(); VideoBox.Image = null; - VideoBox.Image = VideoBox.ErrorImage; + VideoBox.Image = global::MissionPlanner.Properties.Resources.no_video; return; } From e2950f0d4f6912ce094a64aeb5e480540ed4fef6 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Thu, 26 Dec 2024 21:39:10 +1100 Subject: [PATCH 42/45] FlightData: fix gimbal video context menu bugs --- GCSViews/FlightData.cs | 22 ++++++++++++++-------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/GCSViews/FlightData.cs b/GCSViews/FlightData.cs index e35cf591e2..46e35d1461 100644 --- a/GCSViews/FlightData.cs +++ b/GCSViews/FlightData.cs @@ -6470,20 +6470,16 @@ private void jumpToTagToolStripMenuItem_Click(object sender, EventArgs e) ToolStripMenuItem gimbalVideoShowMiniMap = new ToolStripMenuItem("Mini map"); ToolStripMenuItem gimbalVideoSwapPosition = new ToolStripMenuItem("Swap with map"); ToolStripMenuItem gimbalVideoClose = new ToolStripMenuItem("Close"); + bool gimbalMenuHandlersInitialized = false; GimbalVideoControl _gimbalVideoControl; GimbalVideoControl gimbalVideoControl { get { - // Check if we need to construct a gimbalVideoControl - if (_gimbalVideoControl == null || _gimbalVideoControl.IsDisposed) + // If this is the first call, create the handlers for the context menu items + if (!gimbalMenuHandlersInitialized) { - _gimbalVideoControl = new GimbalVideoControl(); - _gimbalVideoControl.Dock = DockStyle.Fill; - - // Add option to show/hide minimap - gimbalVideoShowMiniMap.CheckOnClick = true; - gimbalVideoShowMiniMap.Checked = true; + gimbalMenuHandlersInitialized = true; gimbalVideoShowMiniMap.CheckedChanged += (s, ev) => { gMapControl1.Visible = gimbalVideoShowMiniMap.Checked; @@ -6507,6 +6503,16 @@ GimbalVideoControl gimbalVideoControl gimbalVideoControl.Stop(); gimbalVideoControl.Dispose(); }; + } + // Check if we need to construct a gimbalVideoControl + if (_gimbalVideoControl == null || _gimbalVideoControl.IsDisposed) + { + _gimbalVideoControl = new GimbalVideoControl(); + _gimbalVideoControl.Dock = DockStyle.Fill; + + // Add option to show/hide minimap + gimbalVideoShowMiniMap.CheckOnClick = true; + gimbalVideoShowMiniMap.Checked = true; _gimbalVideoControl.VideoBoxContextMenu.Items.Add(gimbalVideoShowMiniMap); _gimbalVideoControl.VideoBoxContextMenu.Items.Add(gimbalVideoSwapPosition); From 80b8fff667472d44e0a94a3349fdb87a58c9557e Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sat, 4 Jan 2025 01:32:02 +1100 Subject: [PATCH 43/45] GimbalVideoControl: fix bug in getMousePosition --- Controls/GimbalVideoControl.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs index 7b536d7d25..cce6dd2ded 100644 --- a/Controls/GimbalVideoControl.cs +++ b/Controls/GimbalVideoControl.cs @@ -681,13 +681,13 @@ private void VideoBox_Click(object sender, EventArgs e) { x -= (VideoBox.Width - imageWidth) / 2; x *= VideoBox.Width / imageWidth; - x = Math.Max(0, Math.Min(VideoBox.Width, x)); + x = Math.Max(0, Math.Min(imageWidth, x)); } if (imageHeight < VideoBox.Height) { y -= (VideoBox.Height - imageHeight) / 2; y *= VideoBox.Height / imageHeight; - y = Math.Max(0, Math.Min(VideoBox.Height, y)); + y = Math.Max(0, Math.Min(imageHeight, y)); } return (2 * x / (double)imageWidth - 1, 2 * y / (double)imageHeight - 1); From 3098277be0cfd510cac0b9b653f8d49fbc71ac91 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 7 Jan 2025 00:22:25 +1100 Subject: [PATCH 44/45] GimbalControlSettingsForm: warn about binding clashes --- Controls/GimbalControlSettingsForm.cs | 108 +++++++++++++++++++++++++- 1 file changed, 105 insertions(+), 3 deletions(-) diff --git a/Controls/GimbalControlSettingsForm.cs b/Controls/GimbalControlSettingsForm.cs index e80b431acb..eee199f85e 100644 --- a/Controls/GimbalControlSettingsForm.cs +++ b/Controls/GimbalControlSettingsForm.cs @@ -4,6 +4,7 @@ using System.Data; using System.Drawing; using System.Linq; +using System.Reflection; using System.Text; using System.Threading.Tasks; using System.Windows.Forms; @@ -67,7 +68,7 @@ private void LoadPreferences() }; keyBindingButton.KeyBindingChanged += (sender, e) => { - property.SetValue(preferences, keyBindingButton.KeyBinding); + HandleKeyBindingChanged(keyBindingButton, property); }; SettingsTablePanel.Controls.Add(keyBindingButton, 1, row); var clearKeyButton = new MyButton @@ -90,7 +91,7 @@ private void LoadPreferences() }; clickBindingButton.ClickBindingChanged += (sender, e) => { - property.SetValue(preferences, clickBindingButton.ClickBinding); + HandleClickBindingChanged(clickBindingButton, property); }; SettingsTablePanel.Controls.Add(clickBindingButton, 1, row); var clearClickButton = new MyButton @@ -115,7 +116,7 @@ private void LoadPreferences() SettingsTablePanel.Controls.Add(modifierBinding, 1, row); modifierBinding.KeyBindingChanged += (sender, e) => { - property.SetValue(preferences, modifierBinding.KeyBinding); + HandleKeyBindingChanged(modifierBinding, property); }; var clearModifierButton = new MyButton { @@ -174,6 +175,107 @@ private void but_cancel_Click(object sender, EventArgs e) this.DialogResult = DialogResult.Cancel; this.Close(); } + + // Flag to suppress handlers to prevent re-entry when reverting to the previous value + private bool suppressHandlers = false; + + /// + /// After a new key binding has been assigned, check for clashes and update the preferences object or revert if needed. + /// + /// Button that triggered this event + /// Property that the button is controlling + private void HandleKeyBindingChanged(KeyBindingButton keyBindingButton, PropertyInfo property) + { + if (suppressHandlers) + { + return; + } + if (keyBindingButton.KeyBinding == Keys.None || + GetClashes(keyBindingButton.KeyBinding, property) == (int)DialogResult.Yes) + { + property.SetValue(preferences, keyBindingButton.KeyBinding); + } + else + { + suppressHandlers = true; + keyBindingButton.KeyBinding = (Keys)property.GetValue(preferences); + suppressHandlers = false; + } + } + + /// + /// After a new click binding has been assigned, check for clashes and update the preferences object or revert if needed. + /// + /// Button that triggered this event + /// Property that the button is controlling + private void HandleClickBindingChanged(ClickBindingButton clickBindingButton, PropertyInfo property) + { + if (suppressHandlers) + { + return; + } + if (clickBindingButton.ClickBinding == (Keys.None, MouseButtons.None) || + GetClashes(clickBindingButton.ClickBinding, property) == (int)DialogResult.Yes) + { + property.SetValue(preferences, clickBindingButton.ClickBinding); + } + else + { + suppressHandlers = true; + clickBindingButton.ClickBinding = ((Keys, MouseButtons))property.GetValue(preferences); + suppressHandlers = false; + } + } + + /// + /// Scan through all properties of the preferences object and check for clashes with the new binding. If any are found, show a warning message. + /// + /// + /// New binding that we are attempting to set + /// Property that stores the keybinding + /// Dialog result of warning pop-up + private int GetClashes(T binding, PropertyInfo property) + { + var properties = preferences.GetType().GetProperties(); + var clashList = new List(); + foreach (var otherProperty in properties) + { + if (otherProperty == property) + { + continue; + } + if (otherProperty.PropertyType != property.PropertyType) + { + continue; + } + var attributes = otherProperty.GetCustomAttributes(typeof(PreferencesAttribute), true); + if (attributes.Length == 0) + { + continue; + } + var attribute = (PreferencesAttribute)attributes[0]; + if (object.Equals(otherProperty.GetValue(preferences), binding)) + { + clashList.Add(attribute); + } + } + + if (clashList.Count > 0) + { + var clashMessage = new StringBuilder(); + clashMessage.AppendLine("This binding is already used by the following:"); + clashMessage.AppendLine(); + foreach (var clash in clashList) + { + clashMessage.AppendLine($"- {clash.LabelText}"); + } + clashMessage.AppendLine(); + clashMessage.AppendLine("Are you sure you want to use this binding?"); + return CustomMessageBox.Show(clashMessage.ToString(), "Key Binding Clash", MessageBoxButtons.YesNo, MessageBoxIcon.Warning); + } + + return (int)DialogResult.Yes; + } } public enum ControlType From 4af93834498da8ff8ac6a45f4d6d98aa4f0475ff Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 7 Jan 2025 00:24:43 +1100 Subject: [PATCH 45/45] GimbalVideoControl: allow multiple funcs per key Probably none of these make sense to combine, but the else-ifs made an unwritten priority order that was not clear. This behavior is more expected, and we clearly warn the user now on the settings page about clashes. --- Controls/GimbalVideoControl.cs | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs index cce6dd2ded..d185342d36 100644 --- a/Controls/GimbalVideoControl.cs +++ b/Controls/GimbalVideoControl.cs @@ -462,43 +462,43 @@ private void HandleKeyPress(Keys key) { TakePicture(); } - else if (key == preferences.ToggleRecording) + if (key == preferences.ToggleRecording) { SetRecording(!isRecording); } - else if (key == preferences.StartRecording) + if (key == preferences.StartRecording) { SetRecording(true); } - else if (key == preferences.StopRecording) + if (key == preferences.StopRecording) { SetRecording(false); } - else if (key == preferences.ToggleLockFollow) + if (key == preferences.ToggleLockFollow) { SetYawLock(!yaw_lock); } - else if (key == preferences.SetLock) + if (key == preferences.SetLock) { SetYawLock(true); } - else if (key == preferences.SetFollow) + if (key == preferences.SetFollow) { SetYawLock(false); } - else if (key == preferences.Retract) + if (key == preferences.Retract) { Retract(); } - else if (key == preferences.Neutral) + if (key == preferences.Neutral) { Neutral(); } - else if (key == preferences.PointDown) + if (key == preferences.PointDown) { PointDown(); } - else if (key == preferences.Home) + if (key == preferences.Home) { Home(); }