APM Planner 1.1.82

modify 3dr radio packet size 64 > 32
add andrews link status Mod - thanks
add hw voltage to status
add generic logbrowse pid class "pid-*" > "pid-1"
This commit is contained in:
Michael Oborne 2012-05-08 21:21:19 +08:00
parent a04b6d6993
commit 5b70b5a389
21 changed files with 891 additions and 160 deletions

View File

@ -32,20 +32,21 @@
</PropertyGroup> </PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|x86' "> <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|x86' ">
<PlatformTarget>x86</PlatformTarget> <PlatformTarget>x86</PlatformTarget>
<DebugSymbols>true</DebugSymbols> <DebugSymbols>false</DebugSymbols>
<DebugType>full</DebugType> <DebugType>full</DebugType>
<Optimize>false</Optimize> <Optimize>false</Optimize>
<OutputPath>bin\Debug\</OutputPath> <OutputPath>bin\Debug\</OutputPath>
<DefineConstants>DEBUG;TRACE</DefineConstants> <DefineConstants>DEBUG</DefineConstants>
<ErrorReport>prompt</ErrorReport> <ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel> <WarningLevel>4</WarningLevel>
</PropertyGroup> </PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|x86' "> <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|x86' ">
<PlatformTarget>x86</PlatformTarget> <PlatformTarget>x86</PlatformTarget>
<DebugType>pdbonly</DebugType> <DebugType>none</DebugType>
<Optimize>true</Optimize> <Optimize>true</Optimize>
<OutputPath>bin\Release\</OutputPath> <OutputPath>bin\Release\</OutputPath>
<DefineConstants>TRACE</DefineConstants> <DefineConstants>
</DefineConstants>
<ErrorReport>prompt</ErrorReport> <ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel> <WarningLevel>4</WarningLevel>
</PropertyGroup> </PropertyGroup>
@ -195,12 +196,6 @@
<Install>true</Install> <Install>true</Install>
</BootstrapperPackage> </BootstrapperPackage>
</ItemGroup> </ItemGroup>
<ItemGroup>
<None Include="Resources\3dr.png" />
</ItemGroup>
<ItemGroup>
<None Include="Resources\3dr2.png" />
</ItemGroup>
<ItemGroup> <ItemGroup>
<Content Include="3dr.ico" /> <Content Include="3dr.ico" />
<None Include="Resources\3dr3.png" /> <None Include="Resources\3dr3.png" />

View File

@ -1518,11 +1518,8 @@
//////////////////////////////////8= //////////////////////////////////8=
</value> </value>
</data> </data>
<data name="$this.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="$this.Text" xml:space="preserve"> <data name="$this.Text" xml:space="preserve">
<value>3DRRadio Config</value> <value>3DRRadio Config 0.2</value>
</data> </data>
<data name="&gt;&gt;$this.Name" xml:space="preserve"> <data name="&gt;&gt;$this.Name" xml:space="preserve">
<value>Config</value> <value>Config</value>

View File

@ -12,7 +12,7 @@ namespace _3DRRadio
/// The main entry point for the application. /// The main entry point for the application.
/// </summary> /// </summary>
[STAThread] [STAThread]
static void Main() static void Main(string[] args)
{ {
XmlConfigurator.Configure(); XmlConfigurator.Configure();

View File

@ -32,5 +32,5 @@ using System.Runtime.InteropServices;
// You can specify all the values or you can default the Build and Revision Numbers // You can specify all the values or you can default the Build and Revision Numbers
// by using the '*' as shown below: // by using the '*' as shown below:
// [assembly: AssemblyVersion("1.0.*")] // [assembly: AssemblyVersion("1.0.*")]
[assembly: AssemblyVersion("0.1.*")] [assembly: AssemblyVersion("0.2.*")]
[assembly: AssemblyFileVersion("0.1.0.0")] [assembly: AssemblyFileVersion("0.2.0.0")]

View File

@ -60,20 +60,6 @@ namespace _3DRRadio.Properties {
} }
} }
internal static System.Drawing.Bitmap _3dr {
get {
object obj = ResourceManager.GetObject("3dr", resourceCulture);
return ((System.Drawing.Bitmap)(obj));
}
}
internal static System.Drawing.Bitmap _3dr2 {
get {
object obj = ResourceManager.GetObject("3dr2", resourceCulture);
return ((System.Drawing.Bitmap)(obj));
}
}
internal static System.Drawing.Bitmap _3dr3 { internal static System.Drawing.Bitmap _3dr3 {
get { get {
object obj = ResourceManager.GetObject("3dr3", resourceCulture); object obj = ResourceManager.GetObject("3dr3", resourceCulture);

View File

@ -118,13 +118,6 @@
<value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value> <value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</resheader> </resheader>
<assembly alias="System.Windows.Forms" name="System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089" /> <assembly alias="System.Windows.Forms" name="System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089" />
<data name="3dr2" type="System.Resources.ResXFileRef, System.Windows.Forms">
<value>..\Resources\3dr2.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
</data>
<data name="3dr" type="System.Resources.ResXFileRef, System.Windows.Forms">
<value>..\Resources\3dr.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
</data>
<assembly alias="System.Windows.Forms" name="System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089" />
<data name="3dr3" type="System.Resources.ResXFileRef, System.Windows.Forms"> <data name="3dr3" type="System.Resources.ResXFileRef, System.Windows.Forms">
<value>..\Resources\3dr3.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value> <value>..\Resources\3dr3.png;System.Drawing.Bitmap, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a</value>
</data> </data>

View File

@ -201,6 +201,10 @@
<Reference Include="System.Management"> <Reference Include="System.Management">
<Private>False</Private> <Private>False</Private>
</Reference> </Reference>
<Reference Include="System.Reactive, Version=1.0.10621.0, Culture=neutral, PublicKeyToken=31bf3856ad364e35, processorArchitecture=MSIL">
<SpecificVersion>False</SpecificVersion>
<HintPath>Lib\System.Reactive.dll</HintPath>
</Reference>
<Reference Include="System.Speech"> <Reference Include="System.Speech">
<Private>True</Private> <Private>True</Private>
</Reference> </Reference>
@ -288,6 +292,12 @@
<Compile Include="Controls\ConfigPanel.Designer.cs"> <Compile Include="Controls\ConfigPanel.Designer.cs">
<DependentUpon>ConfigPanel.cs</DependentUpon> <DependentUpon>ConfigPanel.cs</DependentUpon>
</Compile> </Compile>
<Compile Include="Controls\ConnectionStats.cs">
<SubType>UserControl</SubType>
</Compile>
<Compile Include="Controls\ConnectionStats.designer.cs">
<DependentUpon>ConnectionStats.cs</DependentUpon>
</Compile>
<Compile Include="Controls\CustomMessageBox.cs" /> <Compile Include="Controls\CustomMessageBox.cs" />
<Compile Include="Controls\LineSeparator.cs"> <Compile Include="Controls\LineSeparator.cs">
<SubType>UserControl</SubType> <SubType>UserControl</SubType>
@ -602,6 +612,9 @@
<EmbeddedResource Include="Controls\ConfigPanel.resx"> <EmbeddedResource Include="Controls\ConfigPanel.resx">
<DependentUpon>ConfigPanel.cs</DependentUpon> <DependentUpon>ConfigPanel.cs</DependentUpon>
</EmbeddedResource> </EmbeddedResource>
<EmbeddedResource Include="Controls\ConnectionStats.resx">
<DependentUpon>ConnectionStats.cs</DependentUpon>
</EmbeddedResource>
<EmbeddedResource Include="Controls\HSI.resx"> <EmbeddedResource Include="Controls\HSI.resx">
<DependentUpon>HSI.cs</DependentUpon> <DependentUpon>HSI.cs</DependentUpon>
</EmbeddedResource> </EmbeddedResource>
@ -1122,13 +1135,9 @@
<EmbeddedResource Include="temp.resx"> <EmbeddedResource Include="temp.resx">
<DependentUpon>temp.cs</DependentUpon> <DependentUpon>temp.cs</DependentUpon>
</EmbeddedResource> </EmbeddedResource>
<None Include="AeroSimRCAPMHil.zip"> <None Include="AeroSimRCAPMHil.zip" />
<CopyToOutputDirectory>Always</CopyToOutputDirectory>
</None>
<None Include="app.config" /> <None Include="app.config" />
<None Include="arducopter-xplane.zip"> <None Include="arducopter-xplane.zip" />
<CopyToOutputDirectory>Always</CopyToOutputDirectory>
</None>
<None Include="m3u\GeoRefnetworklink.kml"> <None Include="m3u\GeoRefnetworklink.kml">
<CopyToOutputDirectory>Always</CopyToOutputDirectory> <CopyToOutputDirectory>Always</CopyToOutputDirectory>
</None> </None>
@ -1158,9 +1167,7 @@
<CopyToOutputDirectory>Always</CopyToOutputDirectory> <CopyToOutputDirectory>Always</CopyToOutputDirectory>
<SubType>Designer</SubType> <SubType>Designer</SubType>
</Content> </Content>
<Content Include="JSBSim.exe"> <Content Include="JSBSim.exe" />
<CopyToOutputDirectory>Always</CopyToOutputDirectory>
</Content>
<Content Include="mavcmd.xml"> <Content Include="mavcmd.xml">
<CopyToOutputDirectory>Always</CopyToOutputDirectory> <CopyToOutputDirectory>Always</CopyToOutputDirectory>
</Content> </Content>

View File

@ -465,7 +465,6 @@ namespace ArdupilotMega
CH6_STABILIZE_KD = 29 CH6_STABILIZE_KD = 29
} }
public static void linearRegression() public static void linearRegression()
{ {
double[] values = { 4.8, 4.8, 4.5, 3.9, 4.4, 3.6, 3.6, 2.9, 3.5, 3.0, 2.5, 2.2, 2.6, 2.1, 2.2 }; double[] values = { 4.8, 4.8, 4.5, 3.9, 4.4, 3.6, 3.6, 2.9, 3.5, 3.0, 2.5, 2.2, 2.6, 2.1, 2.2 };

View File

@ -31,6 +31,7 @@
this.cmb_Baud = new System.Windows.Forms.ComboBox(); this.cmb_Baud = new System.Windows.Forms.ComboBox();
this.cmb_ConnectionType = new System.Windows.Forms.ComboBox(); this.cmb_ConnectionType = new System.Windows.Forms.ComboBox();
this.cmb_Connection = new System.Windows.Forms.ComboBox(); this.cmb_Connection = new System.Windows.Forms.ComboBox();
this.linkLabel1 = new System.Windows.Forms.LinkLabel();
this.SuspendLayout(); this.SuspendLayout();
// //
// cmb_Baud // cmb_Baud
@ -66,17 +67,28 @@
this.cmb_Connection.Size = new System.Drawing.Size(121, 21); this.cmb_Connection.Size = new System.Drawing.Size(121, 21);
this.cmb_Connection.TabIndex = 2; this.cmb_Connection.TabIndex = 2;
// //
this.linkLabel1.AutoSize = true;
this.linkLabel1.Image = global::ArdupilotMega.Properties.Resources.bg;
this.linkLabel1.Location = new System.Drawing.Point(3, 60);
this.linkLabel1.Name = "linkLabel1";
this.linkLabel1.Size = new System.Drawing.Size(63, 13);
this.linkLabel1.TabIndex = 3;
this.linkLabel1.TabStop = true;
this.linkLabel1.Text = "Link Stats...";
this.linkLabel1.Visible = false;
// ConnectionControl // ConnectionControl
// //
this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F);
this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font;
this.BackgroundImage = global::ArdupilotMega.Properties.Resources.bg; this.BackgroundImage = global::ArdupilotMega.Properties.Resources.bg;
this.Controls.Add(this.linkLabel1);
this.Controls.Add(this.cmb_Connection); this.Controls.Add(this.cmb_Connection);
this.Controls.Add(this.cmb_ConnectionType); this.Controls.Add(this.cmb_ConnectionType);
this.Controls.Add(this.cmb_Baud); this.Controls.Add(this.cmb_Baud);
this.Name = "ConnectionControl"; this.Name = "ConnectionControl";
this.Size = new System.Drawing.Size(230, 76); this.Size = new System.Drawing.Size(230, 76);
this.ResumeLayout(false); this.ResumeLayout(false);
this.PerformLayout();
} }
@ -85,5 +97,6 @@
private System.Windows.Forms.ComboBox cmb_Baud; private System.Windows.Forms.ComboBox cmb_Baud;
private System.Windows.Forms.ComboBox cmb_ConnectionType; private System.Windows.Forms.ComboBox cmb_ConnectionType;
private System.Windows.Forms.ComboBox cmb_Connection; private System.Windows.Forms.ComboBox cmb_Connection;
private System.Windows.Forms.LinkLabel linkLabel1;
} }
} }

