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..eee199f85e
--- /dev/null
+++ b/Controls/GimbalControlSettingsForm.cs
@@ -0,0 +1,434 @@
+using System;
+using System.Collections.Generic;
+using System.ComponentModel;
+using System.Data;
+using System.Drawing;
+using System.Linq;
+using System.Reflection;
+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) =>
+ {
+ HandleKeyBindingChanged(keyBindingButton, property);
+ };
+ 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) =>
+ {
+ HandleClickBindingChanged(clickBindingButton, property);
+ };
+ 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) =>
+ {
+ HandleKeyBindingChanged(modifierBinding, property);
+ };
+ 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();
+ }
+
+ // 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
+ {
+ 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 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)]
+ 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]
+ CameraHFOV = 40.0m; // horizontal, degrees
+ CameraVFOV = 30.0m; // vertical, degrees
+
+ // Default boolean options
+ UseFOVReportedByCamera = true;
+ DefaultLockedMode = false;
+ }
+
+ 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
new file mode 100644
index 0000000000..165b907435
--- /dev/null
+++ b/Controls/GimbalVideoControl.Designer.cs
@@ -0,0 +1,201 @@
+namespace MissionPlanner.Controls
+{
+ partial class GimbalVideoControl
+ {
+ ///
+ /// Required designer variable.
+ ///
+ private System.ComponentModel.IContainer components = null;
+
+ #region Component Designer generated code
+
+ ///
+ /// Required method for Designer support - do not modify
+ /// the contents of this method with the code editor.
+ ///
+ private void InitializeComponent()
+ {
+ this.components = new System.ComponentModel.Container();
+ this.VideoBox = new System.Windows.Forms.PictureBox();
+ this.VideoBoxContextMenu = new System.Windows.Forms.ContextMenuStrip(this.components);
+ this.videoStreamToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem();
+ 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.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);
+ ((System.ComponentModel.ISupportInitialize)(this.VideoBox)).BeginInit();
+ this.VideoBoxContextMenu.SuspendLayout();
+ this.SuspendLayout();
+ //
+ // VideoBox
+ //
+ 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(0, 0);
+ this.VideoBox.Margin = new System.Windows.Forms.Padding(0);
+ this.VideoBox.Name = "VideoBox";
+ 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;
+ this.VideoBox.Click += new System.EventHandler(this.VideoBox_Click);
+ this.VideoBox.MouseLeave += new System.EventHandler(this.VideoBox_MouseLeave);
+ this.VideoBox.MouseMove += new System.Windows.Forms.MouseEventHandler(this.VideoBox_MouseMove);
+ //
+ // VideoBoxContextMenu
+ //
+ this.VideoBoxContextMenu.Items.AddRange(new System.Windows.Forms.ToolStripItem[] {
+ this.videoStreamToolStripMenuItem,
+ this.toolStripMenuItem1,
+ this.retractToolStripMenuItem,
+ this.neutralToolStripMenuItem,
+ this.pointDownToolStripMenuItem,
+ this.pointHomeToolStripMenuItem,
+ this.yawLockToolStripMenuItem,
+ this.toolStripSeparator1,
+ this.takePictureToolStripMenuItem,
+ this.startRecordingToolStripMenuItem,
+ this.stopRecordingToolStripMenuItem,
+ this.toolStripMenuItem2,
+ this.settingsToolStripMenuItem});
+ this.VideoBoxContextMenu.Name = "VideoBoxContextMenu";
+ this.VideoBoxContextMenu.Size = new System.Drawing.Size(156, 242);
+ //
+ // videoStreamToolStripMenuItem
+ //
+ this.videoStreamToolStripMenuItem.Name = "videoStreamToolStripMenuItem";
+ 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(152, 6);
+ //
+ // retractToolStripMenuItem
+ //
+ 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
+ //
+ 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);
+ //
+ // takePictureToolStripMenuItem
+ //
+ 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.Text = "Start Recording";
+ this.startRecordingToolStripMenuItem.Click += new System.EventHandler(this.startRecordingToolStripMenuItem_Click);
+ //
+ // stopRecordingToolStripMenuItem
+ //
+ this.stopRecordingToolStripMenuItem.Name = "stopRecordingToolStripMenuItem";
+ 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(152, 6);
+ //
+ // settingsToolStripMenuItem
+ //
+ this.settingsToolStripMenuItem.Name = "settingsToolStripMenuItem";
+ this.settingsToolStripMenuItem.Size = new System.Drawing.Size(155, 22);
+ this.settingsToolStripMenuItem.Text = "Settings";
+ this.settingsToolStripMenuItem.Click += new System.EventHandler(this.settingsToolStripMenuItem_Click);
+ //
+ // 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.VideoBox);
+ this.Margin = new System.Windows.Forms.Padding(0);
+ this.Name = "GimbalVideoControl";
+ this.Size = new System.Drawing.Size(640, 480);
+ ((System.ComponentModel.ISupportInitialize)(this.VideoBox)).EndInit();
+ this.VideoBoxContextMenu.ResumeLayout(false);
+ this.ResumeLayout(false);
+
+ }
+
+ #endregion
+ private System.Windows.Forms.ToolTip ControlInfoTooltip;
+ private System.Windows.Forms.ToolStripMenuItem videoStreamToolStripMenuItem;
+ private System.Windows.Forms.ToolStripMenuItem retractToolStripMenuItem;
+ private System.Windows.Forms.ToolStripMenuItem neutralToolStripMenuItem;
+ private System.Windows.Forms.ToolStripSeparator toolStripMenuItem1;
+ private System.Windows.Forms.ToolStripSeparator toolStripMenuItem2;
+ 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;
+ 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
new file mode 100644
index 0000000000..d185342d36
--- /dev/null
+++ b/Controls/GimbalVideoControl.cs
@@ -0,0 +1,797 @@
+#if !LIB
+// XXX: We need both the System.Drawing.Bitmap from System.Drawing and MissionPlanner.Drawing
+extern alias Drawing;
+using MPBitmap = Drawing::System.Drawing.Bitmap;
+#else
+using MPBitmap = System.Drawing.Bitmap;
+#endif
+
+using System;
+using System.Drawing;
+using System.Drawing.Imaging;
+using System.Windows.Forms;
+using MissionPlanner.Utilities;
+using SkiaSharp;
+using log4net;
+using System.Collections.Generic;
+using static MAVLink;
+using MissionPlanner.GCSViews;
+using System.Threading.Tasks;
+using MissionPlanner.ArduPilot.Mavlink;
+using GMap.NET.WindowsForms;
+using MouseEventArgs = System.Windows.Forms.MouseEventArgs;
+using System.Linq;
+
+namespace MissionPlanner.Controls
+{
+ public partial class GimbalVideoControl : UserControl, IMessageFilter
+ {
+ // logger
+ private static readonly ILog log = LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
+
+ private GimbalControlSettings preferences = new GimbalControlSettings();
+
+ private readonly GStreamer _stream = new GStreamer();
+
+ private HashSet heldKeys = new HashSet();
+ private HashSet boundHoldKeys = new HashSet();
+ private HashSet boundPressKeys = new HashSet();
+
+ private Dictionary mod2key = new Dictionary()
+ {
+ { Keys.Shift, Keys.ShiftKey },
+ { Keys.Control, Keys.ControlKey },
+ { Keys.Alt, Keys.Menu }
+ };
+
+ private float previousPitchRate = 0;
+ private float previousYawRate = 0;
+ private float previousZoomRate = 0;
+ private bool yaw_lock = false;
+
+ private CameraProtocol _selectedCamera;
+ private CameraProtocol selectedCamera
+ {
+ get
+ {
+ return _selectedCamera ?? MainV2.comPort?.MAV?.Camera;
+ }
+ set
+ {
+ _selectedCamera = value;
+ }
+ }
+
+ 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
+ {
+ get
+ {
+ return _selectedGimbalManager ?? MainV2.comPort?.MAV?.GimbalManager;
+ }
+ set
+ {
+ _selectedGimbalManager = value;
+ }
+ }
+
+ // The selected gimbal ID for the currently-selected gimbal manager
+ // (0 means all gimbals)
+ private byte selectedGimbalID = 0;
+
+ private GMapOverlay mouseMapMarker;
+
+ private readonly System.Timers.Timer AutoConnectTimer;
+ public GimbalVideoControl()
+ {
+ InitializeComponent();
+
+ loadPreferences();
+
+ yaw_lock = preferences.DefaultLockedMode;
+
+ // Register the global key handler
+ Application.AddMessageFilter(this);
+
+ 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()
+ {
+ preferences = new GimbalControlSettings();
+ var json = Settings.Instance["GimbalControlPreferences", ""];
+ if (json != "")
+ {
+ try
+ {
+ preferences = Newtonsoft.Json.JsonConvert.DeserializeObject(json);
+ }
+ catch (Exception ex)
+ {
+ log.Error("Invalid GimbalControlPreferences, reverting to default", ex);
+ }
+ }
+
+ // Populate the list of keys that are expected to be pressed
+ boundPressKeys.Clear();
+ boundPressKeys.Add(preferences.TakePicture);
+ boundPressKeys.Add(preferences.ToggleRecording);
+ boundPressKeys.Add(preferences.StartRecording);
+ boundPressKeys.Add(preferences.StopRecording);
+ boundPressKeys.Add(preferences.ToggleLockFollow);
+ boundPressKeys.Add(preferences.SetLock);
+ 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
+ boundHoldKeys.Clear();
+ boundHoldKeys.Add(preferences.SlewLeft);
+ boundHoldKeys.Add(preferences.SlewRight);
+ boundHoldKeys.Add(preferences.SlewUp);
+ boundHoldKeys.Add(preferences.SlewDown);
+ boundHoldKeys.Add(preferences.ZoomIn);
+ boundHoldKeys.Add(preferences.ZoomOut);
+ // Add relevant modifier keys
+ boundHoldKeys.Add(mod2key[preferences.SlewFastModifier]);
+ boundHoldKeys.Add(mod2key[preferences.SlewSlowModifier]);
+ }
+
+ private void RenderFrame(object sender, MPBitmap image)
+ {
+ if (InvokeRequired)
+ {
+ BeginInvoke(new Action(() => RenderFrame(sender, image)));
+ return;
+ }
+ try
+ {
+ if (image == null || image.Width <= 0 || image.Height <= 0)
+ {
+ VideoBox.Image?.Dispose();
+ VideoBox.Image = null;
+ VideoBox.Image = global::MissionPlanner.Properties.Resources.no_video;
+ return;
+ }
+
+ var old = VideoBox.Image;
+ VideoBox.Image = new Bitmap(
+ image.Width, image.Height, 4 * image.Width,
+ PixelFormat.Format32bppPArgb,
+ 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();
+ }
+ catch (Exception ex)
+ {
+ log.Error("Error rendering frame", ex);
+ }
+ }
+
+ protected override void Dispose(bool disposing)
+ {
+ if (disposing)
+ {
+ Stop();
+ if (components != null)
+ {
+ components.Dispose();
+ }
+ }
+ base.Dispose(disposing);
+ }
+
+ public void Stop()
+ {
+ _stream.OnNewImage -= RenderFrame;
+ _stream.Stop();
+ }
+
+ private void videoStreamToolStripMenuItem_Click(object sender, EventArgs e)
+ {
+ GStreamer.GstLaunch = GStreamer.LookForGstreamer();
+
+ if (!GStreamer.GstLaunchExists)
+ {
+ GStreamerUI.DownloadGStreamer();
+
+ if (!GStreamer.GstLaunchExists)
+ {
+ return;
+ }
+ }
+
+ var form = new VideoStreamSelector()
+ {
+ StartPosition = FormStartPosition.CenterParent,
+ };
+ if (form.ShowDialog() == DialogResult.OK)
+ {
+ _stream.Start(form.gstreamer_pipeline);
+ }
+ }
+
+ public bool PreFilterMessage(ref Message m)
+ {
+ // Don't hog the keyboard when this control doesn't have focus
+ if (!(Parent?.ContainsFocus ?? false))
+ {
+ if(heldKeys.Count > 0)
+ {
+ heldKeys.Clear();
+ HandleHeldKeys();
+ }
+ return false;
+ }
+
+ const int WM_KEYDOWN = 0x0100;
+ const int WM_KEYUP = 0x0101;
+ const int WM_SYSKEYDOWN = 0x0104;
+ const int WM_SYSKEYUP = 0x0105;
+
+ if (m.Msg == WM_KEYDOWN || m.Msg == WM_SYSKEYDOWN)
+ {
+ // Don't handle repeated keydown events from holding down a key
+ if ((m.LParam.ToInt32() & 0x40000000) != 0)
+ {
+ return false;
+ }
+ return HandleKeyDown((Keys)m.WParam);
+ }
+ else if (m.Msg == WM_KEYUP || m.Msg == WM_SYSKEYUP)
+ {
+ return HandleKeyUp((Keys)m.WParam);
+ }
+
+ return false; // Allow the message to continue to the next filter
+ }
+
+ private bool HandleKeyDown(Keys key)
+ {
+ if (boundHoldKeys.Contains(key))
+ {
+ heldKeys.Add(key);
+ HandleHeldKeys();
+ return true;
+ }
+ else if (boundPressKeys.Contains(key | Control.ModifierKeys))
+ {
+ HandleKeyPress(key | Control.ModifierKeys);
+ return true;
+ }
+ return false;
+ }
+
+ private bool HandleKeyUp(Keys key)
+ {
+ // Always try to remove the key from the list of pressed keys, even if not bound, just in case
+ heldKeys.Remove(key);
+ if (boundHoldKeys.Contains(key))
+ {
+ HandleHeldKeys();
+ }
+ return boundHoldKeys.Contains(key);
+ }
+
+ private void HandleHeldKeys()
+ {
+ float pitch = 0;
+ float yaw = 0;
+ if (heldKeys.Contains(preferences.SlewDown))
+ {
+ pitch -= 1;
+ }
+ if (heldKeys.Contains(preferences.SlewUp))
+ {
+ pitch += 1;
+ }
+ if (heldKeys.Contains(preferences.SlewLeft))
+ {
+ yaw -= 1;
+ }
+ if (heldKeys.Contains(preferences.SlewRight))
+ {
+ yaw += 1;
+ }
+
+ float speed = (float)preferences.SlewSpeedNormal;
+ if (Control.ModifierKeys == preferences.SlewFastModifier)
+ {
+ speed = (float)preferences.SlewSpeedFast;
+ }
+ else if (Control.ModifierKeys == preferences.SlewSlowModifier)
+ {
+ speed = (float)preferences.SlewSpeedSlow;
+ }
+
+ pitch *= speed;
+ yaw *= speed;
+
+ if (pitch != previousPitchRate || yaw != previousYawRate)
+ {
+ previousPitchRate = pitch;
+ previousYawRate = yaw;
+ selectedGimbalManager?.SetRatesCommandAsync(pitch, yaw, yaw_lock, selectedGimbalID);
+ Console.WriteLine($"Pitch: {pitch}, Yaw: {yaw}");
+ }
+
+ float zoom = 0;
+ if (heldKeys.Contains(preferences.ZoomIn))
+ {
+ zoom += 1;
+ }
+ if (heldKeys.Contains(preferences.ZoomOut))
+ {
+ zoom -= 1;
+ }
+
+ zoom *= (float)preferences.ZoomSpeed;
+
+ if (zoom != previousZoomRate)
+ {
+ previousZoomRate = zoom;
+ selectedCamera?.SetZoomAsync(zoom, CAMERA_ZOOM_TYPE.ZOOM_TYPE_CONTINUOUS);
+ Console.WriteLine($"Zoom: {zoom}");
+ }
+ }
+
+ private void HandleKeyPress(Keys key)
+ {
+ if (key == preferences.TakePicture)
+ {
+ TakePicture();
+ }
+ if (key == preferences.ToggleRecording)
+ {
+ SetRecording(!isRecording);
+ }
+ if (key == preferences.StartRecording)
+ {
+ SetRecording(true);
+ }
+ if (key == preferences.StopRecording)
+ {
+ SetRecording(false);
+ }
+ if (key == preferences.ToggleLockFollow)
+ {
+ SetYawLock(!yaw_lock);
+ }
+ if (key == preferences.SetLock)
+ {
+ SetYawLock(true);
+ }
+ if (key == preferences.SetFollow)
+ {
+ SetYawLock(false);
+ }
+ if (key == preferences.Retract)
+ {
+ Retract();
+ }
+ if (key == preferences.Neutral)
+ {
+ Neutral();
+ }
+ if (key == preferences.PointDown)
+ {
+ PointDown();
+ }
+ if (key == preferences.Home)
+ {
+ 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 (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))
+ {
+ lastMouseMove = DateTime.Now;
+ mouseMapMarker.Clear();
+
+
+ var point = getMousePosition(e.X, e.Y);
+ if(!point.HasValue) { return; }
+ var latlon = selectedCamera?.CalculateImagePointLocation(point.Value.x, point.Value.y);
+ if (latlon != null)
+ {
+ mouseMapMarker.Markers.Add(
+ new GMap.NET.WindowsForms.Markers.GMarkerGoogle(
+ new GMap.NET.PointLatLng(latlon.Lat, latlon.Lng),
+ GMap.NET.WindowsForms.Markers.GMarkerGoogleType.blue_small
+ )
+ );
+ }
+ }
+
+ 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)
+ {
+ // Focus the control when clicked
+ VideoBox.Focus();
+
+ MouseEventArgs me = (MouseEventArgs)e;
+ var point = getMousePosition(me.X, me.Y);
+ if (!point.HasValue)
+ {
+ return;
+ }
+
+ // Check the key/button combination to determine the action
+ if ((Control.ModifierKeys, me.Button) == preferences.MoveCameraToMouseLocation)
+ {
+ 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, me.Button) == preferences.MoveCameraPOIToMouseLocation)
+ {
+ 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);
+ }
+ 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)
+ {
+ 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)
+ {
+ if (VideoBox.Image == null)
+ {
+ return null;
+ }
+
+ // Find the point within the image inside VideoBox, not just the VideoBox
+ var imageWidth = Math.Min(VideoBox.Width, VideoBox.Height * VideoBox.Image.Width / VideoBox.Image.Height);
+ var imageHeight = Math.Min(VideoBox.Height, VideoBox.Width * VideoBox.Image.Height / VideoBox.Image.Width);
+ if (imageWidth < VideoBox.Width)
+ {
+ x -= (VideoBox.Width - imageWidth) / 2;
+ x *= VideoBox.Width / imageWidth;
+ 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(imageHeight, y));
+ }
+
+ 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;
+
+ 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)
+ {
+ 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);
+ }
+
+ 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();
+ }
+ }
+
+ 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/Controls/GimbalVideoControl.resx b/Controls/GimbalVideoControl.resx
new file mode 100644
index 0000000000..97b70718cb
--- /dev/null
+++ b/Controls/GimbalVideoControl.resx
@@ -0,0 +1,129 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 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
+
+
+ 345, 17
+
+
\ No newline at end of file
diff --git a/Controls/Video.cs b/Controls/Video.cs
index 42bfbe4938..051266beaa 100644
--- a/Controls/Video.cs
+++ b/Controls/Video.cs
@@ -7,6 +7,8 @@ namespace MissionPlanner.Controls
{
public partial class Video : Form
{
+ private readonly GStreamer stream = new GStreamer();
+
public Video()
{
InitializeComponent();
@@ -47,7 +49,7 @@ private void Video_Load(object sender, EventArgs e)
but.Size = TextRenderer.MeasureText(but.Text + " ", but.Font);
but.Click += delegate (object o, EventArgs args)
{
- GStreamer.StartA(String.Format(
+ stream.Start(String.Format(
"rtspsrc location=rtsp://{0}:{1}{2}?width={3}&height={4} ! application/x-rtp ! rtpjpegdepay ! videoconvert ! video/x-raw,format=BGRA ! appsink name=outsink",
zeroconfHost.IPAddress, service.Value.Port, service.Value.PTR, width,
height));
diff --git a/Controls/VideoStreamSelector.Designer.cs b/Controls/VideoStreamSelector.Designer.cs
new file mode 100644
index 0000000000..27eb1b5890
--- /dev/null
+++ b/Controls/VideoStreamSelector.Designer.cs
@@ -0,0 +1,113 @@
+namespace MissionPlanner.Controls
+{
+ partial class VideoStreamSelector
+ {
+ ///
+ /// 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.cmb_detectedstreams = new System.Windows.Forms.ComboBox();
+ this.but_launch = new MissionPlanner.Controls.MyButton();
+ this.txt_gstreamraw = new System.Windows.Forms.TextBox();
+ this.label1 = new System.Windows.Forms.Label();
+ this.label2 = new System.Windows.Forms.Label();
+ this.SuspendLayout();
+ //
+ // cmb_detectedstreams
+ //
+ this.cmb_detectedstreams.Anchor = ((System.Windows.Forms.AnchorStyles)(((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Left)
+ | System.Windows.Forms.AnchorStyles.Right)));
+ this.cmb_detectedstreams.FormattingEnabled = true;
+ this.cmb_detectedstreams.Location = new System.Drawing.Point(115, 6);
+ this.cmb_detectedstreams.Name = "cmb_detectedstreams";
+ this.cmb_detectedstreams.Size = new System.Drawing.Size(303, 21);
+ this.cmb_detectedstreams.TabIndex = 0;
+ this.cmb_detectedstreams.SelectedIndexChanged += new System.EventHandler(this.cmb_detectedstreams_SelectedIndexChanged);
+ //
+ // but_launch
+ //
+ this.but_launch.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Right)));
+ this.but_launch.Location = new System.Drawing.Point(343, 59);
+ this.but_launch.Name = "but_launch";
+ this.but_launch.Size = new System.Drawing.Size(75, 21);
+ this.but_launch.TabIndex = 1;
+ this.but_launch.Text = "Connect";
+ this.but_launch.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4)))));
+ this.but_launch.UseVisualStyleBackColor = true;
+ this.but_launch.Click += new System.EventHandler(this.but_launch_Click);
+ //
+ // txt_gstreamraw
+ //
+ this.txt_gstreamraw.Anchor = ((System.Windows.Forms.AnchorStyles)(((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Left)
+ | System.Windows.Forms.AnchorStyles.Right)));
+ this.txt_gstreamraw.Location = new System.Drawing.Point(115, 33);
+ this.txt_gstreamraw.Name = "txt_gstreamraw";
+ this.txt_gstreamraw.Size = new System.Drawing.Size(303, 20);
+ this.txt_gstreamraw.TabIndex = 2;
+ //
+ // label1
+ //
+ this.label1.AutoSize = true;
+ this.label1.Location = new System.Drawing.Point(12, 9);
+ this.label1.Name = "label1";
+ this.label1.Size = new System.Drawing.Size(92, 13);
+ this.label1.TabIndex = 3;
+ this.label1.Text = "Detected Streams";
+ //
+ // label2
+ //
+ this.label2.AutoSize = true;
+ this.label2.Location = new System.Drawing.Point(12, 36);
+ this.label2.Name = "label2";
+ this.label2.Size = new System.Drawing.Size(97, 13);
+ this.label2.TabIndex = 4;
+ this.label2.Text = "GStreamer Pipeline";
+ //
+ // VideoStreamSelector
+ //
+ this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F);
+ this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font;
+ this.ClientSize = new System.Drawing.Size(430, 93);
+ this.Controls.Add(this.label2);
+ this.Controls.Add(this.label1);
+ this.Controls.Add(this.txt_gstreamraw);
+ this.Controls.Add(this.but_launch);
+ this.Controls.Add(this.cmb_detectedstreams);
+ this.Name = "VideoStreamSelector";
+ this.Text = "VideoStreamSelector";
+ this.ResumeLayout(false);
+ this.PerformLayout();
+
+ }
+
+ #endregion
+
+ private System.Windows.Forms.ComboBox cmb_detectedstreams;
+ private MyButton but_launch;
+ private System.Windows.Forms.TextBox txt_gstreamraw;
+ private System.Windows.Forms.Label label1;
+ private System.Windows.Forms.Label label2;
+ }
+}
\ No newline at end of file
diff --git a/Controls/VideoStreamSelector.cs b/Controls/VideoStreamSelector.cs
new file mode 100644
index 0000000000..6a7464bb91
--- /dev/null
+++ b/Controls/VideoStreamSelector.cs
@@ -0,0 +1,55 @@
+using MissionPlanner.ArduPilot.Mavlink;
+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 static MAVLink;
+
+namespace MissionPlanner.Controls
+{
+ public partial class VideoStreamSelector : Form
+ {
+
+ public string gstreamer_pipeline = "";
+ public VideoStreamSelector()
+ {
+ InitializeComponent();
+
+ // Populate the combobox with the available video streams
+ cmb_detectedstreams.DisplayMember = "Key";
+ cmb_detectedstreams.ValueMember = "Value";
+ cmb_detectedstreams.DataSource = CameraProtocol.VideoStreams.Values.Select
+ (
+ x => new KeyValuePair
+ (
+ System.Text.Encoding.UTF8.GetString(x.name).Split('\0')[0], x
+ )
+ ).ToList();
+
+ Utilities.ThemeManager.ApplyThemeTo(this);
+ }
+
+ private void but_launch_Click(object sender, EventArgs e)
+ {
+ if(txt_gstreamraw.Text != "")
+ {
+ gstreamer_pipeline = txt_gstreamraw.Text;
+ DialogResult = DialogResult.OK;
+ }
+
+ Close();
+ }
+
+ private void cmb_detectedstreams_SelectedIndexChanged(object sender, EventArgs e)
+ {
+ if (cmb_detectedstreams.SelectedValue == null)
+ return;
+ txt_gstreamraw.Text = CameraProtocol.GStreamerPipeline((mavlink_video_stream_information_t)cmb_detectedstreams.SelectedValue);
+ }
+ }
+}
diff --git a/Controls/VideoStreamSelector.resx b/Controls/VideoStreamSelector.resx
new file mode 100644
index 0000000000..29dcb1b3a3
--- /dev/null
+++ b/Controls/VideoStreamSelector.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/ExtLibs/ArduPilot/CurrentState.cs b/ExtLibs/ArduPilot/CurrentState.cs
index b3401219a9..f2d7d61f31 100644
--- a/ExtLibs/ArduPilot/CurrentState.cs
+++ b/ExtLibs/ArduPilot/CurrentState.cs
@@ -4459,6 +4459,8 @@ public void UpdateCurrentSettings(Action bs, bool updatenow,
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, MAV.cs.raterc,
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/CameraProtocol.cs b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs
index 334e96b105..89345631df 100644
--- a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs
+++ b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs
@@ -1,129 +1,657 @@
-using System;
-using System.Collections.Generic;
-using System.Reflection;
-using System.Text;
-using System.Threading.Tasks;
-using log4net;
-
-namespace MissionPlanner.ArduPilot.Mavlink
-{
- public class CameraProtocol
- {
- private static readonly ILog log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
-
- private MAVState parent;
-
- public static event EventHandler OnRTSPDetected;
-
- public async Task StartID(MAVState mavState)
- {
- parent = mavState;
-
- mavState.parent.OnPacketReceived += (sender, message) =>
- {
- if (message.sysid != parent.sysid || message.compid != parent.compid)
- return;
-
- switch ((MAVLink.MAVLINK_MSG_ID) message.msgid)
- {
- case MAVLink.MAVLINK_MSG_ID.CAMERA_INFORMATION:
-
- CameraInformation = ((MAVLink.mavlink_camera_information_t) message.data);
-
- if (ASCIIEncoding.UTF8.GetString(CameraInformation.cam_definition_uri) != "")
- {
- // get the uri
- }
-
- //if ((info.flags & (int) MAVLink.CAMERA_CAP_FLAGS.HAS_MODES) > 0)
- try
- {
- Task.Run(async () =>
- {
- if ((CameraInformation.flags & (int)MAVLink.CAMERA_CAP_FLAGS.CAPTURE_IMAGE) > 0)
- {
- await parent.parent.doCommandAsync(parent.sysid, parent.compid,
- MAVLink.MAV_CMD.REQUEST_MESSAGE, (int)MAVLink.MAVLINK_MSG_ID.CAMERA_SETTINGS, 0, 0, 0, 0, 0, 0).ConfigureAwait(false);
- }
- if ((CameraInformation.flags & (int)MAVLink.CAMERA_CAP_FLAGS.CAPTURE_VIDEO) > 0)
- {
- await parent.parent.doCommandAsync(parent.sysid, parent.compid,
- MAVLink.MAV_CMD.REQUEST_MESSAGE, (int)MAVLink.MAVLINK_MSG_ID.VIDEO_STREAM_INFORMATION, 0, 0, 0, 0, 0, 0).ConfigureAwait(false);
- }
- if ((CameraInformation.flags & (int)MAVLink.CAMERA_CAP_FLAGS.CAPTURE_VIDEO) > 0)
- {
- await parent.parent.doCommandAsync(parent.sysid, parent.compid,
- MAVLink.MAV_CMD.REQUEST_MESSAGE, (int)MAVLink.MAVLINK_MSG_ID.STORAGE_INFORMATION, 0, 0, 0, 0, 0, 0).ConfigureAwait(false);
- }
- });
- }
- catch (Exception ex)
- {
- log.Error(ex);
- }
-
- break;
- case MAVLink.MAVLINK_MSG_ID.CAMERA_SETTINGS:
- var cameraSettings = ((MAVLink.mavlink_camera_settings_t) message.data);
-
- break;
- case MAVLink.MAVLINK_MSG_ID.VIDEO_STREAM_INFORMATION:
- VideoStreamInformation = ((MAVLink.mavlink_video_stream_information_t) message.data);
- var uri = ASCIIEncoding.UTF8.GetString(VideoStreamInformation.uri);
- int ind = uri.IndexOf('\0');
- if (ind != -1)
- uri = uri.Substring(0, ind);
- if (uri.ToLower().StartsWith("rtsp://"))
- {
- Task.Run(() =>
- {
- try
- {
- OnRTSPDetected?.Invoke(this, uri);
- }
- catch (Exception e)
- {
- log.Error(e);
- }
- });
- }
- break;
- case MAVLink.MAVLINK_MSG_ID.VIDEO_STREAM_STATUS:
- var videoStreamStatus = ((MAVLink.mavlink_video_stream_status_t) message.data);
-
- break;
- case MAVLink.MAVLINK_MSG_ID.CAMERA_CAPTURE_STATUS:
- var cameraCaptureStatus = ((MAVLink.mavlink_camera_capture_status_t) message.data);
-
- break;
- case MAVLink.MAVLINK_MSG_ID.CAMERA_IMAGE_CAPTURED:
- var cameraImageCaptured = ((MAVLink.mavlink_camera_image_captured_t)message.data);
-
- break;
- }
- };
-
- try
- {
- var resp = await parent.parent.doCommandAsync(parent.sysid, parent.compid,
- MAVLink.MAV_CMD.REQUEST_MESSAGE, (int)MAVLink.MAVLINK_MSG_ID.CAMERA_INFORMATION, 0, 0, 0, 0, 0, 0);
- if (resp)
- {
- // no use
- }
- }
- catch (Exception ex)
- {
- log.Error(ex);
- }
- }
-
- public MAVLink.mavlink_video_stream_information_t VideoStreamInformation { get; set; }
- public MAVLink.mavlink_camera_information_t CameraInformation { get; set; }
-
- public MAVLink.CAMERA_CAP_FLAGS GetCameraModes()
- {
- return (MAVLink.CAMERA_CAP_FLAGS)CameraInformation.flags;
- }
- }
-}
+using System;
+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;
+using log4net;
+using MissionPlanner.Utilities;
+
+namespace MissionPlanner.ArduPilot.Mavlink
+{
+ ///
+ /// Handles communication and control for camera operations via MAVLink protocol.
+ /// This includes starting/stopping video capture, taking pictures, and fetching camera settings and status.
+ ///
+ public class CameraProtocol
+ {
+ // Logger for capturing runtime information and errors
+ private static readonly ILog log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+ // Reference to the parent MAVState, used for MAVLink communication
+ private MAVState parent;
+
+ // Tracks whether we have received a `CAMERA_INFORMATION` message yet
+ private bool have_camera_information = false;
+
+ public MAVLink.mavlink_camera_information_t CameraInformation { get; private set; }
+ 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>();
+
+ 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)
+ {
+ if (!int.TryParse(uri, out port))
+ {
+ 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)
+ {
+ 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
+ ///
+ public bool HasModes { get => (CameraInformation.flags & (int)MAVLink.CAMERA_CAP_FLAGS.HAS_MODES) > 0; }
+
+ ///
+ /// True if the camera supports zoom in/out commands.
+ ///
+ public bool HasZoom { get => (CameraInformation.flags & (int)MAVLink.CAMERA_CAP_FLAGS.HAS_BASIC_ZOOM) > 0; }
+
+ ///
+ /// True if the camera supports focus control commands.
+ ///
+ public bool HasFocus { get => (CameraInformation.flags & (int)MAVLink.CAMERA_CAP_FLAGS.HAS_BASIC_FOCUS) > 0; }
+
+ ///
+ /// True if the camera is currently able to capture a video, based on the capabilities and the current mode.
+ ///
+ public bool CanCaptureVideo
+ {
+ get
+ {
+ // If we don't have video capture at all, return false immediately
+ if ((CameraInformation.flags & (int)MAVLink.CAMERA_CAP_FLAGS.CAPTURE_VIDEO) == 0)
+ {
+ return false;
+ }
+ // If we don't have modes, or if we are in video mode return true
+ if (!HasModes || CameraSettings.mode_id == (byte)MAVLink.CAMERA_MODE.VIDEO)
+ {
+ return true;
+ }
+ // If we are in image mode, see if we can capture a video in image mode
+ if (CameraSettings.mode_id == (byte)MAVLink.CAMERA_MODE.IMAGE ||
+ CameraSettings.mode_id == (byte)MAVLink.CAMERA_MODE.IMAGE_SURVEY)
+ {
+ return (CameraInformation.flags & (int)MAVLink.CAMERA_CAP_FLAGS.CAN_CAPTURE_VIDEO_IN_IMAGE_MODE) > 0;
+ }
+ // Unknown mode, assume we cannot capture a video
+ return false;
+ }
+ }
+
+ ///
+ /// True if the camera is currently able to capture an image, based on the capabilities and the current mode.
+ ///
+ public bool CanCaptureImage
+ {
+ get
+ {
+ // If we don't have image capture at all, return false immediately
+ if ((CameraInformation.flags & (int)MAVLink.CAMERA_CAP_FLAGS.CAPTURE_IMAGE) == 0)
+ {
+ return false;
+ }
+ // If we don't have modes, or we are in image mode, return true;
+ // (includes image-survey, even though it's not explicitly mentioned whether manual
+ // image capture is available in this mode)
+ if (!HasModes ||
+ CameraSettings.mode_id == (byte)MAVLink.CAMERA_MODE.IMAGE ||
+ CameraSettings.mode_id == (byte)MAVLink.CAMERA_MODE.IMAGE_SURVEY)
+ {
+ return true;
+ }
+ // If we are in video mode, see if we can capture an image in video mode
+ if (CameraSettings.mode_id == (byte)MAVLink.CAMERA_MODE.VIDEO)
+ {
+ return (CameraInformation.flags & (int)MAVLink.CAMERA_CAP_FLAGS.CAN_CAPTURE_IMAGE_IN_VIDEO_MODE) > 0;
+ }
+ // Unknown mode, assume we cannot capture an image
+ return false;
+ }
+ }
+
+ 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.
+ ///
+ /// MAVState parent of this driver
+ public Task StartID(MAVState mavState)
+ {
+ parent = mavState;
+
+ mavState.parent.OnPacketReceived += ParseMessages;
+
+ return RequestCameraInformationAsync();
+ }
+
+ ///
+ /// Sends an asynchronous request to fetch camera information via.
+ ///
+ public async Task RequestCameraInformationAsync()
+ {
+ try
+ {
+ if (parent?.parent != null)
+ {
+ // New-style request
+ 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)
+ {
+ await parent.parent.doCommandAsync(
+ parent.sysid, parent.compid,
+ MAVLink.MAV_CMD.REQUEST_CAMERA_INFORMATION,
+ 0, 0, 0, 0, 0, 0, 0,
+ false // Don't wait for response
+ );
+ }
+
+ // 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,
+ false // Don't wait for response
+ );
+ }
+ }
+ catch (Exception ex)
+ {
+ log.Error(ex);
+ }
+ }
+
+ ///
+ /// Event handler for OnPacketReceived.
+ /// Parses incoming MAVLink messages related to camera operations and updates internal state accordingly.
+ ///
+ /// MAVLink interface
+ /// MAVLink message to parse
+ public void ParseMessages(object sender, MAVLink.MAVLinkMessage message)
+ {
+ if (message.sysid != parent.sysid || message.compid != parent.compid)
+ return;
+
+ switch ((MAVLink.MAVLINK_MSG_ID)message.msgid)
+ {
+ case MAVLink.MAVLINK_MSG_ID.CAMERA_INFORMATION:
+ have_camera_information = true;
+ CameraInformation = (MAVLink.mavlink_camera_information_t)message.data;
+ break;
+ case MAVLink.MAVLINK_MSG_ID.CAMERA_SETTINGS:
+ CameraSettings = (MAVLink.mavlink_camera_settings_t)message.data;
+ break;
+ 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;
+ case MAVLink.MAVLINK_MSG_ID.CAMERA_TRACKING_IMAGE_STATUS:
+ CameraTrackingImageStatus = (MAVLink.mavlink_camera_tracking_image_status_t)message.data;
+ break;
+ }
+ }
+
+ ///
+ /// Requests that the camera send specific messages types at a specified rate.
+ /// The messages are selected based on the camera's reported capabilities.
+ ///
+ /// Message frequency in messages per second.
+ public void RequestMessageIntervals(int ratehz)
+ {
+ if (parent?.parent == null)
+ {
+ return;
+ }
+
+ float interval_us = (float)(1e6 / ratehz);
+
+ Task.Run(RequestCameraInformationAsync);
+
+ // Request FOV status
+ Task.Run(async () =>
+ {
+ await parent.parent.doCommandAsync(
+ parent.sysid, parent.compid,
+ MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL,
+ (float)MAVLink.MAVLINK_MSG_ID.CAMERA_FOV_STATUS,
+ interval_us,
+ 0, 0, 0, 0, 0,
+ false // Don't wait for response
+ ).ConfigureAwait(false);
+ });
+
+ // Get camera settings
+ if (HasModes || HasZoom || HasFocus)
+ {
+ Task.Run(async () =>
+ {
+ await parent.parent.doCommandAsync(
+ parent.sysid, parent.compid,
+ MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL,
+ (float)MAVLink.MAVLINK_MSG_ID.CAMERA_SETTINGS,
+ interval_us,
+ 0, 0, 0, 0, 0,
+ false // Don't wait for response
+ ).ConfigureAwait(false);
+ });
+ }
+
+ // We use the capability flags directly here, and NOT whether we are currently able to do these things
+ var can_capture_video = (CameraInformation.flags & (int)MAVLink.CAMERA_CAP_FLAGS.CAPTURE_VIDEO) > 0;
+ var can_capture_image = (CameraInformation.flags & (int)MAVLink.CAMERA_CAP_FLAGS.CAPTURE_IMAGE) > 0;
+ if (can_capture_video || can_capture_image)
+ {
+ Task.Run(async () =>
+ {
+ await parent.parent.doCommandAsync(
+ parent.sysid, parent.compid,
+ MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL,
+ (float)MAVLink.MAVLINK_MSG_ID.CAMERA_CAPTURE_STATUS,
+ interval_us,
+ 0, 0, 0, 0, 0,
+ false // Don't wait for response
+ ).ConfigureAwait(false);
+ });
+ }
+ }
+
+ 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.
+ ///
+ /// The index of the camera to trigger, defaults to 0 meaning "all cameras".
+ private int _image_sequence = 1;
+ public Task TakeSinglePictureAsync(int camera = 0)
+ {
+ return parent.parent.doCommandAsync(
+ parent.sysid, parent.compid,
+ MAVLink.MAV_CMD.IMAGE_START_CAPTURE,
+ camera,
+ 0, // Interval
+ 1, // One image
+ _image_sequence++, // Sequence number (prevents retries from accidentally double-triggering)
+ 0, 0, 0
+ );
+ }
+
+ ///
+ /// Start capturing images at a specified rate.
+ ///
+ /// Seconds between each image
+ /// Camera index to trigger (optional). Defaults to 0 for "all cameras"
+ public Task StartIntervalCaptureAsync(float interval, int camera = 0)
+ {
+ return parent.parent.doCommandAsync(
+ parent.sysid, parent.compid,
+ MAVLink.MAV_CMD.IMAGE_START_CAPTURE,
+ camera,
+ interval, // Interval
+ 0, // "Capture forever"
+ 0, // Sequence (unused in interval, set to 0)
+ 0, 0, // Reserved (set to 0)
+ float.NaN // Reserved (set to NaN)
+ );
+ }
+
+ ///
+ /// Stop capturing images
+ ///
+ /// Camera index to trigger (optional). Defaults to 0 for "all cameras"
+ public Task StopIntervalCaptureAsync(int camera = 0)
+ {
+ return parent.parent.doCommandAsync(
+ parent.sysid, parent.compid,
+ MAVLink.MAV_CMD.IMAGE_STOP_CAPTURE,
+ camera,
+ float.NaN, float.NaN, float.NaN, // Reserved (set to NaN)
+ 0, 0, // Reserved (set to 0)
+ float.NaN // Reserved (set to NaN)
+ );
+ }
+
+ ///
+ /// Start capturing video
+ ///
+ /// Stream ID to record (optional). Defaults to 0 for "all streams"
+ public Task StartRecordingAsync(int stream_id = 0)
+ {
+ return parent.parent.doCommandAsync(
+ parent.sysid, parent.compid,
+ MAVLink.MAV_CMD.VIDEO_START_CAPTURE,
+ stream_id,
+ float.NaN, // Frequency of CAMERA_CAPTURE_STATUS messages sent while recording (this parameter is not actually implemented in ArduPilot, and we are requesting CAMERA_CAPTURE_STATUS at all times anyway)
+ float.NaN, float.NaN, // Reserved (set to NaN)
+ 0, 0, // Reserved (set to 0)
+ float.NaN // Reserved (set to NaN)
+ );
+ }
+
+ ///
+ /// Stop capturing video
+ ///
+ /// Stream ID to stop (optional). Defaults to 0 for "all streams"
+ public Task StopRecordingAsync(int stream_id = 0)
+ {
+ return parent.parent.doCommandAsync(
+ parent.sysid, parent.compid,
+ MAVLink.MAV_CMD.VIDEO_STOP_CAPTURE,
+ stream_id,
+ float.NaN, float.NaN, float.NaN, // Reserved (set to NaN)
+ 0, 0, // Reserved (set to 0)
+ float.NaN // Reserved (set to NaN)
+ );
+ }
+
+ ///
+ /// Control the camera zoom level.
+ ///
+ /// The zoom level to set. The range of valid values depend on the zoom type.
+ /// The type of zoom to perform
+ public Task SetZoomAsync(float zoom_level, MAVLink.CAMERA_ZOOM_TYPE zoom_type = MAVLink.CAMERA_ZOOM_TYPE.ZOOM_TYPE_RANGE)
+ {
+ return parent.parent.doCommandAsync(
+ parent.sysid, parent.compid,
+ MAVLink.MAV_CMD.SET_CAMERA_ZOOM,
+ (float)zoom_type,
+ zoom_level,
+ 0, 0, 0, 0, 0
+ );
+ }
+
+ ///
+ /// 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)
+ /// 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)
+ {
+ return imagePosition;
+ }
+
+ 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)
+ {
+ return null;
+ }
+
+ 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 * 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 * HFOV * Math.PI / 180;
+ var side_distance = Math.Sqrt(out_distance * out_distance + height * height) * Math.Tan(side_angle);
+
+ var bearing = camPosition.GetBearing(imagePosition);
+ var pos = camPosition.newpos(bearing, out_distance).newpos(bearing + 90, side_distance);
+ pos.Alt = imagePosition.Alt;
+ return pos;
+ }
+
+
+ ///
+ /// 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)
+ ///
+ private Vector3 CalculateImagePointVectorCameraFrame(double x, double y)
+ {
+ var vector = new Vector3(1, 0, 0); // Camera-frame vector pointing straight ahead
+ if (HFOV != float.NaN && VFOV != float.NaN && x != 0 && y != 0)
+ {
+ 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)
+ 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(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));
+ }
+ }
+}
diff --git a/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs b/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs
new file mode 100644
index 0000000000..c48193bd2c
--- /dev/null
+++ b/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs
@@ -0,0 +1,357 @@
+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.
+ public ConcurrentDictionary ManagerInfo =
+ 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();
+
+ private readonly MAVLinkInterface mavint;
+
+ public GimbalManagerProtocol(MAVLinkInterface mavint, CurrentState cs)
+ {
+ this.mavint = mavint;
+ this.cs = cs;
+ }
+
+ 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);
+ }
+
+ private void MessagesHandler(object sender, MAVLink.MAVLinkMessage message)
+ {
+ if (message.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GIMBAL_MANAGER_INFORMATION)
+ {
+ 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;
+ }
+ }
+
+ 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;
+ }
+ }
+ }
+
+ public bool HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS flags, byte gimbal_device_id = 0)
+ {
+ return ManagerInfo.TryGetValue(gimbal_device_id, out var info) && ((info.cap_flags & (uint)flags) != 0);
+ }
+
+ public bool HasAllCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS flags, byte gimbal_device_id = 0)
+ {
+ 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))
+ {
+ return Task.FromResult(false);
+ }
+ return mavint.doCommandAsync(
+ (byte)mavint.sysidcurrent,
+ (byte)mavint.compidcurrent,
+ MAVLink.MAV_CMD.DO_GIMBAL_MANAGER_PITCHYAW,
+ float.NaN, // pitch angle
+ float.NaN, // yaw angle
+ float.NaN, // pitch rate
+ float.NaN, // yaw rate
+ (float)MAVLink.GIMBAL_MANAGER_FLAGS.RETRACT,
+ 0, // unused
+ gimbal_device_id);
+ }
+
+ public Task NeutralAsync(byte gimbal_device_id = 0)
+ {
+ if (!HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_NEUTRAL))
+ {
+ return Task.FromResult(false);
+ }
+ return mavint.doCommandAsync(
+ (byte)mavint.sysidcurrent,
+ (byte)mavint.compidcurrent,
+ MAVLink.MAV_CMD.DO_GIMBAL_MANAGER_PITCHYAW,
+ float.NaN, // pitch angle
+ float.NaN, // yaw angle
+ float.NaN, // pitch rate
+ float.NaN, // yaw rate
+ (float)MAVLink.GIMBAL_MANAGER_FLAGS.NEUTRAL,
+ 0, // unused
+ gimbal_device_id);
+ }
+
+ public Task SetRCYawLockAsync(bool yaw_lock, byte gimbal_device_id = 0)
+ {
+ if ((yaw_lock && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_YAW_LOCK)) ||
+ (!yaw_lock && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_YAW_FOLLOW)))
+ {
+ return Task.FromResult(false);
+ }
+
+ return mavint.doCommandAsync(
+ (byte)mavint.sysidcurrent,
+ (byte)mavint.compidcurrent,
+ MAVLink.MAV_CMD.DO_GIMBAL_MANAGER_PITCHYAW,
+ float.NaN, // pitch angle
+ float.NaN, // yaw angle
+ float.NaN, // pitch rate
+ float.NaN, // yaw rate
+ yaw_lock ? (float)MAVLink.GIMBAL_MANAGER_FLAGS.YAW_LOCK : 0,
+ 0, // unused
+ gimbal_device_id);
+ }
+
+ ///
+ /// 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_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);
+ }
+
+ return mavint.doCommandAsync(
+ (byte)mavint.sysidcurrent,
+ (byte)mavint.compidcurrent,
+ MAVLink.MAV_CMD.DO_GIMBAL_MANAGER_PITCHYAW,
+ (float)wrap_180(pitch),
+ (float)wrap_180(yaw),
+ float.NaN, // pitch rate
+ float.NaN, // yaw rate
+ yaw_lock ? (float)MAVLink.GIMBAL_MANAGER_FLAGS.YAW_LOCK : 0, // flags
+ 0, // unused
+ gimbal_device_id);
+ }
+
+ public void SetAnglesStream(float pitch, float yaw, bool yaw_in_earth_frame, byte gimbal_device_id = 0)
+ {
+ MAVLink.mavlink_gimbal_manager_set_pitchyaw_t set = new MAVLink.mavlink_gimbal_manager_set_pitchyaw_t()
+ {
+ target_system = (byte)mavint.sysidcurrent,
+ target_component = (byte)mavint.compidcurrent,
+ gimbal_device_id = gimbal_device_id,
+ pitch = pitch,
+ yaw = yaw,
+ pitch_rate = float.NaN,
+ yaw_rate = float.NaN,
+ flags = yaw_in_earth_frame ? (uint)MAVLink.GIMBAL_MANAGER_FLAGS.YAW_LOCK : 0
+ };
+ mavint.sendPacket(set, mavint.sysidcurrent, mavint.compidcurrent);
+ }
+
+ public Task SetRatesCommandAsync(float pitchRate, float yawRate, bool yaw_in_earth_frame, byte gimbal_device_id = 0)
+ {
+ if ((pitchRate != 0 && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_PITCH_AXIS)) ||
+ (yawRate != 0 && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_YAW_AXIS)) ||
+ (yawRate != 0 && yaw_in_earth_frame && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_YAW_LOCK)) ||
+ (yawRate != 0 && !yaw_in_earth_frame && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_YAW_FOLLOW)))
+ {
+ return Task.FromResult(false);
+ }
+
+ return mavint.doCommandAsync(
+ (byte)mavint.sysidcurrent,
+ (byte)mavint.compidcurrent,
+ MAVLink.MAV_CMD.DO_GIMBAL_MANAGER_PITCHYAW,
+ float.NaN, // pitch angle
+ float.NaN, // yaw angle
+ pitchRate,
+ yawRate,
+ yaw_in_earth_frame ? (float)MAVLink.GIMBAL_MANAGER_FLAGS.YAW_LOCK : 0, // flags
+ 0, // unused
+ gimbal_device_id);
+ }
+
+ public void SetRatesStream(float pitchRate, float yawRate, bool yaw_in_earth_frame, byte gimbal_device_id = 0)
+ {
+ MAVLink.mavlink_gimbal_manager_set_pitchyaw_t set = new MAVLink.mavlink_gimbal_manager_set_pitchyaw_t()
+ {
+ target_system = (byte)mavint.sysidcurrent,
+ target_component = (byte)mavint.compidcurrent,
+ gimbal_device_id = gimbal_device_id,
+ pitch = float.NaN,
+ yaw = float.NaN,
+ pitch_rate = pitchRate,
+ yaw_rate = yawRate,
+ flags = yaw_in_earth_frame ? (uint)MAVLink.GIMBAL_MANAGER_FLAGS.YAW_LOCK : 0
+ };
+ mavint.sendPacket(set, mavint.sysidcurrent, mavint.compidcurrent);
+ }
+
+ public Task SetROILocationAsync(double lat, double lon, double alt = 0, byte gimbal_device_id = 0, MAVLink.MAV_FRAME frame = MAVLink.MAV_FRAME.GLOBAL_TERRAIN_ALT)
+ {
+ if (!HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.CAN_POINT_LOCATION_GLOBAL))
+ {
+ return Task.FromResult(false);
+ }
+
+ return mavint.doCommandIntAsync(
+ (byte)mavint.sysidcurrent,
+ (byte)mavint.compidcurrent,
+ MAVLink.MAV_CMD.DO_SET_ROI_LOCATION,
+ gimbal_device_id,
+ 0, 0, 0, // unused
+ (int)(lat * 1e7),
+ (int)(lon * 1e7),
+ (float)alt,
+ frame: frame);
+ }
+
+ public Task SetROINoneAsync(byte gimbal_device_id = 0)
+ {
+ return mavint.doCommandAsync(
+ (byte)mavint.sysidcurrent,
+ (byte)mavint.compidcurrent,
+ MAVLink.MAV_CMD.DO_SET_ROI_NONE,
+ gimbal_device_id,
+ 0, 0, 0, 0, 0, 0);
+ }
+
+ public Task SetROISysIDAsync(byte sysid, byte gimbal_device_id = 0)
+ {
+ if (!HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.CAN_POINT_LOCATION_GLOBAL))
+ {
+ return Task.FromResult(false);
+ }
+
+ return mavint.doCommandAsync(
+ (byte)mavint.sysidcurrent,
+ (byte)mavint.compidcurrent,
+ MAVLink.MAV_CMD.DO_SET_ROI_SYSID,
+ sysid,
+ gimbal_device_id,
+ 0, 0, 0, 0, 0);
+ }
+ }
+}
diff --git a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs
index 71075049e3..28acf12fbe 100644
--- a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs
+++ b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs
@@ -501,8 +501,9 @@ private void OnMAVDetected(object sender, (byte, byte) tuple)
{
// check for a camera
- if (tuple.Item2 >= (byte) MAVLink.MAV_COMPONENT.MAV_COMP_ID_CAMERA &&
- tuple.Item2 <= (byte) MAV_COMPONENT.MAV_COMP_ID_CAMERA6)
+ if (tuple.Item2 == (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1 ||
+ (tuple.Item2 >= (byte) MAVLink.MAV_COMPONENT.MAV_COMP_ID_CAMERA &&
+ tuple.Item2 <= (byte) MAV_COMPONENT.MAV_COMP_ID_CAMERA6))
{
MAVlist[tuple.Item1, tuple.Item2].Camera = new CameraProtocol();
Task.Run(async () =>
@@ -556,6 +557,33 @@ private void OnMAVDetected(object sender, (byte, byte) tuple)
}
});
}
+
+ if (tuple.Item2 == (byte)MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1 ||
+ (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].cs);
+ Task.Run(async () =>
+ {
+ try
+ {
+ // Open holds this
+ while (!_openComplete)
+ {
+ await Task.Delay(1000);
+ }
+
+ await Task.Delay(2000);
+
+ MAVlist[tuple.Item1, tuple.Item2]
+ .GimbalManager.Discover();
+ }
+ catch (Exception e)
+ {
+ log.Error(e);
+ }
+ });
+ }
}
public MAVLinkInterface(Stream logfileStream)
diff --git a/ExtLibs/ArduPilot/Mavlink/MAVState.cs b/ExtLibs/ArduPilot/Mavlink/MAVState.cs
index 970f7bdaa9..a719aec47f 100644
--- a/ExtLibs/ArduPilot/Mavlink/MAVState.cs
+++ b/ExtLibs/ArduPilot/Mavlink/MAVState.cs
@@ -328,7 +328,10 @@ public ap_product Product_ID
internal int recvpacketcount = 0;
public Int64 time_offset_ns { get; set; }
public CameraProtocol Camera { get; set; }
+
+ [Obsolete("Use GimbalManager instead")]
public GimbalProtocol Gimbal { get; set; }
+ public GimbalManagerProtocol GimbalManager { get; set; }
public override string ToString()
{
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
diff --git a/ExtLibs/Utilities/GStreamer.cs b/ExtLibs/Utilities/GStreamer.cs
index cbae46288e..3416d28f46 100644
--- a/ExtLibs/Utilities/GStreamer.cs
+++ b/ExtLibs/Utilities/GStreamer.cs
@@ -1,22 +1,17 @@
-using log4net;
-using SkiaSharp;
-using System;
+using System;
using System.Collections.Generic;
-using System.Diagnostics;
using System.Drawing;
-using System.Drawing.Imaging;
using System.IO;
using System.IO.Compression;
-using System.IO.Pipes;
using System.Linq;
using System.Net;
-using System.Net.Sockets;
using System.Reflection;
using System.Runtime.ExceptionServices;
using System.Runtime.InteropServices;
using System.Security;
using System.Text;
using System.Threading;
+using log4net;
using gsize = System.UInt64;
using GstClockTime = System.UInt64;
using guint = System.UInt32;
@@ -28,18 +23,18 @@ public class GStreamer
private static readonly ILog log =
LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
- private static List processList = new List();
-
- static object _lock = new object();
-
- private static event EventHandler _onNewImage;
-
- public static event EventHandler onNewImage
+ private event EventHandler _onNewImage;
+ public event EventHandler OnNewImage
{
add { _onNewImage += value; }
remove { _onNewImage -= value; }
}
+ private bool threadShouldRun = true;
+ private Thread _backgroundWorker;
+
+#pragma warning disable IDE1006 // Naming Styles
+
public static class NativeMethods
{
public enum BackendEnum
@@ -108,13 +103,10 @@ public static bool gst_init_check(IntPtr argc, IntPtr argv, out IntPtr error)
default:
case BackendEnum.Windows:
return WinNativeMethods.gst_init_check(argc, argv, out error);
- break;
case BackendEnum.Linux:
return LinuxNativeMethods.gst_init_check(argc, argv, out error);
- break;
case BackendEnum.Android:
return AndroidNativeMethods.gst_init_check(argc, argv, out error);
- break;
}
}
@@ -146,13 +138,10 @@ public static IntPtr gst_version_string()
default:
case BackendEnum.Windows:
return WinNativeMethods.gst_version_string();
- break;
case BackendEnum.Linux:
return LinuxNativeMethods.gst_version_string();
- break;
case BackendEnum.Android:
return AndroidNativeMethods.gst_version_string();
- break;
}
}
@@ -164,13 +153,10 @@ public static UIntPtr gst_buffer_extract(IntPtr raw, UIntPtr offset, byte[] dest
default:
case BackendEnum.Windows:
return WinNativeMethods.gst_buffer_extract(raw, offset, dest, size);
- break;
case BackendEnum.Linux:
return LinuxNativeMethods.gst_buffer_extract(raw, offset, dest, size);
- break;
case BackendEnum.Android:
return AndroidNativeMethods.gst_buffer_extract(raw, offset, dest, size);
- break;
}
}
@@ -181,13 +167,10 @@ public static IntPtr gst_bus_timed_pop_filtered(IntPtr raw, ulong timeout, int t
default:
case BackendEnum.Windows:
return WinNativeMethods.gst_bus_timed_pop_filtered(raw, timeout, types);
- break;
case BackendEnum.Linux:
return LinuxNativeMethods.gst_bus_timed_pop_filtered(raw, timeout, types);
- break;
case BackendEnum.Android:
return AndroidNativeMethods.gst_bus_timed_pop_filtered(raw, timeout, types);
- break;
}
}
@@ -218,13 +201,10 @@ public static GstStateChangeReturn gst_element_set_state(IntPtr pipeline, GstSta
default:
case BackendEnum.Windows:
return WinNativeMethods.gst_element_set_state(pipeline, gST_STATE_PLAYING);
- break;
case BackendEnum.Linux:
return LinuxNativeMethods.gst_element_set_state(pipeline, gST_STATE_PLAYING);
- break;
case BackendEnum.Android:
return AndroidNativeMethods.gst_element_set_state(pipeline, gST_STATE_PLAYING);
- break;
}
}
@@ -236,13 +216,10 @@ public static IntPtr gst_parse_launch(string cmdline, out IntPtr error)
default:
case BackendEnum.Windows:
return WinNativeMethods.gst_parse_launch(cmdline, out error);
- break;
case BackendEnum.Linux:
return LinuxNativeMethods.gst_parse_launch(cmdline, out error);
- break;
case BackendEnum.Android:
return AndroidNativeMethods.gst_parse_launch(cmdline, out error);
- break;
}
}
@@ -255,13 +232,10 @@ public static IntPtr gst_element_get_bus(IntPtr pipeline)
default:
case BackendEnum.Windows:
return WinNativeMethods.gst_element_get_bus(pipeline);
- break;
case BackendEnum.Linux:
return LinuxNativeMethods.gst_element_get_bus(pipeline);
- break;
case BackendEnum.Android:
return AndroidNativeMethods.gst_element_get_bus(pipeline);
- break;
}
}
@@ -295,13 +269,10 @@ public static IntPtr gst_app_sink_try_pull_sample(IntPtr appsink,
default:
case BackendEnum.Windows:
return WinNativeMethods.gst_app_sink_try_pull_sample(appsink, timeout);
- break;
case BackendEnum.Linux:
return LinuxNativeMethods.gst_app_sink_try_pull_sample(appsink, timeout);
- break;
case BackendEnum.Android:
return AndroidNativeMethods.gst_app_sink_try_pull_sample(appsink, timeout);
- break;
}
}
@@ -332,13 +303,10 @@ public static IntPtr gst_bin_get_by_name(IntPtr pipeline, string name)
default:
case BackendEnum.Windows:
return WinNativeMethods.gst_bin_get_by_name(pipeline, name);
- break;
case BackendEnum.Linux:
return LinuxNativeMethods.gst_bin_get_by_name(pipeline, name);
- break;
case BackendEnum.Android:
return AndroidNativeMethods.gst_bin_get_by_name(pipeline, name);
- break;
}
}
@@ -350,13 +318,10 @@ public static IntPtr gst_sample_get_buffer(IntPtr sample)
default:
case BackendEnum.Windows:
return WinNativeMethods.gst_sample_get_buffer(sample);
- break;
case BackendEnum.Linux:
return LinuxNativeMethods.gst_sample_get_buffer(sample);
- break;
case BackendEnum.Android:
return AndroidNativeMethods.gst_sample_get_buffer(sample);
- break;
}
}
@@ -369,13 +334,10 @@ public static IntPtr
default:
case BackendEnum.Windows:
return WinNativeMethods.gst_sample_get_caps(sample);
- break;
case BackendEnum.Linux:
return LinuxNativeMethods.gst_sample_get_caps(sample);
- break;
case BackendEnum.Android:
return AndroidNativeMethods.gst_sample_get_caps(sample);
- break;
}
}
@@ -389,13 +351,10 @@ public static bool
default:
case BackendEnum.Windows:
return WinNativeMethods.gst_structure_get_int(structure, fieldname, out value);
- break;
case BackendEnum.Linux:
return LinuxNativeMethods.gst_structure_get_int(structure, fieldname, out value);
- break;
case BackendEnum.Android:
return AndroidNativeMethods.gst_structure_get_int(structure, fieldname, out value);
- break;
}
}
@@ -409,13 +368,10 @@ public static IntPtr
default:
case BackendEnum.Windows:
return WinNativeMethods.gst_caps_get_structure(caps, index);
- break;
case BackendEnum.Linux:
return LinuxNativeMethods.gst_caps_get_structure(caps, index);
- break;
case BackendEnum.Android:
return AndroidNativeMethods.gst_caps_get_structure(caps, index);
- break;
}
}
@@ -428,13 +384,10 @@ public static bool gst_buffer_map(IntPtr buffer, out GstMapInfo info, GstMapFlag
default:
case BackendEnum.Windows:
return WinNativeMethods.gst_buffer_map(buffer, out info, GstMapFlags);
- break;
case BackendEnum.Linux:
return LinuxNativeMethods.gst_buffer_map(buffer, out info, GstMapFlags);
- break;
case BackendEnum.Android:
return AndroidNativeMethods.gst_buffer_map(buffer, out info, GstMapFlags);
- break;
}
}
@@ -494,13 +447,10 @@ public static bool gst_app_sink_is_eos(IntPtr appsink)
default:
case BackendEnum.Windows:
return WinNativeMethods.gst_app_sink_is_eos(appsink);
- break;
case BackendEnum.Linux:
return LinuxNativeMethods.gst_app_sink_is_eos(appsink);
- break;
case BackendEnum.Android:
return AndroidNativeMethods.gst_app_sink_is_eos(appsink);
- break;
}
}
@@ -1081,6 +1031,7 @@ public static extern void
public static extern void gst_app_sink_set_callbacks(IntPtr appsink, GstAppSinkCallbacks callbacks,
IntPtr user_data, IntPtr notify);
}
+#pragma warning restore IDE1006 // Naming Styles
public const UInt64 GST_CLOCK_TIME_NONE = 18446744073709551615;
@@ -1096,8 +1047,6 @@ public struct GstMapInfo
public gsize size;
public gsize maxsize;
- //[MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)] public IntPtr[] user_data; //4
- //[MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)] public IntPtr[] _gst_reserved; //4
}
public delegate void eos(IntPtr sink, IntPtr user_data);
@@ -1223,21 +1172,6 @@ public enum GstMessageType
GST_MESSAGE_ANY = ~0
}
-
- [StructLayout(LayoutKind.Sequential)]
- public struct GstObject
- {
- IntPtr _lock;
- public string name;
- public Object parent;
- public uint flags;
- IntPtr controlBindings;
- public int control_rate;
- public int last_sync;
-
- private IntPtr[] _gstGstReserved;
- }
-
[StructLayout(LayoutKind.Sequential)]
public struct GError
{
@@ -1247,18 +1181,18 @@ public struct GError
}
[HandleProcessCorruptedStateExceptions, SecurityCritical]
- public static Thread StartA(string stringpipeline)
+ public Thread Start(string stringpipeline)
{
- var th = new Thread(ThreadStart) {IsBackground = true, Name = "gstreamer"};
-
- th.Start(stringpipeline);
+ Stop();
+ _backgroundWorker = new Thread(ThreadStart) {IsBackground = true, Name = "gstreamer"};
+ _backgroundWorker.Start(stringpipeline);
- return th;
+ return _backgroundWorker;
}
[HandleProcessCorruptedStateExceptions, SecurityCritical]
- static void ThreadStart(object datao)
+ void ThreadStart(object datao)
{
string stringpipeline = (string)datao;
@@ -1266,31 +1200,27 @@ static void ThreadStart(object datao)
try
{
-
-
//https://github.com/GStreamer/gstreamer/blob/master/tools/gst-launch.c#L1125
NativeMethods.gst_init(IntPtr.Zero, IntPtr.Zero);
}
catch (DllNotFoundException ex)
{
- CustomMessageBox.Show("The file was not found at " + gstlaunch +
+ CustomMessageBox.Show("The file was not found at " + GstLaunch +
"\nPlease verify permissions " + ex.ToString());
return;
}
catch (BadImageFormatException)
{
- CustomMessageBox.Show("The incorrect exe architecture has been detected at " + gstlaunch +
+ CustomMessageBox.Show("The incorrect exe architecture has been detected at " + GstLaunch +
"\nPlease install gstreamer for the correct architecture");
return;
}
- uint v1 = 0, v2 = 0, v3 = 0, v4 = 0;
- NativeMethods.gst_version(out v1, out v2, out v3, out v4);
+ NativeMethods.gst_version(out uint v1, out uint v2, out uint v3, out uint v4);
log.InfoFormat("GStreamer {0}.{1}.{2}.{3}", v1, v2, v3, v4);
- IntPtr error;
- NativeMethods.gst_init_check(IntPtr.Zero, IntPtr.Zero, out error);
+ NativeMethods.gst_init_check(IntPtr.Zero, IntPtr.Zero, out IntPtr error);
if (error != IntPtr.Zero)
{
@@ -1304,14 +1234,8 @@ static void ThreadStart(object datao)
log.InfoFormat("GStreamer parse {0}", stringpipeline);
var pipeline = NativeMethods.gst_parse_launch(
stringpipeline,
- //@"videotestsrc ! video/x-raw, width=1280, height=720, framerate=30/1 ! x264enc speed-preset=1 threads=1 sliced-threads=1 mb-tree=0 rc-lookahead=0 sync-lookahead=0 bframes=0 ! rtph264pay ! application/x-rtp ! rtph264depay ! avdec_h264 ! videoconvert ! video/x-raw,format=BGRA ! appsink name=outsink",
- //@"-v udpsrc port=5601 buffer-size=300000 ! application/x-rtp ! rtph264depay ! avdec_h264 ! videoconvert ! video/x-raw,format=BGRA ! appsink name=outsink",
- //@"rtspsrc location=rtsp://192.168.1.252/video1 ! application/x-rtp ! rtph264depay ! avdec_h264 ! videoconvert ! video/x-raw,format=BGRA ! appsink name=outsink",
out error);
- //rtspsrc location=rtsp://192.168.1.21/live ! application/x-rtp ! rtph265depay ! avdec_h265 ! videoconvert ! video/x-raw,format=BGRA ! appsink name=outsink
- //videotestsrc ! video/x-raw, width=1280, height=720, framerate=30/1 ! videoconvert ! video/x-raw,format=BGRA ! appsink name=outsink
-
if (error != IntPtr.Zero)
{
var er = Marshal.PtrToStructure(error);
@@ -1324,29 +1248,11 @@ static void ThreadStart(object datao)
log.Info("graphviz of pipeline is at " + Path.GetTempPath() + "pipeline.dot");
- //var msg = GStreamer.gst_bus_timed_pop_filtered(bus, GStreamer.GST_CLOCK_TIME_NONE, GStreamer.GstMessageType.GST_MESSAGE_ERROR | GStreamer.GstMessageType.GST_MESSAGE_EOS);
-
-
// appsink is part of the parse launch
var appsink = NativeMethods.gst_bin_get_by_name(pipeline, "outsink");
log.Info("got appsink ");
- //var appsink = NativeMethods.gst_element_factory_make("appsink", null);
- bool newdata = false;
GstAppSinkCallbacks callbacks = new GstAppSinkCallbacks();
- //var callbackhandle = GCHandle.Alloc(callbacks, GCHandleType.Pinned);
- /*callbacks.new_buffer += (sink, data) =>
- {
- newdata = true;
- return GstFlowReturn.GST_FLOW_OK;
- };
- callbacks.new_preroll += (sink, data) =>
- {
- log.Info("new_preroll");
- return GstFlowReturn.GST_FLOW_OK;
- };
- callbacks.eos += (sink, data) => { log.Info("EOS"); };
- */
if (appsink != IntPtr.Zero)
{
@@ -1355,9 +1261,6 @@ static void ThreadStart(object datao)
log.Info("set appsink params ");
}
- // callback fail on linux
- //NativeMethods.gst_app_sink_set_callbacks(appsink, callbacks, IntPtr.Zero, IntPtr.Zero);
-
/* Start playing */
var running = NativeMethods.gst_element_set_state(pipeline, GstState.GST_STATE_PLAYING) != GstStateChangeReturn.GST_STATE_CHANGE_FAILURE;
log.Info("set playing ");
@@ -1371,45 +1274,39 @@ static void ThreadStart(object datao)
int trys = 0;
GstAppSinkCallbacks callbacks2 = callbacks;
- run = true;
+ threadShouldRun = true;
// not using appsink
if (appsink == IntPtr.Zero && running)
{
/* Wait until error or EOS */
- var msg = NativeMethods.gst_bus_timed_pop_filtered(bus, GST_CLOCK_TIME_NONE,
+ NativeMethods.gst_bus_timed_pop_filtered(bus, GST_CLOCK_TIME_NONE,
(int)(GstMessageType.GST_MESSAGE_ERROR | GstMessageType.GST_MESSAGE_EOS));
- run = false;
+ threadShouldRun = false;
}
else {
var msg = NativeMethods.gst_bus_timed_pop_filtered(bus, 0,
(int)(GstMessageType.GST_MESSAGE_ERROR | GstMessageType.GST_MESSAGE_EOS));
if (msg != IntPtr.Zero)
- run = false;
+ threadShouldRun = false;
}
log.Info("start frame loop gst_app_sink_is_eos");
- while (run && !NativeMethods.gst_app_sink_is_eos(appsink))
+ while (threadShouldRun && !NativeMethods.gst_app_sink_is_eos(appsink))
{
try
{
- //log.Info("gst_app_sink_try_pull_sample ");
var sample = NativeMethods.gst_app_sink_try_pull_sample(appsink, GST_SECOND * 5);
if (sample != IntPtr.Zero)
{
trys = 0;
- //var caps = gst_app_sink_get_caps(appsink);
- //log.Info("gst_sample_get_caps ");
var caps = NativeMethods.gst_sample_get_caps(sample);
var caps_s = NativeMethods.gst_caps_get_structure(caps, 0);
NativeMethods.gst_structure_get_int(caps_s, "width", out Width);
NativeMethods.gst_structure_get_int(caps_s, "height", out Height);
var capsstring = NativeMethods.gst_caps_to_string(caps_s);
- //var structure = gst_sample_get_info(sample);
- //var structstring = gst_structure_to_string(structure);
- //log.Info("gst_sample_get_buffer ");
var buffer = NativeMethods.gst_sample_get_buffer(sample);
if (buffer != IntPtr.Zero)
{
@@ -1440,11 +1337,10 @@ 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();
-
// cleanup
_onNewImage?.Invoke(null, null);
@@ -1453,26 +1349,19 @@ static void ThreadStart(object datao)
~GStreamer()
{
- StopAll();
+ Stop();
}
static GStreamer()
{
- UdpPort = 5600;
- OutputPort = 1235;
-
var dataDirectory = Settings.GetDataDirectory();
var gstdir = Path.Combine(dataDirectory, @"gstreamer\1.0\x86_64\bin\libgstreamer-1.0-0.dll");
-
SetGSTPath(gstdir);
}
private static void SetGSTPath(string gstdir)
{
- //
- //C:\gstreamer\1.0\x86_64\bin\gst-launch-1.0.exe
-
var orig = gstdir;
if (!File.Exists(gstdir))
@@ -1508,55 +1397,39 @@ private static void SetGSTPath(string gstdir)
}catch { }
}
- //C:\ProgramData\Mission Planner\gstreamer\1.0\x86_64\bin
-
- //gst-launch-1.0.exe videotestsrc pattern=ball is-live=true ! video/x-raw,width=640,height=480 ! clockoverlay ! x264enc ! rtph264pay ! udpsink host=127.0.0.1 port=5600
- //gst-launch-1.0.exe -v udpsrc port=5600 buffer-size=60000 ! application/x-rtp ! rtph264depay ! avdec_h264 ! queue leaky=2 ! avenc_mjpeg ! tcpserversink host=127.0.0.1 port=1235 sync=false
- //udpsrc port=5600 buffer-size=60000 ! application/x-rtp ! rtph264depay ! avdec_h264 ! queue leaky=2 ! video/x-raw,format=BGRx ! appsink name=outsink
-
- //gst-launch-1.0.exe -v videotestsrc ! video/x-raw,format=BGRA,framerate=25/1 ! videoconvert ! autovideosink
-
- //gst-launch-1.0 videotestsrc pattern=ball ! x264enc ! rtph264pay ! udpsink host=127.0.0.1 port=5600
- //gst-launch-1.0 udpsrc port=5600 caps='application/x-rtp, media=(string)video, clock-rate=(int)90000, encoding-name=(string)H264' ! rtph264depay ! avdec_h264 ! autovideosink fps-update-interval=1000 sync=false
-
- //gst-launch-1.0.exe videotestsrc ! video/x-raw, width=1280, height=720, framerate=25/1 ! x264enc ! rtph264pay ! udpsink port=1234 host=192.168.0.1
- //gst-launch-1.0.exe -v udpsrc port=1234 buffer-size=60000 ! application/x-rtp ! rtph264depay ! avdec_h264 ! queue ! avenc_mjpeg ! tcpserversink host=127.0.0.1 port=1235 sync=false
-
- // list plugins
- // gst-inspect-1.0
-
- public static string gstlaunch
+ public static string GstLaunch
{
get { return Settings.Instance["gstlaunchexe"]; }
set { Settings.Instance["gstlaunchexe"] = value; }
}
- public static bool gstlaunchexists
+ public static bool GstLaunchExists
{
get
{
if (Android)
return true;
- return File.Exists(gstlaunch);
+ return File.Exists(GstLaunch);
}
}
public static string LookForGstreamer()
{
- List dirs = new List();
-
- // linux
- dirs.Add("/usr/lib/x86_64-linux-gnu");
- // rpi
- dirs.Add("/usr/lib/arm-linux-gnueabihf");
- // current
- dirs.Add(Path.GetDirectoryName(Assembly.GetExecutingAssembly().Location));
- // settings
- dirs.Add(Settings.GetDataDirectory());
- // custom settings
- dirs.Add(Settings.CustomUserDataDirectory);
- // sitl bindle
- dirs.Add(GStreamer.BundledPath);
+ List dirs = new List
+ {
+ // linux
+ "/usr/lib/x86_64-linux-gnu",
+ // rpi
+ "/usr/lib/arm-linux-gnueabihf",
+ // current
+ Path.GetDirectoryName(Assembly.GetExecutingAssembly().Location),
+ // settings
+ Settings.GetDataDirectory(),
+ // custom settings
+ Settings.CustomUserDataDirectory,
+ // sitl bindle
+ GStreamer.BundledPath
+ };
DriveInfo[] allDrives = DriveInfo.GetDrives();
foreach (DriveInfo d in allDrives)
@@ -1610,574 +1483,23 @@ public static string LookForGstreamer()
return "";
}
- public static int UdpPort { get; set; }
-
- public static int OutputPort { get; set; }
-
- private static bool isrunning
- {
- get
- {
- return processList != null && processList.Count > 0 && !processList.Any(a =>
- {
- try
- {
- return a.HasExited;
- }
- catch
- {
- return true;
- }
- });
- }
- }
-
// custom search path for .so
public static string BundledPath { get; set; }
public static bool Android { get; set; }
- public static Process Start(string custompipelinesrc = "", bool externalpipeline = false,
- bool allowmultiple = false)
- {
- // prevent starting 2 process's
- lock (_lock)
- {
- if (!allowmultiple && isrunning)
- {
- log.Info("already running");
- return null;
- }
-
- if (GStreamer.gstlaunchexists)
- {
- ProcessStartInfo psi = new ProcessStartInfo(gstlaunch,
- String.Format(
- "-v udpsrc port={0} buffer-size=300000 ! application/x-rtp ! rtph264depay ! avdec_h264 ",
- UdpPort));
-
- if (custompipelinesrc != "")
- {
- psi.Arguments = custompipelinesrc;
-
- if (!externalpipeline)
- {
- psi.Arguments += String.Format(
- " ! queue leaky=2 ! tcpserversink host=127.0.0.1 port={0} sync=false",
- OutputPort);
- }
- else
- {
- psi.Arguments += " ! decodebin ! queue leaky=2 ! autovideosink";
- }
- }
- else
- {
- if (!externalpipeline)
- {
- psi.Arguments += String.Format(
- " ! queue leaky=2 ! jpegenc ! queue leaky=2 ! tcpserversink host=127.0.0.1 port={0} sync=false",
- OutputPort);
- }
- else
- {
- psi.Arguments += " ! decodebin ! queue leaky=2 ! autovideosink";
- }
- }
-
- //"-v udpsrc port=5600 buffer-size=300000 ! application/x-rtp ! rtph264depay ! avdec_h264 ! videoconvert ! video/x-raw,format=BGRA ! queue ! rtpvrawpay ! giosink location=\\\\\\\\.\\\\pipe\\\\gstreamer");
-
- //avenc_mjpeg
-
- psi.UseShellExecute = false;
-
- log.Info("Starting " + psi.FileName + " " + psi.Arguments);
-
- psi.RedirectStandardInput = true;
- psi.RedirectStandardOutput = true;
- psi.RedirectStandardError = true;
-
- var process = Process.Start(psi);
- GStreamer.processList.Add(process);
-
- process.Exited += delegate(object sender, EventArgs args) { Stop(process); };
-
- process.ErrorDataReceived += (sender, args) => { log.Error(args.Data); };
- process.OutputDataReceived += (sender, args) => { log.Info(args.Data); };
-
- process.BeginOutputReadLine();
- process.BeginErrorReadLine();
-
- System.Threading.ThreadPool.QueueUserWorkItem(_Start);
-
- return process;
- }
- else
- {
- log.Info("No gstreamer found");
- }
- }
-
- return null;
- }
-
- private static void NamedPipeConnect(NamedPipeServerStream pipeServer)
+ public void Stop()
{
- // 1080 * 1920 * 4(int) = 8294400 * 30fps = buffer 1/3 sec
- using (var stream = new BufferedStream(pipeServer, 1024 * 1024 * 9 * 10))
- {
- using (MemoryStream ms = new MemoryStream())
- {
- while (pipeServer.IsConnected)
- {
- //readJPGData(stream, ms);
-
- readRTPData(stream, ms);
-
- System.Threading.Thread.Sleep(0);
- }
-
- //cleanup on disconnect
- _onNewImage?.Invoke(null, null);
- }
- }
- }
-
- static void _Start(object nothing)
- {
- try
- {
- var deadline = DateTime.Now.AddSeconds(20);
-
- log.Info("_Start");
-
- while (DateTime.Now < deadline)
- {
- try
- {
- TcpClient client = new TcpClient("127.0.0.1", OutputPort);
- Console.WriteLine("Port open");
- client.Close();
- break;
- }
- catch (Exception)
- {
- Console.WriteLine("Port closed");
- }
- }
-
- using (TcpClient client = new TcpClient("127.0.0.1", OutputPort))
- {
- client.ReceiveBufferSize = 1024 * 1024 * 5; // 5mb
-
- using (MemoryStream ms = new MemoryStream())
- {
- using (var stream = new BufferedStream(client.GetStream(), 1024 * 1024 * 5))
- {
- DateTime lastdata = DateTime.Now;
- while (client.Client.Connected)
- {
- int bytestoread = client.Available;
-
- while (bytestoread > 0)
- {
- //bytestoread -= readRTPData(stream, ms);
- bytestoread -= readJPGData(stream, ms);
- lastdata = DateTime.Now;
- }
-
- // up to 100 fps or 50 with 10ms process time
- System.Threading.Thread.Sleep(10);
-
- if (lastdata.AddSeconds(5) < DateTime.Now)
- {
- client.Client.Send(new byte[0]);
- }
- }
-
- //cleanup on disconnect
- _onNewImage?.Invoke(null, null);
- }
- }
- }
- }
- catch (Exception ex)
- {
- _onNewImage?.Invoke(null, null);
- log.Error(ex);
- }
- }
-
- class rtpheader
- {
- // 2 bits
- public byte version { get; set; }
-
- // 1 bit
- public byte padding { get; set; }
-
- // 1 bit
- public byte extension { get; set; }
-
- // 4 bits
- public byte csrccount { get; set; }
-
- // 1 bits
- public byte marker { get; set; }
-
- // 7 bits
- public byte payloadtype { get; set; }
-
- // 16 bits
- public ushort sequencenumber { get; set; }
-
- // 32 bits
- public uint timestamp { get; set; }
-
- // 32 bits
- public uint ssrc { get; set; }
-
- public ushort extsequencenumber { get; set; }
-
- public ushort length { get; set; }
- public byte F { get; set; }
- public ushort lineno { get; set; }
- public byte C { get; set; }
- public ushort offset { get; set; }
-
- // only is C is set above;
- public ushort length2 { get; set; }
-
- public byte F2 { get; set; }
- public ushort lineno2 { get; set; }
- public byte C2 { get; set; }
- public ushort offset2 { get; set; }
-
- // rfc 4175 - https://tools.ietf.org/html/rfc4175
- public rtpheader(byte[] buffer)
- {
- version = (byte) ((buffer[0] >> 6) & 3);
- padding = (byte) ((buffer[0] >> 5) & 1);
- extension = (byte) ((buffer[0] >> 4) & 1);
- csrccount = (byte) ((buffer[0] >> 0) & 15);
-
- marker = (byte) ((buffer[1] >> 7) & 1);
- payloadtype = (byte) ((buffer[1] >> 0) & 127);
-
- sequencenumber = (ushort) (buffer[2] << 8 + buffer[3]);
- timestamp = (uint) (buffer[4] << 24 + buffer[5] << 26 + buffer[6] << 8 + buffer[7]);
- ssrc = (uint) (buffer[8] << 24 + buffer[9] << 26 + buffer[10] << 8 + buffer[11]);
-
- // 2 bytes https://fossies.org/linux/gst-plugins-good/gst/rtp/gstrtpvrawpay.c #384
- // always 0
- extsequencenumber = (ushort) (buffer[12] << 8 + buffer[13]);
-
- // 6 byte - https://fossies.org/linux/gst-plugins-good/gst/rtp/gstrtpvrawpay.c #467
- // (pixels * pgroup) / xinc ; pgroup = 4/32bit BGRA xinc = 1 for BGRA
- length = (ushort) ((buffer[14] << 8) + buffer[15]);
- F = (byte) (buffer[16] >> 7);
- // height line number
- lineno = (ushort) (((buffer[16] & 127) << 8) + buffer[17]);
- // there is more than one height here
- C = (byte) (buffer[18] >> 7);
- offset = (ushort) (((buffer[18] & 127) << 8) + buffer[19]);
-
- if (buffer.Length >= 26)
- {
- length2 = (ushort) ((buffer[20] << 8) + buffer[21]);
- F2 = (byte) (buffer[22] >> 7);
- // height line number
- lineno2 = (ushort) (((buffer[22] & 127) << 8) + buffer[23]);
- // there is more than one height here
- C2 = (byte) (buffer[24] >> 7);
- offset2 = (ushort) (((buffer[24] & 127) << 8) + buffer[25]);
-
- return;
- }
-
- var actoffset = offset * 4;
-
- //Console.WriteLine("rtp {0} - mark {1} {2} {3} {4} {5} {6}={7} {8}", payloadtype, marker, sequencenumber, length, C, lineno, offset, actoffset,F);
- }
- }
-
- static byte rtpbyte1 = 0x80;
-
- // mark bit notset
- static byte rtpbyte2 = 0x60;
-
- // mark bit set
- static byte rtpbyte2_2 = 0xe0;
-
- // image width
- static int width = 0;
-
- private static byte[] buffer;
- private static Bitmap img;
-
- public static int readRTPData(Stream stream, MemoryStream ms)
- {
- int readamount = 0;
-
- var ch1 = stream.ReadByte();
- readamount++;
- if (ch1 == rtpbyte1)
- {
- var ch2 = stream.ReadByte();
- readamount++;
- // handle 2 rtpbyte1's in a row
- if (ch2 == rtpbyte1)
- {
- ch1 = ch2;
- ch2 = stream.ReadByte();
- readamount++;
- }
-
- if (ch2 == rtpbyte2 || ch2 == rtpbyte2_2)
- {
- byte[] headerBytes = new byte[4 * 5];
- headerBytes[0] = (byte) ch1;
- headerBytes[1] = (byte) ch2;
-
- readamount += stream.Read(headerBytes, 2, headerBytes.Length - 2);
-
- GStreamer.rtpheader header = new rtpheader(headerBytes);
-
- // this check is the same as rtpbyte1 and rtpbyte2/rtpbyte2_2
- if (header.version == 2 && header.payloadtype == 96 && header.extsequencenumber == 0)
- {
- // read additial C
- if (header.C > 0)
- {
- var oldsize = headerBytes.Length;
- Array.Resize(ref headerBytes, headerBytes.Length + 6);
- readamount += stream.Read(headerBytes, oldsize, 6);
-
- header = new rtpheader(headerBytes);
- }
-
- var pixels = header.length / 4;
- if (header.C > 0 && header.lineno == 0)
- {
- width = header.offset + pixels;
- }
-
- //p0 + (lin * ystride) + (offs * pgroup), length
- var fileoffset = (header.lineno) * width * 4 + header.offset * 4;
- if (fileoffset != ms.Position)
- {
- }
-
- ms.Seek(fileoffset, SeekOrigin.Begin);
-
- int neededbytes = header.length + header.length2;
-
- if (buffer == null || buffer.Length < (neededbytes))
- buffer = new byte[neededbytes];
-
- var read = stream.Read(buffer, 0, neededbytes);
- ms.Write(buffer, 0, read);
- readamount += read;
-
- if (header.marker > 0 && width != 0)
- {
- ms.Seek(0, SeekOrigin.Begin);
- try
- {
- if (img == null || img.Width < width || img.Height < header.lineno + 1)
- img = new Bitmap(width, header.lineno + 1, SKColorType.Bgra8888);
-
- lock (img)
- {
- BitmapData bmpData = img.LockBits(new Rectangle(0, 0, img.Width, img.Height),
- ImageLockMode.WriteOnly, img.PixelFormat);
-
- IntPtr ptr = bmpData.Scan0;
-
- Marshal.Copy(ms.GetBuffer(), 0, ptr, (int) img.Width * img.Height * 4);
-
- img.UnlockBits(bmpData);
- }
-
- //img.Save("test.bmp");
-
- _onNewImage?.Invoke(null, img);
-
- tempno++;
- persecond++;
-
- if (lastsecond.Second != DateTime.Now.Second)
- {
- Console.WriteLine("image {0}x{1} size {2} miss {3} ps {4}",
- img.Width,
- img.Height, 0, miss, persecond);
- persecond = 0;
- lastsecond = DateTime.Now;
- miss = 0;
- }
-
- ms.SetLength(0);
- }
- catch
- {
- ms.SetLength(0);
- }
- }
- }
- else
- {
- miss++;
- Console.WriteLine("packet failed header check ");
- }
- }
- else
- {
- miss++;
- Console.WriteLine("out of sync2 {0:X}", ch1);
- }
- }
- else
- {
- miss++;
- Console.WriteLine("out of sync1 {0:X}", ch1);
- }
-
- return readamount;
- }
-
- static int tempno = 0;
- static int miss = 0;
- static int persecond = 0;
- static DateTime lastsecond = DateTime.Now;
-
- private static int readJPGData(Stream stream, MemoryStream ms)
- {
- var bytesread = 0;
-
- // start header
- byte ch3 = (byte) stream.ReadByte();
- bytesread++;
- if (ch3 == 0xff)
- {
- byte ch4 = (byte) stream.ReadByte();
- bytesread++;
- if (ch4 == 0xd8)
- {
- ms.Seek(0, SeekOrigin.Begin);
- ms.WriteByte(ch3);
- ms.WriteByte(ch4);
- int last = 0;
- do
- {
- int datach = stream.ReadByte();
- bytesread++;
- if (datach < 0)
- break;
-
- ms.WriteByte((byte) datach);
-
- if (last == 0xff)
- {
- if (datach == 0xd9)
- break;
- }
-
- last = datach;
- } while (true);
-
- ms.Seek(0, SeekOrigin.Begin);
- try
- {
- var temp = Bitmap.FromStream(ms);
-
- //File.WriteAllBytes(tempno + ".bmp", ms.ToArray());
-
- _onNewImage?.Invoke(null, temp);
-
- tempno++;
- persecond++;
-
- if (lastsecond.Second != DateTime.Now.Second)
- {
- Console.WriteLine("image {0}x{1} size {2} miss {3} ps {4}",
- temp.Width,
- temp.Height, 0, miss, persecond);
- persecond = 0;
- lastsecond = DateTime.Now;
- miss = 0;
- }
- }
- catch
- {
- }
- }
- else
- {
- miss++;
- }
- }
- else
- {
- miss++;
- }
-
- return bytesread;
- }
-
- public static void Stop(Process run)
- {
- try
- {
- log.Info("Stop");
-
- if (run != null)
- {
- try
- {
- log.Info("StandardInput close");
- run.StandardInput.Write('\x3');
- run.StandardInput.Close();
- }
- catch
- {
- }
-
- if (!run.CloseMainWindow())
- {
- Thread.Sleep(100);
- log.Info("Kill");
- run.Kill();
- }
-
- log.Info("Close");
- run.Close();
- }
- }
- catch
- {
- }
-
- run = null;
- }
-
- private static bool run = true;
- public static void StopAll()
- {
- run = false;
- Thread.Sleep(50);
- foreach (var process in processList)
- {
- Stop(process);
- }
+ threadShouldRun = false;
+ _backgroundWorker?.Join();
}
public static void DownloadGStreamer(Action status = null)
{
- string output = "";
- string url = "";
-
- if(RuntimeInformation.OSArchitecture == Architecture.Arm || RuntimeInformation.OSArchitecture == Architecture.Arm64)
+ if (RuntimeInformation.OSArchitecture == Architecture.Arm || RuntimeInformation.OSArchitecture == Architecture.Arm64)
return;
+ string output;
+ string url;
if (System.Environment.Is64BitProcess)
{
output = Settings.GetDataDirectory() + "gstreamer-1.0-x86_64-1.14.4.zip";
diff --git a/ExtLibs/Utilities/Quaternion.cs b/ExtLibs/Utilities/Quaternion.cs
index 09dc98c2e5..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,300 +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 earth to body frame
- public void earth_to_body(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);
- v = m * 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;
- v /= theta;
- from_axis_angle(v, theta);
+ 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);
+
+ 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)
- {
- return Math.Atan2(l, q1);
- }
-
- private double sqrt(double v)
- {
- return Math.Sqrt(v);
- }
-
- private double sq(double q2)
+
+ ///
+ /// Square a number
+ ///
+ ///
+ ///
+ private double sq(double n)
{
- return q2 * q2;
+ return n * n;
}
- 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;
@@ -407,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/GCSViews/FlightData.Designer.cs b/GCSViews/FlightData.Designer.cs
index d23a1ff518..3eeea28fc8 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
{
@@ -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();
@@ -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);
@@ -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,12 +2081,13 @@ 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);
//
// 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 +2096,13 @@ 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.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.gimbalVideoPopOutToolStripMenuItem_Click);
//
// groupBoxRoll
//
@@ -2128,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);
//
@@ -2202,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);
//
@@ -2213,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);
//
@@ -2224,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);
//
@@ -2235,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);
//
@@ -2246,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);
//
@@ -2257,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);
//
@@ -2268,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);
//
@@ -2278,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);
//
@@ -2304,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);
//
@@ -2314,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);
//
@@ -2351,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);
//
@@ -2358,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
@@ -2367,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);
//
@@ -2377,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);
//
@@ -2387,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);
//
@@ -2397,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);
//
@@ -2407,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);
//
@@ -2447,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
//
@@ -2475,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");
//
@@ -2589,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;
@@ -2596,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");
@@ -2607,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";
@@ -2698,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");
@@ -2724,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");
@@ -2810,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);
@@ -2836,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);
@@ -2887,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();
@@ -3066,7 +3146,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;
@@ -3126,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 9e66efeb78..46e35d1461 100644
--- a/GCSViews/FlightData.cs
+++ b/GCSViews/FlightData.cs
@@ -1,4 +1,4 @@
-using DirectShowLib;
+using DirectShowLib;
using GMap.NET;
using GMap.NET.WindowsForms;
using GMap.NET.WindowsForms.Markers;
@@ -42,12 +42,14 @@ public partial class FlightData : MyUserControl, IActivate, IDeactivate
public static FlightData instance;
public static GMapOverlay kmlpolygons;
public static HUD myhud;
+ public static readonly GStreamer hudGStreamer = new GStreamer();
public static myGMAP mymap;
public static bool threadrun;
public SplitContainer MainHcopy;
internal static GMapOverlay geofence;
internal static GMapOverlay photosoverlay;
internal static GMapOverlay poioverlay = new GMapOverlay("POI");
+ internal static GMapOverlay cameraBounds;
internal static GMapOverlay rallypointoverlay;
internal static GMapOverlay tfrpolygons;
internal GMapMarker CurrentGMapMarker;
@@ -385,6 +387,9 @@ public FlightData()
photosoverlay = new GMapOverlay("photos overlay");
gMapControl1.Overlays.Add(photosoverlay);
+ cameraBounds = new GMapOverlay("camera bounds");
+ gMapControl1.Overlays.Add(cameraBounds);
+
routes = new GMapOverlay("routes");
gMapControl1.Overlays.Add(routes);
@@ -1855,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()
@@ -3080,13 +3088,11 @@ private void Gspeed_DoubleClick(object sender, EventArgs e)
private void GStreamerStopToolStripMenuItem_Click(object sender, EventArgs e)
{
- GStreamer.StopAll();
+ hudGStreamer.Stop();
}
private void HereLinkVideoToolStripMenuItem_Click(object sender, EventArgs e)
{
- GStreamer.StopAll();
-
string ipaddr = "192.168.43.1";
if (Settings.Instance["herelinkip"] != null)
@@ -3100,19 +3106,19 @@ private void HereLinkVideoToolStripMenuItem_Click(object sender, EventArgs e)
"rtspsrc location=rtsp://{0}:8554/fpv_stream latency=1 udp-reconnect=1 timeout=0 do-retransmission=false ! application/x-rtp ! rtph264depay ! h264parse ! queue ! avdec_h264 ! queue max-size-buffers=1 leaky=2 ! videoconvert ! video/x-raw,format=BGRx ! appsink name=outsink",
ipaddr);
- GStreamer.gstlaunch = GStreamer.LookForGstreamer();
+ GStreamer.GstLaunch = GStreamer.LookForGstreamer();
- if (!GStreamer.gstlaunchexists)
+ if (!GStreamer.GstLaunchExists)
{
GStreamerUI.DownloadGStreamer();
- if (!GStreamer.gstlaunchexists)
+ if (!GStreamer.GstLaunchExists)
{
return;
}
}
- GStreamer.StartA(url);
+ GCSViews.FlightData.hudGStreamer.Start(url);
}
private void hud_UserItem(object sender, EventArgs e)
@@ -4193,6 +4199,37 @@ private void mainloop()
prop.altasl = MainV2.comPort.MAV.cs.altasl;
prop.center = gMapControl1.Position;
+ // Update camera bounds
+ cameraBounds.Polygons.Clear();
+ if (MainV2.comPort?.MAV?.Camera != null)
+ {
+ var cam = MainV2.comPort.MAV.Camera;
+ var p1 = cam?.CalculateImagePointLocation(-1, -1);
+ var p2 = cam?.CalculateImagePointLocation(-1, 1);
+ var p3 = cam?.CalculateImagePointLocation(1, 1);
+ var p4 = cam?.CalculateImagePointLocation(1, -1);
+
+ if(p1 != null && p2 != null && p3 != null && p4 != null)
+ {
+ cameraBounds.Polygons.Add(
+ new GMap.NET.WindowsForms.GMapPolygon(
+ new List
+ {
+ new GMap.NET.PointLatLng(p1.Lat, p1.Lng),
+ new GMap.NET.PointLatLng(p2.Lat, p2.Lng),
+ new GMap.NET.PointLatLng(p3.Lat, p3.Lng),
+ new GMap.NET.PointLatLng(p4.Lat, p4.Lng)
+ },
+ "CameraBounds"
+ )
+ {
+ Fill = Brushes.Transparent,
+ Stroke = new Pen(Color.DarkBlue, 3)
+ }
+ );
+ }
+ }
+
gMapControl1.HoldInvalidation = false;
if (gMapControl1.Visible)
@@ -4724,15 +4761,13 @@ private void setGStreamerSourceToolStripMenuItem_Click(object sender, EventArgs
{
Settings.Instance["gstreamer_url"] = url;
- GStreamer.StopAll();
+ GStreamer.GstLaunch = GStreamer.LookForGstreamer();
- GStreamer.gstlaunch = GStreamer.LookForGstreamer();
-
- if (!GStreamer.gstlaunchexists)
+ if (!GStreamer.GstLaunchExists)
{
GStreamerUI.DownloadGStreamer();
- if (!GStreamer.gstlaunchexists)
+ if (!GStreamer.GstLaunchExists)
{
return;
}
@@ -4740,7 +4775,7 @@ private void setGStreamerSourceToolStripMenuItem_Click(object sender, EventArgs
try
{
- GStreamer.StartA(url);
+ hudGStreamer.Start(url);
}
catch (Exception ex)
{
@@ -4749,7 +4784,7 @@ private void setGStreamerSourceToolStripMenuItem_Click(object sender, EventArgs
}
else
{
- GStreamer.Stop(null);
+ hudGStreamer.Stop();
}
}
@@ -6431,5 +6466,185 @@ private void jumpToTagToolStripMenuItem_Click(object sender, EventArgs e)
CustomMessageBox.Show(Strings.CommandFailed + ex.ToString(), Strings.ERROR);
}
}
+
+ 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
+ {
+ // If this is the first call, create the handlers for the context menu items
+ if (!gimbalMenuHandlersInitialized)
+ {
+ gimbalMenuHandlersInitialized = 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();
+ };
+ }
+ // 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);
+ _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(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 3c83e3bb3b..f8dddae42c 100644
--- a/GCSViews/FlightData.resx
+++ b/GCSViews/FlightData.resx
@@ -1,4 +1,4 @@
-
+