View File

@ -14,10 +14,28 @@ namespace ArdupilotMega.Controls
public ConnectionControl() public ConnectionControl()
{ {
InitializeComponent(); InitializeComponent();
this.linkLabel1.Click += (sender, e) =>
{
if (ShowLinkStats!=null)
ShowLinkStats.Invoke(this, EventArgs.Empty);
};
} }
public event EventHandler ShowLinkStats;
public ComboBox CMB_baudrate { get { return this.cmb_Baud; } } public ComboBox CMB_baudrate { get { return this.cmb_Baud; } }
public ComboBox CMB_serialport { get { return this.cmb_Connection; } } public ComboBox CMB_serialport { get { return this.cmb_Connection; } }
public ComboBox TOOL_APMFirmware { get { return this.cmb_ConnectionType; } } public ComboBox TOOL_APMFirmware { get { return this.cmb_ConnectionType; } }
/// <summary>
/// Called from the main form - set whether we are connected or not currently.
/// UI will be updated accordingly
/// </summary>
/// <param name="isConnected">Whether we are connected</param>
public void IsConnected(bool isConnected)
{
this.linkLabel1.Visible = isConnected;
cmb_Baud.Enabled = !isConnected;
cmb_Connection.Enabled = !isConnected;
}
} }
} }

View File

@ -0,0 +1,292 @@
namespace ArdupilotMega.Controls
{
partial class ConnectionStats
{
/// <summary>
/// Required designer variable.
/// </summary>
private System.ComponentModel.IContainer components = null;
/// <summary>
/// Clean up any resources being used.
/// </summary>
/// <param name="disposing">true if managed resources should be disposed; otherwise, false.</param>
protected override void Dispose(bool disposing)
{
if (disposing && (components != null))
{
components.Dispose();
}
base.Dispose(disposing);
}
#region Component Designer generated code
/// <summary>
/// Required method for Designer support - do not modify
/// the contents of this method with the code editor.
/// </summary>
private void InitializeComponent()
{
this.label1 = new System.Windows.Forms.Label();
this.label2 = new System.Windows.Forms.Label();
this.label3 = new System.Windows.Forms.Label();
this.label4 = new System.Windows.Forms.Label();
this.txt_BytesReceived = new System.Windows.Forms.TextBox();
this.txt_BytesPerSecondRx = new System.Windows.Forms.TextBox();
this.txt_PacketsRx = new System.Windows.Forms.TextBox();
this.txt_PacketsLost = new System.Windows.Forms.TextBox();
this.txt_LinkQuality = new System.Windows.Forms.TextBox();
this.label5 = new System.Windows.Forms.Label();
this.txt_PacketsPerSecond = new System.Windows.Forms.TextBox();
this.txt_BytesSent = new System.Windows.Forms.TextBox();
this.label6 = new System.Windows.Forms.Label();
this.label7 = new System.Windows.Forms.Label();
this.txt_BytesPerSecondSent = new System.Windows.Forms.TextBox();
this.label8 = new System.Windows.Forms.Label();
this.groupBox1 = new System.Windows.Forms.GroupBox();
this.label10 = new System.Windows.Forms.Label();
this.txt_MaxPacketInterval = new System.Windows.Forms.TextBox();
this.groupBox2 = new System.Windows.Forms.GroupBox();
this.groupBox1.SuspendLayout();
this.groupBox2.SuspendLayout();
this.SuspendLayout();
//
// label1
//
this.label1.AutoSize = true;
this.label1.Location = new System.Drawing.Point(10, 18);
this.label1.Name = "label1";
this.label1.Size = new System.Drawing.Size(33, 13);
this.label1.TabIndex = 0;
this.label1.Text = "Bytes";
//
// label2
//
this.label2.AutoSize = true;
this.label2.Location = new System.Drawing.Point(135, 18);
this.label2.Name = "label2";
this.label2.Size = new System.Drawing.Size(43, 13);
this.label2.TabIndex = 1;
this.label2.Text = "Bytes/s";
//
// label3
//
this.label3.AutoSize = true;
this.label3.Location = new System.Drawing.Point(6, 44);
this.label3.Name = "label3";
this.label3.Size = new System.Drawing.Size(46, 13);
this.label3.TabIndex = 2;
this.label3.Text = "Packets";
//
// label4
//
this.label4.AutoSize = true;
this.label4.Location = new System.Drawing.Point(3, 70);
this.label4.Name = "label4";
this.label4.Size = new System.Drawing.Size(51, 13);
this.label4.TabIndex = 3;
this.label4.Text = "Pkts Lost";
//
// txt_BytesReceived
//
this.txt_BytesReceived.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle;
this.txt_BytesReceived.Location = new System.Drawing.Point(58, 16);
this.txt_BytesReceived.Name = "txt_BytesReceived";
this.txt_BytesReceived.ReadOnly = true;
this.txt_BytesReceived.Size = new System.Drawing.Size(64, 20);
this.txt_BytesReceived.TabIndex = 4;
//
// txt_BytesPerSecondRx
//
this.txt_BytesPerSecondRx.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle;
this.txt_BytesPerSecondRx.Location = new System.Drawing.Point(184, 16);
this.txt_BytesPerSecondRx.Name = "txt_BytesPerSecondRx";
this.txt_BytesPerSecondRx.ReadOnly = true;
this.txt_BytesPerSecondRx.Size = new System.Drawing.Size(51, 20);
this.txt_BytesPerSecondRx.TabIndex = 5;
//
// txt_PacketsRx
//
this.txt_PacketsRx.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle;
this.txt_PacketsRx.Location = new System.Drawing.Point(58, 42);
this.txt_PacketsRx.Name = "txt_PacketsRx";
this.txt_PacketsRx.ReadOnly = true;
this.txt_PacketsRx.Size = new System.Drawing.Size(64, 20);
this.txt_PacketsRx.TabIndex = 6;
//
// txt_PacketsLost
//
this.txt_PacketsLost.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle;
this.txt_PacketsLost.Location = new System.Drawing.Point(58, 68);
this.txt_PacketsLost.Name = "txt_PacketsLost";
this.txt_PacketsLost.ReadOnly = true;
this.txt_PacketsLost.Size = new System.Drawing.Size(64, 20);
this.txt_PacketsLost.TabIndex = 7;
//
// txt_LinkQuality
//
this.txt_LinkQuality.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle;
this.txt_LinkQuality.Location = new System.Drawing.Point(184, 68);
this.txt_LinkQuality.Name = "txt_LinkQuality";
this.txt_LinkQuality.ReadOnly = true;
this.txt_LinkQuality.Size = new System.Drawing.Size(51, 20);
this.txt_LinkQuality.TabIndex = 9;
//
// label5
//
this.label5.AutoSize = true;
this.label5.Location = new System.Drawing.Point(139, 68);
this.label5.Name = "label5";
this.label5.Size = new System.Drawing.Size(39, 13);
this.label5.TabIndex = 8;
this.label5.Text = "Quality";
//
// txt_PacketsPerSecond
//
this.txt_PacketsPerSecond.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle;
this.txt_PacketsPerSecond.Location = new System.Drawing.Point(184, 42);
this.txt_PacketsPerSecond.Name = "txt_PacketsPerSecond";
this.txt_PacketsPerSecond.ReadOnly = true;
this.txt_PacketsPerSecond.Size = new System.Drawing.Size(51, 20);
this.txt_PacketsPerSecond.TabIndex = 10;
//
// txt_BytesSent
//
this.txt_BytesSent.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle;
this.txt_BytesSent.Location = new System.Drawing.Point(74, 16);
this.txt_BytesSent.Name = "txt_BytesSent";
this.txt_BytesSent.ReadOnly = true;
this.txt_BytesSent.Size = new System.Drawing.Size(64, 20);
this.txt_BytesSent.TabIndex = 12;
//
// label6
//
this.label6.AutoSize = true;
this.label6.Location = new System.Drawing.Point(6, 18);
this.label6.Name = "label6";
this.label6.Size = new System.Drawing.Size(60, 13);
this.label6.TabIndex = 11;
this.label6.Text = "Total Bytes";
//
// label7
//
this.label7.AutoSize = true;
this.label7.Location = new System.Drawing.Point(23, 43);
this.label7.Name = "label7";
this.label7.Size = new System.Drawing.Size(43, 13);
this.label7.TabIndex = 13;
this.label7.Text = "Bytes/s";
//
// txt_BytesPerSecondSent
//
this.txt_BytesPerSecondSent.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle;
this.txt_BytesPerSecondSent.Location = new System.Drawing.Point(74, 42);
this.txt_BytesPerSecondSent.Name = "txt_BytesPerSecondSent";
this.txt_BytesPerSecondSent.ReadOnly = true;
this.txt_BytesPerSecondSent.Size = new System.Drawing.Size(64, 20);
this.txt_BytesPerSecondSent.TabIndex = 14;
//
// label8
//
this.label8.AutoSize = true;
this.label8.Location = new System.Drawing.Point(126, 43);
this.label8.Name = "label8";
this.label8.Size = new System.Drawing.Size(56, 13);
this.label8.TabIndex = 15;
this.label8.Text = "Packets/s";
//
// groupBox1
//
this.groupBox1.Controls.Add(this.label10);
this.groupBox1.Controls.Add(this.txt_MaxPacketInterval);
this.groupBox1.Controls.Add(this.label8);
this.groupBox1.Controls.Add(this.label2);
this.groupBox1.Controls.Add(this.label1);
this.groupBox1.Controls.Add(this.label3);
this.groupBox1.Controls.Add(this.label4);
this.groupBox1.Controls.Add(this.txt_BytesReceived);
this.groupBox1.Controls.Add(this.txt_PacketsPerSecond);
this.groupBox1.Controls.Add(this.txt_BytesPerSecondRx);
this.groupBox1.Controls.Add(this.txt_LinkQuality);
this.groupBox1.Controls.Add(this.txt_PacketsRx);
this.groupBox1.Controls.Add(this.label5);
this.groupBox1.Controls.Add(this.txt_PacketsLost);
this.groupBox1.Location = new System.Drawing.Point(4, 4);
this.groupBox1.Name = "groupBox1";
this.groupBox1.Size = new System.Drawing.Size(245, 124);
this.groupBox1.TabIndex = 16;
this.groupBox1.TabStop = false;
this.groupBox1.Text = "Download";
//
// label10
//
this.label10.AutoSize = true;
this.label10.Location = new System.Drawing.Point(22, 96);
this.label10.Name = "label10";
this.label10.Size = new System.Drawing.Size(156, 13);
this.label10.TabIndex = 16;
this.label10.Text = "Max time between packets (ms)";
//
// txt_MaxPacketInterval
//
this.txt_MaxPacketInterval.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle;
this.txt_MaxPacketInterval.Location = new System.Drawing.Point(184, 94);
this.txt_MaxPacketInterval.Name = "txt_MaxPacketInterval";
this.txt_MaxPacketInterval.ReadOnly = true;
this.txt_MaxPacketInterval.Size = new System.Drawing.Size(51, 20);
this.txt_MaxPacketInterval.TabIndex = 17;
//
// groupBox2
//
this.groupBox2.Controls.Add(this.txt_BytesPerSecondSent);
this.groupBox2.Controls.Add(this.label7);
this.groupBox2.Controls.Add(this.label6);
this.groupBox2.Controls.Add(this.txt_BytesSent);
this.groupBox2.Location = new System.Drawing.Point(255, 4);
this.groupBox2.Name = "groupBox2";
this.groupBox2.Size = new System.Drawing.Size(144, 83);
this.groupBox2.TabIndex = 17;
this.groupBox2.TabStop = false;
this.groupBox2.Text = "Upload";
//
// ConnectionStats
//
this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F);
this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font;
this.Controls.Add(this.groupBox1);
this.Controls.Add(this.groupBox2);
this.Name = "ConnectionStats";
this.Size = new System.Drawing.Size(408, 136);
this.groupBox1.ResumeLayout(false);
this.groupBox1.PerformLayout();
this.groupBox2.ResumeLayout(false);
this.groupBox2.PerformLayout();
this.ResumeLayout(false);
}
#endregion
private System.Windows.Forms.Label label1;
private System.Windows.Forms.Label label2;
private System.Windows.Forms.Label label3;
private System.Windows.Forms.Label label4;
private System.Windows.Forms.TextBox txt_BytesReceived;
private System.Windows.Forms.TextBox txt_BytesPerSecondRx;
private System.Windows.Forms.TextBox txt_PacketsRx;
private System.Windows.Forms.TextBox txt_PacketsLost;
private System.Windows.Forms.TextBox txt_LinkQuality;
private System.Windows.Forms.Label label5;
private System.Windows.Forms.TextBox txt_PacketsPerSecond;
private System.Windows.Forms.TextBox txt_BytesSent;
private System.Windows.Forms.Label label6;
private System.Windows.Forms.Label label7;
private System.Windows.Forms.TextBox txt_BytesPerSecondSent;
private System.Windows.Forms.Label label8;
private System.Windows.Forms.GroupBox groupBox1;
private System.Windows.Forms.GroupBox groupBox2;
private System.Windows.Forms.Label label10;
private System.Windows.Forms.TextBox txt_MaxPacketInterval;
}
}

View File

@ -0,0 +1,171 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Reactive.Disposables;
using System.Reactive.Linq;
using System.Reactive.Subjects;
using System.Threading;
using System.Windows.Forms;
namespace ArdupilotMega.Controls
{
public partial class ConnectionStats : UserControl
{
private readonly MAVLink _mavlink;
private CompositeDisposable _subscriptionsDisposable;
public ConnectionStats(MAVLink comPort) : this()
{
_mavlink = comPort;
this.Load += ConnectionStats_Load;
this.Disposed += (sender, e) => StopUpdates();
}
public ConnectionStats()
{
InitializeComponent();
}
void ConnectionStats_Load(object sender, EventArgs e)
{
_subscriptionsDisposable = new CompositeDisposable();
var packetsReceivedCount = _mavlink.WhenPacketReceived.Scan(0, (x, y) => x + y);
var packetsLostCount = _mavlink.WhenPacketLost.Scan(0, (x, y) => x + y);
var bytesReceivedEverySecond = _mavlink.BytesReceived
.Buffer(TimeSpan.FromSeconds(1))
.Select(bytes => bytes.Sum());
var subscriptions = new List<IDisposable>
{
// Total number of packets received
// but only update the text box at 4Hz
packetsReceivedCount
.Sample(TimeSpan.FromMilliseconds(250))
.SubscribeForTextUpdates(txt_PacketsRx),
packetsLostCount
.Sample(TimeSpan.FromMilliseconds(250))
.SubscribeForTextUpdates(txt_PacketsLost),
// Packets per second = total number of packets received over the
// last 3 seconds divided by three
// Do that every second
_mavlink.WhenPacketReceived
.Buffer(TimeSpan.FromSeconds(3), TimeSpan.FromSeconds(1))
.Select(xs => xs.Sum()/3.0)
.ObserveOn(SynchronizationContext.Current)
.Subscribe(x => this.txt_PacketsPerSecond.Text = x.ToString("0")),
// Link quality is a percentage of the number of good packets received
// to the number of packets missed (detected by mavlink seq no.)
// Calculated as an average over the last 3 seconds (non weighted)
// Calculated every second
CombineWithDefault(_mavlink.WhenPacketReceived, _mavlink.WhenPacketLost, Tuple.Create)
.Buffer(TimeSpan.FromSeconds(3), TimeSpan.FromSeconds(1))
.Select(CalculateAverage)
.ObserveOn(SynchronizationContext.Current)
.Subscribe(x => this.txt_LinkQuality.Text = x.ToString("00%")),
// Bytes per second is the average number of bytes received every second
// sampled for the last 3 seconds
// updated every second
bytesReceivedEverySecond
.Buffer(3, 1)
.Select(xs => (int) xs.Average())
.Select(ToHumanReadableByteCount)
.SubscribeForTextUpdates(txt_BytesPerSecondRx),
// Total bytes received - just count them up,
// but only update the text box at 4Hz so as not to swamp the UI thread
// Also use a human friendly version e.g '1.3K' not 1345
_mavlink.BytesReceived
.Scan(0, (x, y) => x + y)
.Sample(TimeSpan.FromMilliseconds(250))
.Select(ToHumanReadableByteCount)
.SubscribeForTextUpdates(txt_BytesReceived),
_mavlink.BytesSent
.Scan(0, (x, y) => x + y)
.Sample(TimeSpan.FromMilliseconds(250))
.Select(ToHumanReadableByteCount)
.SubscribeForTextUpdates(txt_BytesSent),
_mavlink.BytesSent
.Buffer(TimeSpan.FromSeconds(5), TimeSpan.FromSeconds(1))
.Select(xs => xs.Any() ? xs.Average() : 0)
.Select(x => ToHumanReadableByteCount((int) x))
.SubscribeForTextUpdates(txt_BytesPerSecondSent),
// Observable.Interval(TimeSpan.FromSeconds(1))
// .Scan(TimeSpan.Zero, (a, _) => a.Add(TimeSpan.FromSeconds(1)))
// .ObserveOn(SynchronizationContext.Current)
// .Subscribe(ts => this.txt_TimeConnected.Text = ts.ToString()),
// The maximum length of time between reception of good packets
// evaluated continuously
_mavlink.WhenPacketReceived
.TimeInterval()
.Select(x => x.Interval.Ticks)
.Scan(0L, Math.Max)
.Select(TimeSpan.FromTicks)
.Select(ts => ts.Milliseconds)
.ObserveOn(SynchronizationContext.Current)
.SubscribeForTextUpdates(txt_MaxPacketInterval),
};
subscriptions.ForEach(d => _subscriptionsDisposable.Add(d));
}
public void StopUpdates()
{
_subscriptionsDisposable.Dispose();
}
private static IObservable<TResult> CombineWithDefault<TSource, TResult>(IObservable<TSource> first, Subject<TSource> second, Func<TSource, TSource, TResult> resultSelector)
{
return Observable.Defer(() =>
{
var foo = new Subject<TResult>();
first.Select(x => resultSelector(x, default(TSource))).Subscribe(foo);
second.Select(x => resultSelector(default(TSource), x)).Subscribe(foo);
return foo;
});
}
private static double CalculateAverage(IList<Tuple<int, int>> xs)
{
var packetsReceived = xs.Sum(t => t.Item1);
var packetsLost = xs.Sum(t => t.Item2);
return packetsReceived/(packetsReceived + (double)packetsLost);
}
private static string ToHumanReadableByteCount(int i)
{
if (i > 1024)
return string.Format("{0:0.00}K", i/ (float)1024);
if (i > 1024 * 1024)
return string.Format("{0:0.00}Mb", i / (float)(1024 * 1024));
return string.Format("{0:####}",i);
}
}
public static class CompositeDisposableEx
{
public static IDisposable SubscribeForTextUpdates<T>(this IObservable<T> source, TextBox txtBox)
{
return source
.ObserveOn(SynchronizationContext.Current)
.Subscribe(x => txtBox.Text = x.ToString());
}
}
}

View File

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

View File

@ -296,6 +296,8 @@ namespace ArdupilotMega
// stats // stats
public ushort packetdropremote { get; set; } public ushort packetdropremote { get; set; }
public ushort linkqualitygcs { get; set; } public ushort linkqualitygcs { get; set; }
public ushort hwvoltage { get; set; }
public ushort i2cerrors { get; set; }
// requested stream rates // requested stream rates
public byte rateattitude { get; set; } public byte rateattitude { get; set; }
@ -400,9 +402,22 @@ namespace ArdupilotMega
hilch3 = (int)(hil.throttle * 10000); hilch3 = (int)(hil.throttle * 10000);
hilch4 = (int)(hil.yaw_rudder * 10000); hilch4 = (int)(hil.yaw_rudder * 10000);
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] = null; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_HIL_CONTROLS] = null;
} }
bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_HWSTATUS];
if (bytearray != null)
{
var hwstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_hwstatus_t>(6);
hwvoltage = hwstatus.Vcc;
i2cerrors = hwstatus.I2Cerr;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_HWSTATUS] = null;
}
bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT]; bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT];
if (bytearray != null) if (bytearray != null)

View File

@ -153,6 +153,9 @@ namespace ArdupilotMega
{ {
string option = dataGridView1[0, e.RowIndex].EditedFormattedValue.ToString(); string option = dataGridView1[0, e.RowIndex].EditedFormattedValue.ToString();
if (option.StartsWith("PID-"))
option = "PID-1";
using (XmlReader reader = XmlReader.Create(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + "dataflashlog.xml")) using (XmlReader reader = XmlReader.Create(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + "dataflashlog.xml"))
{ {
reader.Read(); reader.Read();

View File

@ -1,5 +1,6 @@
using System; using System;
using System.Collections.Generic; using System.Collections.Generic;
using System.Reactive.Subjects;
using System.Text; using System.Text;
using System.Runtime.InteropServices; using System.Runtime.InteropServices;
using System.Collections; // hashs using System.Collections; // hashs
@ -58,6 +59,30 @@ namespace ArdupilotMega
/// time last seen a packet of a type /// time last seen a packet of a type
/// </summary> /// </summary>
DateTime[] packetspersecondbuild = new DateTime[256]; DateTime[] packetspersecondbuild = new DateTime[256];
private readonly Subject<int> _bytesReceivedSubj = new Subject<int>();
private readonly Subject<int> _bytesSentSubj = new Subject<int>();
/// <summary>
/// Observable of the count of bytes received, notified when the bytes themselves are received
/// </summary>
public IObservable<int> BytesReceived { get { return _bytesReceivedSubj; } }
/// <summary>
/// Observable of the count of bytes sent, notified when the bytes themselves are received
/// </summary>
public IObservable<int> BytesSent { get { return _bytesSentSubj; } }
/// <summary>
/// Observable of the count of packets skipped (on reception),
/// calculated from periods where received packet sequence is not
/// contiguous
/// </summary>
public readonly Subject<int> WhenPacketLost = new Subject<int>();
public Subject<int> WhenPacketReceived = new Subject<int>();
/// <summary> /// <summary>
/// used as a serial port write lock /// used as a serial port write lock
/// </summary> /// </summary>
@ -163,7 +188,7 @@ namespace ArdupilotMega
OpenBg(false, e); OpenBg(false, e);
} }
private void OpenBg(bool getparams, ProgressWorkerEventArgs progressWorkerEventArgs) private void OpenBg(bool getparams, ProgressWorkerEventArgs progressWorkerEventArgs)
{ {
frmProgressReporter.UpdateProgressAndStatus(-1, "Mavlink Connecting..."); frmProgressReporter.UpdateProgressAndStatus(-1, "Mavlink Connecting...");
@ -286,8 +311,8 @@ namespace ArdupilotMega
countDown.Stop(); countDown.Stop();
// if (Progress != null) // if (Progress != null)
// Progress(-1, "Getting Params.. (sysid " + sysid + " compid " + compid + ") "); // Progress(-1, "Getting Params.. (sysid " + sysid + " compid " + compid + ") ");
frmProgressReporter.UpdateProgressAndStatus(0, "Getting Params.. (sysid " + sysid + " compid " + compid + ") "); frmProgressReporter.UpdateProgressAndStatus(0, "Getting Params.. (sysid " + sysid + " compid " + compid + ") ");
if (getparams) if (getparams)
@ -309,8 +334,8 @@ namespace ArdupilotMega
} }
catch { } catch { }
MainV2.giveComport = false; MainV2.giveComport = false;
// if (Progress != null) // if (Progress != null)
// Progress(-1, "Connect Failed\n" + e.Message); // Progress(-1, "Connect Failed\n" + e.Message);
if (string.IsNullOrEmpty(progressWorkerEventArgs.ErrorMessage)) if (string.IsNullOrEmpty(progressWorkerEventArgs.ErrorMessage))
progressWorkerEventArgs.ErrorMessage = "Connect Failed"; progressWorkerEventArgs.ErrorMessage = "Connect Failed";
throw e; throw e;
@ -429,6 +454,7 @@ namespace ArdupilotMega
{ {
BaseStream.Write(packet, 0, i); BaseStream.Write(packet, 0, i);
} }
_bytesSentSubj.OnNext(i);
} }
try try
@ -474,6 +500,7 @@ namespace ArdupilotMega
{ {
BaseStream.Write(line); BaseStream.Write(line);
} }
_bytesSentSubj.OnNext(line.Length);
return true; return true;
} }
@ -492,7 +519,7 @@ namespace ArdupilotMega
if ((float)param[paramname] == value) if ((float)param[paramname] == value)
{ {
log.Debug("setParam "+paramname + " not modified"); log.Debug("setParam " + paramname + " not modified");
return true; return true;
} }
@ -592,7 +619,7 @@ namespace ArdupilotMega
if (ParamListChanged != null) if (ParamListChanged != null)
{ {
ParamListChanged(this, null); ParamListChanged(this, null);
} }
} }
@ -622,7 +649,7 @@ namespace ArdupilotMega
int param_count = 0; int param_count = 0;
int param_total = 5; int param_total = 5;
goagain: goagain:
mavlink_param_request_list_t req = new mavlink_param_request_list_t(); mavlink_param_request_list_t req = new mavlink_param_request_list_t();
req.target_system = sysid; req.target_system = sysid;
@ -686,12 +713,12 @@ namespace ArdupilotMega
// check if we already have it // check if we already have it
if (got.Contains(par.param_index)) if (got.Contains(par.param_index))
{ {
log.Info("Already got "+(par.param_index) + " '" + paramID + "'"); log.Info("Already got " + (par.param_index) + " '" + paramID + "'");
this.frmProgressReporter.UpdateProgressAndStatus((got.Count * 100) / param_total, "Already Got param " + paramID); this.frmProgressReporter.UpdateProgressAndStatus((got.Count * 100) / param_total, "Already Got param " + paramID);
continue; continue;
} }
log.Info(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + (par.param_count - 2) + " name: " + paramID ); log.Info(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + (par.param_count - 2) + " name: " + paramID);
modifyParamForDisplay(true, paramID, ref par.param_value); modifyParamForDisplay(true, paramID, ref par.param_value);
param[paramID] = (par.param_value); param[paramID] = (par.param_value);
@ -1478,7 +1505,7 @@ namespace ArdupilotMega
public object DebugPacket(byte[] datin) public object DebugPacket(byte[] datin)
{ {
string text = ""; string text = "";
return DebugPacket(datin, ref text,true); return DebugPacket(datin, ref text, true);
} }
public object DebugPacket(byte[] datin, bool PrintToConsole) public object DebugPacket(byte[] datin, bool PrintToConsole)
@ -2055,6 +2082,7 @@ namespace ArdupilotMega
TCPConsole.Write(temp[0]); TCPConsole.Write(temp[0]);
Console.Write((char)temp[0]); Console.Write((char)temp[0]);
} }
_bytesReceivedSubj.OnNext(1);
count = 0; count = 0;
lastbad[0] = lastbad[1]; lastbad[0] = lastbad[1];
lastbad[1] = temp[0]; lastbad[1] = temp[0];
@ -2077,7 +2105,9 @@ namespace ArdupilotMega
if (temp[3] == '3' && temp[4] == 'D') if (temp[3] == '3' && temp[4] == 'D')
{ {
// this is a 3dr radio rssi packet // this is a 3dr radio rssi packet
} else { }
else
{
log.InfoFormat("Mavlink Bad Packet (not addressed to this MAV) got {0} {1} vs {2} {3}", temp[3], temp[4], sysid, compid); log.InfoFormat("Mavlink Bad Packet (not addressed to this MAV) got {0} {1} vs {2} {3}", temp[3], temp[4], sysid, compid);
return new byte[0]; return new byte[0];
} }
@ -2130,6 +2160,8 @@ namespace ArdupilotMega
Array.Resize<byte>(ref temp, count); Array.Resize<byte>(ref temp, count);
_bytesReceivedSubj.OnNext(temp.Length);
if (packetlosttimer.AddSeconds(5) < DateTime.Now) if (packetlosttimer.AddSeconds(5) < DateTime.Now)
{ {
packetlosttimer = DateTime.Now; packetlosttimer = DateTime.Now;
@ -2212,26 +2244,33 @@ namespace ArdupilotMega
} }
else else
{ {
if (temp[2] != ((recvpacketcount + 1) % 0x100)) byte packetSeqNo = temp[2];
int expectedPacketSeqNo = ((recvpacketcount + 1) % 0x100);
if (packetSeqNo != expectedPacketSeqNo)
{ {
synclost++; // actualy sync loss's synclost++; // actualy sync loss's
int numLost = 0;
if (temp[2] < ((recvpacketcount + 1))) if (packetSeqNo < ((recvpacketcount + 1))) // recvpacketcount = 255 then 10 < 256 = true if was % 0x100 this would fail
{ {
packetslost += 0x100 - recvpacketcount + temp[2]; numLost = 0x100 - expectedPacketSeqNo + packetSeqNo;
} }
else else
{ {
packetslost += temp[2] - recvpacketcount; numLost = packetSeqNo - recvpacketcount;
} }
packetslost += numLost;
WhenPacketLost.OnNext(numLost);
log.InfoFormat("lost {0} pktslost {1}", temp[2], (int)packetslost); log.InfoFormat("lost {0} pkts {1}", packetSeqNo, (int)packetslost);
} }
packetsnotlost++; packetsnotlost++;
recvpacketcount = temp[2]; recvpacketcount = packetSeqNo;
WhenPacketReceived.OnNext(1);
// Console.WriteLine(DateTime.Now.Millisecond);
} }
//MAVLINK_MSG_ID_GPS_STATUS //MAVLINK_MSG_ID_GPS_STATUS
@ -2507,8 +2546,5 @@ namespace ArdupilotMega
return temp; return temp;
} }
} }
} }

View File

@ -119,6 +119,7 @@ namespace ArdupilotMega
ArduCopter2, ArduCopter2,
} }
DateTime connectButtonUpdate = DateTime.Now;
/// <summary> /// <summary>
/// declared here if i want a "single" instance of the form /// declared here if i want a "single" instance of the form
/// ie configuration gets reloaded on every click /// ie configuration gets reloaded on every click
@ -131,10 +132,15 @@ namespace ArdupilotMega
GCSViews.Firmware Firmware; GCSViews.Firmware Firmware;
GCSViews.Terminal Terminal; GCSViews.Terminal Terminal;
private Form connectionStatsForm;
private ConnectionStats _connectionStats;
/// <summary> /// <summary>
/// control for the serial port and firmware selector. /// This 'Control' is the toolstrip control that holds the comport combo, baudrate combo etc
/// Otiginally seperate controls, each hosted in a toolstip sqaure, combined into this custom
/// control for layout reasons.
/// </summary> /// </summary>
private ConnectionControl _connectionControl; private readonly ConnectionControl _connectionControl;
public MainV2() public MainV2()
{ {
@ -163,6 +169,7 @@ namespace ArdupilotMega
_connectionControl.CMB_serialport.Click += this.CMB_serialport_Click; _connectionControl.CMB_serialport.Click += this.CMB_serialport_Click;
_connectionControl.TOOL_APMFirmware.SelectedIndexChanged += this.TOOL_APMFirmware_SelectedIndexChanged; _connectionControl.TOOL_APMFirmware.SelectedIndexChanged += this.TOOL_APMFirmware_SelectedIndexChanged;
_connectionControl.ShowLinkStats += (sender, e) => ShowConnectionStatsForm();
srtm.datadirectory = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + "srtm"; srtm.datadirectory = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + "srtm";
var t = Type.GetType("Mono.Runtime"); var t = Type.GetType("Mono.Runtime");
@ -174,7 +181,6 @@ namespace ArdupilotMega
MainMenu.Renderer = new MyRenderer(); MainMenu.Renderer = new MyRenderer();
List<object> list = new List<object>();
foreach (object obj in Enum.GetValues(typeof(Firmwares))) foreach (object obj in Enum.GetValues(typeof(Firmwares)))
{ {
_connectionControl.TOOL_APMFirmware.Items.Add(obj); _connectionControl.TOOL_APMFirmware.Items.Add(obj);
@ -188,14 +194,14 @@ namespace ArdupilotMega
comPort.BaseStream.BaudRate = 115200; comPort.BaseStream.BaudRate = 115200;
// ** Old // ** Old
// CMB_serialport.Items.AddRange(SerialPort.GetPortNames()); // CMB_serialport.Items.AddRange(SerialPort.GetPortNames());
// CMB_serialport.Items.Add("TCP"); // CMB_serialport.Items.Add("TCP");
// CMB_serialport.Items.Add("UDP"); // CMB_serialport.Items.Add("UDP");
// if (CMB_serialport.Items.Count > 0) // if (CMB_serialport.Items.Count > 0)
// { // {
// CMB_baudrate.SelectedIndex = 7; // CMB_baudrate.SelectedIndex = 7;
// CMB_serialport.SelectedIndex = 0; // CMB_serialport.SelectedIndex = 0;
// } // }
// ** new // ** new
_connectionControl.CMB_serialport.Items.AddRange(ArdupilotMega.Comms.SerialPort.GetPortNames()); _connectionControl.CMB_serialport.Items.AddRange(ArdupilotMega.Comms.SerialPort.GetPortNames());
_connectionControl.CMB_serialport.Items.Add("TCP"); _connectionControl.CMB_serialport.Items.Add("TCP");
@ -341,6 +347,46 @@ namespace ArdupilotMega
splash.Close(); splash.Close();
} }
private void ResetConnectionStats()
{
// If the form has been closed, or never shown before, we need do nothing, as
// connection stats will be reset when shown
if (this.connectionStatsForm != null && connectionStatsForm.Visible)
{
// else the form is already showing. reset the stats
this.connectionStatsForm.Controls.Clear();
_connectionStats = new ConnectionStats(comPort);
this.connectionStatsForm.Controls.Add(_connectionStats);
ThemeManager.ApplyThemeTo(this.connectionStatsForm);
}
}
private void ShowConnectionStatsForm()
{
if (this.connectionStatsForm == null || this.connectionStatsForm.IsDisposed)
{
// If the form has been closed, or never shown before, we need all new stuff
this.connectionStatsForm = new Form
{
Width = 430,
Height = 180,
MaximizeBox = false,
MinimizeBox = false,
FormBorderStyle = FormBorderStyle.FixedDialog,
Text = "Link Stats"
};
// Change the connection stats control, so that when/if the connection stats form is showing,
// there will be something to see
this.connectionStatsForm.Controls.Clear();
_connectionStats = new ConnectionStats(comPort);
this.connectionStatsForm.Controls.Add(_connectionStats);
}
this.connectionStatsForm.Show();
ThemeManager.ApplyThemeTo(this.connectionStatsForm);
}
/// <summary> /// <summary>
/// used to create planner screenshots - access by control-s /// used to create planner screenshots - access by control-s
/// </summary> /// </summary>
@ -542,7 +588,10 @@ namespace ArdupilotMega
if (speechEngine != null) // cancel all pending speech if (speechEngine != null) // cancel all pending speech
speechEngine.SpeakAsyncCancelAll(); speechEngine.SpeakAsyncCancelAll();
} }
catch { } catch (Exception ex)
{
log.Error(ex);
}
if (comPort.logfile != null) if (comPort.logfile != null)
comPort.logfile.Close(); comPort.logfile.Close();
@ -553,25 +602,39 @@ namespace ArdupilotMega
comPort.BaseStream.DtrEnable = false; comPort.BaseStream.DtrEnable = false;
comPort.Close(); comPort.Close();
} }
catch (Exception ex) { log.Debug(ex.ToString()); } catch (Exception ex)
{
log.Error(ex);
}
// now that we have closed the connection, cancel the connection stats
// so that the 'time connected' etc does not grow, but the user can still
// look at the now frozen stats on the still open form
((ConnectionStats)this.connectionStatsForm.Controls[0]).StopUpdates();
this.MenuConnect.BackgroundImage = global::ArdupilotMega.Properties.Resources.connect; this.MenuConnect.BackgroundImage = global::ArdupilotMega.Properties.Resources.connect;
} }
else else
{ {
if (_connectionControl.CMB_serialport.Text == "TCP") switch (_connectionControl.CMB_serialport.Text)
{ {
comPort.BaseStream = new TcpSerial(); case "TCP":
} comPort.BaseStream = new TcpSerial();
else if (_connectionControl.CMB_serialport.Text == "UDP") break;
{ case "UDP":
comPort.BaseStream = new UdpSerial(); comPort.BaseStream = new UdpSerial();
} break;
else default:
{ comPort.BaseStream = new Comms.SerialPort();
comPort.BaseStream = new ArdupilotMega.Comms.SerialPort(); break;
} }
// Tell the connection UI that we are now connected.
this._connectionControl.IsConnected(true);
// Here we want to reset the connection stats counter etc.
this.ResetConnectionStats();
try try
{ {
// set port, then options // set port, then options
@ -849,11 +912,18 @@ namespace ArdupilotMega
break; break;
} }
} }
catch (Exception ee) { log.Info(ee.Message); } // silent fail on bad entry // silent fail on bad entry
catch (Exception ee)
{
log.Error(ee);
}
} }
} }
} }
catch (Exception ex) { log.Info("Bad Config File: " + ex.ToString()); } // bad config file catch (Exception ex)
{
log.Error("Bad Config File", ex);
}
} }
} }
@ -939,7 +1009,7 @@ namespace ArdupilotMega
} }
*/ */
// Console.WriteLine(DateTime.Now.Millisecond + " {0} {1} {2} {3} {4}", rc.chan1_raw, rc.chan2_raw, rc.chan3_raw, rc.chan4_raw,rate); // Console.WriteLine(DateTime.Now.Millisecond + " {0} {1} {2} {3} {4}", rc.chan1_raw, rc.chan2_raw, rc.chan3_raw, rc.chan4_raw,rate);
comPort.sendPacket(rc); comPort.sendPacket(rc);
count++; count++;
lastjoystick = DateTime.Now; lastjoystick = DateTime.Now;
@ -947,18 +1017,19 @@ namespace ArdupilotMega
} }
} }
System.Threading.Thread.Sleep(20); Thread.Sleep(20);
} }
catch { } // cant fall out catch
{
} // cant fall out
} }
} }
DateTime connectButtonUpdate = DateTime.Now;
/// <summary> /// <summary>
/// Used to fix the icon status for unexpected unplugs etc... /// Used to fix the icon status for unexpected unplugs etc...
/// </summary> /// </summary>
private void updateConnectIcon() private void UpdateConnectIcon()
{ {
if ((DateTime.Now - connectButtonUpdate).Milliseconds > 500) if ((DateTime.Now - connectButtonUpdate).Milliseconds > 500)
{ {
@ -971,8 +1042,7 @@ namespace ArdupilotMega
{ {
this.MenuConnect.BackgroundImage = global::ArdupilotMega.Properties.Resources.disconnect; this.MenuConnect.BackgroundImage = global::ArdupilotMega.Properties.Resources.disconnect;
this.MenuConnect.BackgroundImage.Tag = "Disconnect"; this.MenuConnect.BackgroundImage.Tag = "Disconnect";
_connectionControl.CMB_baudrate.Enabled = false; _connectionControl.IsConnected(true);
_connectionControl.CMB_serialport.Enabled = false;
}); });
} }
} }
@ -984,8 +1054,9 @@ namespace ArdupilotMega
{ {
this.MenuConnect.BackgroundImage = global::ArdupilotMega.Properties.Resources.connect; this.MenuConnect.BackgroundImage = global::ArdupilotMega.Properties.Resources.connect;
this.MenuConnect.BackgroundImage.Tag = "Connect"; this.MenuConnect.BackgroundImage.Tag = "Connect";
_connectionControl.CMB_baudrate.Enabled = true; _connectionControl.IsConnected(false);
_connectionControl.CMB_serialport.Enabled = true; if (_connectionStats != null)
_connectionStats.StopUpdates();
}); });
} }
} }
@ -1022,9 +1093,9 @@ namespace ArdupilotMega
{ {
try try
{ {
System.Threading.Thread.Sleep(5); Thread.Sleep(5);
updateConnectIcon(); UpdateConnectIcon();
if (speechEnable && speechEngine != null && (DateTime.Now - speechcustomtime).TotalSeconds > 30 && MainV2.cs.lat != 0 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen)) if (speechEnable && speechEngine != null && (DateTime.Now - speechcustomtime).TotalSeconds > 30 && MainV2.cs.lat != 0 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen))
{ {
@ -1121,7 +1192,7 @@ namespace ArdupilotMega
} }
catch (Exception e) catch (Exception e)
{ {
log.Info("Serial Reader fail :" + e.Message); log.Error("Serial Reader fail :" + e.Message);
try try
{ {
comPort.Close(); comPort.Close();
@ -1131,6 +1202,10 @@ namespace ArdupilotMega
} }
} }
/// <summary>
/// Override the stock ToolStripProfessionalRenderer to implement 'highlighting' of the
/// currently selected GCS view.
/// </summary>
private class MyRenderer : ToolStripProfessionalRenderer private class MyRenderer : ToolStripProfessionalRenderer
{ {
public static ToolStripItem currentpressed; public static ToolStripItem currentpressed;
@ -1217,11 +1292,11 @@ namespace ArdupilotMega
try try
{ {
CheckForUpdate(); CheckForUpdate();
} }
catch (Exception ex) catch (Exception ex)
{ {
log.Error("Update check failed", ex); log.Error("Update check failed", ex);
} }
} }
@ -1253,7 +1328,11 @@ namespace ArdupilotMega
{ {
listener.Start(); listener.Start();
} }
catch { log.Info("do you have the planner open already"); return; } // in use catch (Exception e)
{
log.Error("Exception starting lister. Possible multiple instances of planner?", e);
return;
} // in use
// Enter the listening loop. // Enter the listening loop.
while (true) while (true)
{ {
@ -1281,12 +1360,12 @@ namespace ArdupilotMega
NetworkStream stream = client.GetStream(); NetworkStream stream = client.GetStream();
System.Text.ASCIIEncoding encoding = new System.Text.ASCIIEncoding(); var asciiEncoding = new ASCIIEncoding();
byte[] request = new byte[1024]; var request = new byte[1024];
int len = stream.Read(request, 0, request.Length); int len = stream.Read(request, 0, request.Length);
string head = System.Text.ASCIIEncoding.ASCII.GetString(request, 0, len); string head = System.Text.Encoding.ASCII.GetString(request, 0, len);
log.Info(head); log.Info(head);
int index = head.IndexOf('\n'); int index = head.IndexOf('\n');
@ -1322,8 +1401,8 @@ namespace ArdupilotMega
while (client.Connected) while (client.Connected)
{ {
System.Threading.Thread.Sleep(200); Thread.Sleep(200);
log.Info(stream.DataAvailable + " " + client.Available); log.Debug(stream.DataAvailable + " " + client.Available);
while (client.Available > 0) while (client.Available > 0)
{ {
@ -1352,7 +1431,7 @@ namespace ArdupilotMega
else if (url.Contains("georefnetwork.kml")) else if (url.Contains("georefnetwork.kml"))
{ {
string header = "HTTP/1.1 200 OK\r\nContent-Type: application/vnd.google-earth.kml+xml\n\n"; string header = "HTTP/1.1 200 OK\r\nContent-Type: application/vnd.google-earth.kml+xml\n\n";
byte[] temp = encoding.GetBytes(header); byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length); stream.Write(temp, 0, temp.Length);
byte[] buffer = Encoding.ASCII.GetBytes(georefkml); byte[] buffer = Encoding.ASCII.GetBytes(georefkml);
@ -1364,7 +1443,7 @@ namespace ArdupilotMega
else if (url.Contains("network.kml")) else if (url.Contains("network.kml"))
{ {
string header = "HTTP/1.1 200 OK\r\nContent-Type: application/vnd.google-earth.kml+xml\n\n"; string header = "HTTP/1.1 200 OK\r\nContent-Type: application/vnd.google-earth.kml+xml\n\n";
byte[] temp = encoding.GetBytes(header); byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length); stream.Write(temp, 0, temp.Length);
SharpKml.Dom.Document kml = new SharpKml.Dom.Document(); SharpKml.Dom.Document kml = new SharpKml.Dom.Document();
@ -1433,7 +1512,7 @@ namespace ArdupilotMega
else if (url.Contains("block_plane_0.dae")) else if (url.Contains("block_plane_0.dae"))
{ {
string header = "HTTP/1.1 200 OK\r\nContent-Type: text/plain\n\n"; string header = "HTTP/1.1 200 OK\r\nContent-Type: text/plain\n\n";
byte[] temp = encoding.GetBytes(header); byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length); stream.Write(temp, 0, temp.Length);
BinaryReader file = new BinaryReader(File.Open("block_plane_0.dae", FileMode.Open, FileAccess.Read, FileShare.Read)); BinaryReader file = new BinaryReader(File.Open("block_plane_0.dae", FileMode.Open, FileAccess.Read, FileShare.Read));
@ -1451,7 +1530,7 @@ namespace ArdupilotMega
else if (url.Contains("hud.html")) else if (url.Contains("hud.html"))
{ {
string header = "HTTP/1.1 200 OK\r\nContent-Type: text/html\n\n"; string header = "HTTP/1.1 200 OK\r\nContent-Type: text/html\n\n";
byte[] temp = encoding.GetBytes(header); byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length); stream.Write(temp, 0, temp.Length);
BinaryReader file = new BinaryReader(File.Open("hud.html", FileMode.Open, FileAccess.Read, FileShare.Read)); BinaryReader file = new BinaryReader(File.Open("hud.html", FileMode.Open, FileAccess.Read, FileShare.Read));
@ -1469,7 +1548,7 @@ namespace ArdupilotMega
else if (url.ToLower().Contains("hud.jpg") || url.ToLower().Contains("map.jpg") || url.ToLower().Contains("both.jpg")) else if (url.ToLower().Contains("hud.jpg") || url.ToLower().Contains("map.jpg") || url.ToLower().Contains("both.jpg"))
{ {
string header = "HTTP/1.1 200 OK\r\nContent-Type: multipart/x-mixed-replace;boundary=APMPLANNER\n\n--APMPLANNER\r\n"; string header = "HTTP/1.1 200 OK\r\nContent-Type: multipart/x-mixed-replace;boundary=APMPLANNER\n\n--APMPLANNER\r\n";
byte[] temp = encoding.GetBytes(header); byte[] temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length); stream.Write(temp, 0, temp.Length);
while (client.Connected) while (client.Connected)
@ -1508,13 +1587,13 @@ namespace ArdupilotMega
} }
header = "Content-Type: image/jpeg\r\nContent-Length: " + data.Length + "\r\n\r\n"; header = "Content-Type: image/jpeg\r\nContent-Length: " + data.Length + "\r\n\r\n";
temp = encoding.GetBytes(header); temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length); stream.Write(temp, 0, temp.Length);
stream.Write(data, 0, data.Length); stream.Write(data, 0, data.Length);
header = "\r\n--APMPLANNER\r\n"; header = "\r\n--APMPLANNER\r\n";
temp = encoding.GetBytes(header); temp = asciiEncoding.GetBytes(header);
stream.Write(temp, 0, temp.Length); stream.Write(temp, 0, temp.Length);
} }
@ -1525,7 +1604,10 @@ namespace ArdupilotMega
} }
stream.Close(); stream.Close();
} }
catch (Exception ee) { log.Info("Failed mjpg " + ee.Message); } catch (Exception ee)
{
log.Error("Failed mjpg ", ee);
}
} }
} }
@ -1730,30 +1812,30 @@ namespace ArdupilotMega
static void DoUpdateWorker_DoWork(object sender, Controls.ProgressWorkerEventArgs e) static void DoUpdateWorker_DoWork(object sender, Controls.ProgressWorkerEventArgs e)
{ {
// TODO: Is this the right place? // TODO: Is this the right place?
#region Fetch Parameter Meta Data #region Fetch Parameter Meta Data
var progressReporterDialogue = ((ProgressReporterDialogue) sender); var progressReporterDialogue = ((ProgressReporterDialogue)sender);
progressReporterDialogue.UpdateProgressAndStatus(-1, "Getting Updated Parameters"); progressReporterDialogue.UpdateProgressAndStatus(-1, "Getting Updated Parameters");
try try
{ {
ParameterMetaDataParser.GetParameterInformation(); ParameterMetaDataParser.GetParameterInformation();
} }
catch (Exception ex) { log.Error(ex.ToString()); CustomMessageBox.Show("Error getting Parameter Information"); } catch (Exception ex) { log.Error(ex.ToString()); CustomMessageBox.Show("Error getting Parameter Information"); }
#endregion Fetch Parameter Meta Data #endregion Fetch Parameter Meta Data
progressReporterDialogue.UpdateProgressAndStatus(-1, "Getting Base URL"); progressReporterDialogue.UpdateProgressAndStatus(-1, "Getting Base URL");
// check for updates // check for updates
if (Debugger.IsAttached) if (Debugger.IsAttached)
{ {
log.Info("Skipping update test as it appears we are debugging"); log.Info("Skipping update test as it appears we are debugging");
} }
else else
{ {
MainV2.updateCheckMain(progressReporterDialogue); MainV2.updateCheckMain(progressReporterDialogue);
} }
} }
private static bool updateCheck(ProgressReporterDialogue frmProgressReporter, string baseurl, string subdir) private static bool updateCheck(ProgressReporterDialogue frmProgressReporter, string baseurl, string subdir)
@ -1920,7 +2002,7 @@ namespace ArdupilotMega
// Get the response. // Get the response.
response = request.GetResponse(); response = request.GetResponse();
// Display the status. // Display the status.
log.Info(((HttpWebResponse)response).StatusDescription); log.Debug(((HttpWebResponse)response).StatusDescription);
// Get the stream containing content returned by the server. // Get the stream containing content returned by the server.
dataStream = response.GetResponseStream(); dataStream = response.GetResponseStream();
@ -1946,7 +2028,7 @@ namespace ArdupilotMega
} }
} }
catch { } catch { }
log.Info(file + " " + bytes); log.Debug(file + " " + bytes);
int len = dataStream.Read(buf1, 0, 1024); int len = dataStream.Read(buf1, 0, 1024);
if (len == 0) if (len == 0)
break; break;
@ -2195,7 +2277,7 @@ namespace ArdupilotMega
private void CMB_baudrate_TextChanged(object sender, EventArgs e) private void CMB_baudrate_TextChanged(object sender, EventArgs e)
{ {
StringBuilder sb = new StringBuilder(); var sb = new StringBuilder();
int baud = 0; int baud = 0;
for (int i = 0; i < _connectionControl.CMB_baudrate.Text.Length; i++) for (int i = 0; i < _connectionControl.CMB_baudrate.Text.Length; i++)
if (char.IsDigit(_connectionControl.CMB_baudrate.Text[i])) if (char.IsDigit(_connectionControl.CMB_baudrate.Text[i]))

View File

@ -34,5 +34,5 @@ using System.Resources;
// by using the '*' as shown below: // by using the '*' as shown below:
// [assembly: AssemblyVersion("1.0.*")] // [assembly: AssemblyVersion("1.0.*")]
[assembly: AssemblyVersion("1.1.*")] [assembly: AssemblyVersion("1.1.*")]
[assembly: AssemblyFileVersion("1.1.81")] [assembly: AssemblyFileVersion("1.1.82")]
[assembly: NeutralResourcesLanguageAttribute("")] [assembly: NeutralResourcesLanguageAttribute("")]

View File

@ -172,6 +172,10 @@ namespace ArdupilotMega
log.Info(message); log.Info(message);
Application.DoEvents(); Application.DoEvents();
} }
else if (level < 5) // 5 = byte data
{
log.Debug(message);
}
} }
catch { } catch { }
} }

View File

@ -33,8 +33,8 @@ namespace uploader
REBOOT = 0x30, REBOOT = 0x30,
// protocol constants // protocol constants
PROG_MULTI_MAX = 64, // maximum number of bytes in a PROG_MULTI command PROG_MULTI_MAX = 32, // maximum number of bytes in a PROG_MULTI command
READ_MULTI_MAX = 64, // from 255 // largest read that can be requested READ_MULTI_MAX = 255, // largest read that can be requested
// device IDs XXX should come with the firmware image... // device IDs XXX should come with the firmware image...
DEVICE_ID_RF50 = 0x4d, DEVICE_ID_RF50 = 0x4d,