Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

This commit is contained in:
Chris Anderson 2012-03-22 07:37:29 -07:00
parent 8e5c321623 5f3e2cf98a
commit 5c8630efec
31 changed files with 5010 additions and 7057 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduCopter/motors_quad.pde
View File

@ -178,24 +178,24 @@ static void output_motor_test()
}else{ }else{
APM_RC.OutputCh(MOT_3, g.rc_2.radio_min); APM_RC.OutputCh(MOT_2, g.rc_2.radio_min);
delay(4000); delay(4000);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
delay(300); delay(300);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
delay(2000); delay(2000);
APM_RC.OutputCh(MOT_2, g.rc_1.radio_min + 100); APM_RC.OutputCh(MOT_1, g.rc_1.radio_min + 100);
delay(300); delay(300);
APM_RC.OutputCh(MOT_2, g.rc_1.radio_min); APM_RC.OutputCh(MOT_1, g.rc_1.radio_min);
delay(2000); delay(2000);
APM_RC.OutputCh(MOT_4, g.rc_4.radio_min + 100); APM_RC.OutputCh(MOT_4, g.rc_4.radio_min + 100);
delay(300); delay(300);
APM_RC.OutputCh(MOT_4, g.rc_4.radio_min); APM_RC.OutputCh(MOT_4, g.rc_4.radio_min);
delay(2000); delay(2000);
APM_RC.OutputCh(MOT_3, g.rc_2.radio_min + 100); APM_RC.OutputCh(MOT_2, g.rc_2.radio_min + 100);
delay(300); delay(300);
} }

4
Tools/ArdupilotMegaPlanner/Common.cs
View File

@ -380,8 +380,8 @@ namespace ArdupilotMega
CH6_OPTFLOW_KD = 19, CH6_OPTFLOW_KD = 19,
CH6_NAV_I = 20, CH6_NAV_I = 20,
CH6_LOITER_RATE_P = 22,
CH6_LOITER_RATE_P = 22 CH6_LOITER_RATE_D = 23
} }

10
Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs
View File

@ -19,6 +19,8 @@ namespace System.IO.Ports
byte[] rbuffer = new byte[0]; byte[] rbuffer = new byte[0];
int rbufferread = 0; int rbufferread = 0;
int retrys = 3;
public int WriteBufferSize { get; set; } public int WriteBufferSize { get; set; }
public int WriteTimeout { get; set; } public int WriteTimeout { get; set; }
public int ReceivedBytesThreshold { get; set; } public int ReceivedBytesThreshold { get; set; }
@ -121,6 +123,14 @@ namespace System.IO.Ports
client.Close(); client.Close();
} }
catch { } catch { }
// this should only happen if we have established a connection in the first place
if (client != null && retrys > 0)
{
client.Connect(ArdupilotMega.MainV2.config["TCP_host"].ToString(), int.Parse(ArdupilotMega.MainV2.config["TCP_port"].ToString()));
retrys--;
}
throw new Exception("The socket/serialproxy is closed"); throw new Exception("The socket/serialproxy is closed");
} }
} }

88
Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs generated
View File

@ -30,8 +30,8 @@
{ {
this.components = new System.ComponentModel.Container(); this.components = new System.ComponentModel.Container();
System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(Configuration)); System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(Configuration));
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle3 = new System.Windows.Forms.DataGridViewCellStyle(); System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle1 = new System.Windows.Forms.DataGridViewCellStyle();
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle4 = new System.Windows.Forms.DataGridViewCellStyle(); System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle2 = new System.Windows.Forms.DataGridViewCellStyle();
this.Params = new System.Windows.Forms.DataGridView(); this.Params = new System.Windows.Forms.DataGridView();
this.Command = new System.Windows.Forms.DataGridViewTextBoxColumn(); this.Command = new System.Windows.Forms.DataGridViewTextBoxColumn();
this.Value = new System.Windows.Forms.DataGridViewTextBoxColumn(); this.Value = new System.Windows.Forms.DataGridViewTextBoxColumn();
@ -236,6 +236,8 @@
this.RATE_RLL_P = new System.Windows.Forms.NumericUpDown(); this.RATE_RLL_P = new System.Windows.Forms.NumericUpDown();
this.label91 = new System.Windows.Forms.Label(); this.label91 = new System.Windows.Forms.Label();
this.TabPlanner = new System.Windows.Forms.TabPage(); this.TabPlanner = new System.Windows.Forms.TabPage();
this.label33 = new System.Windows.Forms.Label();
this.CMB_ratesensors = new System.Windows.Forms.ComboBox();
this.label26 = new System.Windows.Forms.Label(); this.label26 = new System.Windows.Forms.Label();
this.CMB_videoresolutions = new System.Windows.Forms.ComboBox(); this.CMB_videoresolutions = new System.Windows.Forms.ComboBox();
this.label12 = new System.Windows.Forms.Label(); this.label12 = new System.Windows.Forms.Label();
@ -287,8 +289,8 @@
this.BUT_load = new ArdupilotMega.MyButton(); this.BUT_load = new ArdupilotMega.MyButton();
this.toolTip1 = new System.Windows.Forms.ToolTip(this.components); this.toolTip1 = new System.Windows.Forms.ToolTip(this.components);
this.BUT_compare = new ArdupilotMega.MyButton(); this.BUT_compare = new ArdupilotMega.MyButton();
this.label33 = new System.Windows.Forms.Label(); this.myLabel3 = new ArdupilotMega.MyLabel();
this.CMB_ratesensors = new System.Windows.Forms.ComboBox(); this.myLabel4 = new ArdupilotMega.MyLabel();
((System.ComponentModel.ISupportInitialize)(this.Params)).BeginInit(); ((System.ComponentModel.ISupportInitialize)(this.Params)).BeginInit();
this.ConfigTabs.SuspendLayout(); this.ConfigTabs.SuspendLayout();
this.TabAP.SuspendLayout(); this.TabAP.SuspendLayout();
@ -409,14 +411,14 @@
this.Params.AllowUserToAddRows = false; this.Params.AllowUserToAddRows = false;
this.Params.AllowUserToDeleteRows = false; this.Params.AllowUserToDeleteRows = false;
resources.ApplyResources(this.Params, "Params"); resources.ApplyResources(this.Params, "Params");
dataGridViewCellStyle3.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; dataGridViewCellStyle1.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft;
dataGridViewCellStyle3.BackColor = System.Drawing.Color.Maroon; dataGridViewCellStyle1.BackColor = System.Drawing.Color.Maroon;
dataGridViewCellStyle3.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); dataGridViewCellStyle1.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0)));
dataGridViewCellStyle3.ForeColor = System.Drawing.Color.White; dataGridViewCellStyle1.ForeColor = System.Drawing.Color.White;
dataGridViewCellStyle3.SelectionBackColor = System.Drawing.SystemColors.Highlight; dataGridViewCellStyle1.SelectionBackColor = System.Drawing.SystemColors.Highlight;
dataGridViewCellStyle3.SelectionForeColor = System.Drawing.SystemColors.HighlightText; dataGridViewCellStyle1.SelectionForeColor = System.Drawing.SystemColors.HighlightText;
dataGridViewCellStyle3.WrapMode = System.Windows.Forms.DataGridViewTriState.True; dataGridViewCellStyle1.WrapMode = System.Windows.Forms.DataGridViewTriState.True;
this.Params.ColumnHeadersDefaultCellStyle = dataGridViewCellStyle3; this.Params.ColumnHeadersDefaultCellStyle = dataGridViewCellStyle1;
this.Params.ColumnHeadersHeightSizeMode = System.Windows.Forms.DataGridViewColumnHeadersHeightSizeMode.AutoSize; this.Params.ColumnHeadersHeightSizeMode = System.Windows.Forms.DataGridViewColumnHeadersHeightSizeMode.AutoSize;
this.Params.Columns.AddRange(new System.Windows.Forms.DataGridViewColumn[] { this.Params.Columns.AddRange(new System.Windows.Forms.DataGridViewColumn[] {
this.Command, this.Command,
@ -425,14 +427,14 @@
this.mavScale, this.mavScale,
this.RawValue}); this.RawValue});
this.Params.Name = "Params"; this.Params.Name = "Params";
dataGridViewCellStyle4.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; dataGridViewCellStyle2.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft;
dataGridViewCellStyle4.BackColor = System.Drawing.SystemColors.ActiveCaption; dataGridViewCellStyle2.BackColor = System.Drawing.SystemColors.ActiveCaption;
dataGridViewCellStyle4.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); dataGridViewCellStyle2.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0)));
dataGridViewCellStyle4.ForeColor = System.Drawing.SystemColors.WindowText; dataGridViewCellStyle2.ForeColor = System.Drawing.SystemColors.WindowText;
dataGridViewCellStyle4.SelectionBackColor = System.Drawing.SystemColors.Highlight; dataGridViewCellStyle2.SelectionBackColor = System.Drawing.SystemColors.Highlight;
dataGridViewCellStyle4.SelectionForeColor = System.Drawing.SystemColors.HighlightText; dataGridViewCellStyle2.SelectionForeColor = System.Drawing.SystemColors.HighlightText;
dataGridViewCellStyle4.WrapMode = System.Windows.Forms.DataGridViewTriState.True; dataGridViewCellStyle2.WrapMode = System.Windows.Forms.DataGridViewTriState.True;
this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle4; this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle2;
this.Params.RowHeadersVisible = false; this.Params.RowHeadersVisible = false;
this.Params.CellValueChanged += new System.Windows.Forms.DataGridViewCellEventHandler(this.Params_CellValueChanged); this.Params.CellValueChanged += new System.Windows.Forms.DataGridViewCellEventHandler(this.Params_CellValueChanged);
// //
@ -1093,6 +1095,8 @@
// //
// TabAC // TabAC
// //
this.TabAC.Controls.Add(this.myLabel4);
this.TabAC.Controls.Add(this.myLabel3);
this.TabAC.Controls.Add(this.TUNE_LOW); this.TabAC.Controls.Add(this.TUNE_LOW);
this.TabAC.Controls.Add(this.TUNE_HIGH); this.TabAC.Controls.Add(this.TUNE_HIGH);
this.TabAC.Controls.Add(this.myLabel2); this.TabAC.Controls.Add(this.myLabel2);
@ -1765,6 +1769,25 @@
resources.ApplyResources(this.TabPlanner, "TabPlanner"); resources.ApplyResources(this.TabPlanner, "TabPlanner");
this.TabPlanner.Name = "TabPlanner"; this.TabPlanner.Name = "TabPlanner";
// //
// label33
//
resources.ApplyResources(this.label33, "label33");
this.label33.Name = "label33";
//
// CMB_ratesensors
//
this.CMB_ratesensors.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList;
this.CMB_ratesensors.FormattingEnabled = true;
this.CMB_ratesensors.Items.AddRange(new object[] {
resources.GetString("CMB_ratesensors.Items"),
resources.GetString("CMB_ratesensors.Items1"),
resources.GetString("CMB_ratesensors.Items2"),
resources.GetString("CMB_ratesensors.Items3"),
resources.GetString("CMB_ratesensors.Items4")});
resources.ApplyResources(this.CMB_ratesensors, "CMB_ratesensors");
this.CMB_ratesensors.Name = "CMB_ratesensors";
this.CMB_ratesensors.SelectedIndexChanged += new System.EventHandler(this.CMB_ratesensors_SelectedIndexChanged);
//
// label26 // label26
// //
resources.ApplyResources(this.label26, "label26"); resources.ApplyResources(this.label26, "label26");
@ -2136,24 +2159,17 @@
this.BUT_compare.UseVisualStyleBackColor = true; this.BUT_compare.UseVisualStyleBackColor = true;
this.BUT_compare.Click += new System.EventHandler(this.BUT_compare_Click); this.BUT_compare.Click += new System.EventHandler(this.BUT_compare_Click);
// //
// label33 // myLabel3
// //
resources.ApplyResources(this.label33, "label33"); resources.ApplyResources(this.myLabel3, "myLabel3");
this.label33.Name = "label33"; this.myLabel3.Name = "myLabel3";
this.myLabel3.resize = false;
// //
// CMB_ratesensors // myLabel4
// //
this.CMB_ratesensors.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList; resources.ApplyResources(this.myLabel4, "myLabel4");
this.CMB_ratesensors.FormattingEnabled = true; this.myLabel4.Name = "myLabel4";
this.CMB_ratesensors.Items.AddRange(new object[] { this.myLabel4.resize = false;
resources.GetString("CMB_ratesensors.Items"),
resources.GetString("CMB_ratesensors.Items1"),
resources.GetString("CMB_ratesensors.Items2"),
resources.GetString("CMB_ratesensors.Items3"),
resources.GetString("CMB_ratesensors.Items4")});
resources.ApplyResources(this.CMB_ratesensors, "CMB_ratesensors");
this.CMB_ratesensors.Name = "CMB_ratesensors";
this.CMB_ratesensors.SelectedIndexChanged += new System.EventHandler(this.CMB_ratesensors_SelectedIndexChanged);
// //
// Configuration // Configuration
// //
@ -2546,5 +2562,7 @@
private System.Windows.Forms.NumericUpDown TUNE_HIGH; private System.Windows.Forms.NumericUpDown TUNE_HIGH;
private System.Windows.Forms.Label label33; private System.Windows.Forms.Label label33;
private System.Windows.Forms.ComboBox CMB_ratesensors; private System.Windows.Forms.ComboBox CMB_ratesensors;
private MyLabel myLabel4;
private MyLabel myLabel3;
} }
} }

12
Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs
View File

@ -363,7 +363,9 @@ namespace ArdupilotMega.GCSViews
thisctl.Minimum = -9000; thisctl.Minimum = -9000;
thisctl.Value = (decimal)(float)param[value]; thisctl.Value = (decimal)(float)param[value];
thisctl.Increment = (decimal)0.001; thisctl.Increment = (decimal)0.001;
if (thisctl.Name.EndsWith("_P") || thisctl.Name.EndsWith("_I") || thisctl.Name.EndsWith("_D") || thisctl.Value == 0 || thisctl.Value.ToString("0.###", new System.Globalization.CultureInfo("en-US")).Contains(".")) if (thisctl.Name.EndsWith("_P") || thisctl.Name.EndsWith("_I") || thisctl.Name.EndsWith("_D")
|| thisctl.Name.EndsWith("_LOW") || thisctl.Name.EndsWith("_HIGH") || thisctl.Value == 0
|| thisctl.Value.ToString("0.###", new System.Globalization.CultureInfo("en-US")).Contains("."))
{ {
thisctl.DecimalPlaces = 3; thisctl.DecimalPlaces = 3;
} }
@ -373,6 +375,12 @@ namespace ArdupilotMega.GCSViews
thisctl.DecimalPlaces = 1; thisctl.DecimalPlaces = 1;
} }
if (thisctl.Name.EndsWith("_IMAX"))
{
thisctl.Maximum = 180;
thisctl.Minimum = -180;
}
thisctl.Enabled = true; thisctl.Enabled = true;
thisctl.BackColor = Color.FromArgb(0x43, 0x44, 0x45); thisctl.BackColor = Color.FromArgb(0x43, 0x44, 0x45);
@ -403,7 +411,7 @@ namespace ArdupilotMega.GCSViews
} }
if (text.Length == 0) if (text.Length == 0)
{ {
Console.WriteLine(name + " not found"); //Console.WriteLine(name + " not found");
} }
} }

11167
Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx
View File

File diff suppressed because it is too large Load Diff

11
Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs
View File

@ -74,6 +74,8 @@ namespace ArdupilotMega.GCSViews
public static hud.HUD myhud = null; public static hud.HUD myhud = null;
public static GMapControl mymap = null; public static GMapControl mymap = null;
bool playingLog = false;
public static PointLatLngAlt GuidedModeWP = new PointLatLngAlt(); public static PointLatLngAlt GuidedModeWP = new PointLatLngAlt();
@ -360,6 +362,11 @@ namespace ArdupilotMega.GCSViews
if (MainV2.comPort.logreadmode) if (MainV2.comPort.logreadmode)
MainV2.comPort.logreadmode = false; MainV2.comPort.logreadmode = false;
updatePlayPauseButton(false); updatePlayPauseButton(false);
if (!playingLog && MainV2.comPort.logplaybackfile != null)
{
continue;
}
} }
@ -1025,10 +1032,11 @@ namespace ArdupilotMega.GCSViews
{ {
MainV2.comPort.logreadmode = false; MainV2.comPort.logreadmode = false;
ZedGraphTimer.Stop(); ZedGraphTimer.Stop();
playingLog = false;
} }
else else
{ {
BUT_clear_track_Click(sender, e); // BUT_clear_track_Click(sender, e);
MainV2.comPort.logreadmode = true; MainV2.comPort.logreadmode = true;
list1.Clear(); list1.Clear();
list2.Clear(); list2.Clear();
@ -1045,6 +1053,7 @@ namespace ArdupilotMega.GCSViews
zg1.GraphPane.XAxis.Scale.Min = 0; zg1.GraphPane.XAxis.Scale.Min = 0;
zg1.GraphPane.XAxis.Scale.Max = 1; zg1.GraphPane.XAxis.Scale.Max = 1;
ZedGraphTimer.Start(); ZedGraphTimer.Start();
playingLog = true;
} }
} }

9
Tools/ArdupilotMegaPlanner/Joystick.cs
View File

@ -320,6 +320,15 @@ namespace ArdupilotMega
if (getJoystickAxis(4) != Joystick.joystickaxis.None) if (getJoystickAxis(4) != Joystick.joystickaxis.None)
MainV2.cs.rcoverridech4 = pickchannel(4, JoyChannels[4].axis, JoyChannels[4].reverse, JoyChannels[4].expo);//(ushort)(((int)state.X / 65.535) + 1000); MainV2.cs.rcoverridech4 = pickchannel(4, JoyChannels[4].axis, JoyChannels[4].reverse, JoyChannels[4].expo);//(ushort)(((int)state.X / 65.535) + 1000);
if (getJoystickAxis(5) != Joystick.joystickaxis.None)
MainV2.cs.rcoverridech5 = pickchannel(5, JoyChannels[5].axis, JoyChannels[5].reverse, JoyChannels[5].expo);
if (getJoystickAxis(6) != Joystick.joystickaxis.None)
MainV2.cs.rcoverridech6 = pickchannel(6, JoyChannels[6].axis, JoyChannels[6].reverse, JoyChannels[6].expo);
if (getJoystickAxis(7) != Joystick.joystickaxis.None)
MainV2.cs.rcoverridech7 = pickchannel(7, JoyChannels[7].axis, JoyChannels[7].reverse, JoyChannels[7].expo);
if (getJoystickAxis(8) != Joystick.joystickaxis.None)
MainV2.cs.rcoverridech8 = pickchannel(8, JoyChannels[8].axis, JoyChannels[8].reverse, JoyChannels[8].expo);
foreach (JoyButton but in JoyButtons) foreach (JoyButton but in JoyButtons)
{ {
if (but.buttonno != -1 && getButtonState(but.buttonno)) if (but.buttonno != -1 && getButtonState(but.buttonno))

2
Tools/ArdupilotMegaPlanner/Log.cs
View File

@ -785,7 +785,7 @@ namespace ArdupilotMega
System.Threading.Thread.Sleep(500); System.Threading.Thread.Sleep(500);
comPort.Write("erase\r"); comPort.Write("erase\r");
System.Threading.Thread.Sleep(100); System.Threading.Thread.Sleep(100);
TXT_seriallog.AppendText("!!Allow 30 seconds for erase\n"); TXT_seriallog.AppendText("!!Allow 30-90 seconds for erase\n");
status = serialstatus.Done; status = serialstatus.Done;
CHK_logs.Items.Clear(); CHK_logs.Items.Clear();
} }

2
Tools/ArdupilotMegaPlanner/MagCalib.cs
View File

@ -227,7 +227,7 @@ namespace ArdupilotMega
/// <param name="ofs">offsets</param> /// <param name="ofs">offsets</param>
public static void SaveOffsets(double[] ofs) public static void SaveOffsets(double[] ofs)
{ {
if (MainV2.comPort.param.ContainsKey("COMPASS_OFS_X")) if (MainV2.comPort.param.ContainsKey("COMPASS_OFS_X") && MainV2.comPort.BaseStream.IsOpen)
{ {
try try
{ {

10
Tools/ArdupilotMegaPlanner/MainV2.cs
View File

@ -547,7 +547,7 @@ namespace ArdupilotMega
try try
{ {
Directory.CreateDirectory(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs"); Directory.CreateDirectory(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs");
comPort.logfile = new BinaryWriter(File.Open(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd hh-mm-ss") + ".tlog", FileMode.CreateNew, FileAccess.ReadWrite, FileShare.Read)); comPort.logfile = new BinaryWriter(File.Open(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".tlog", FileMode.CreateNew, FileAccess.ReadWrite, FileShare.Read));
} }
catch { CustomMessageBox.Show("Failed to create log - wont log this session"); } // soft fail catch { CustomMessageBox.Show("Failed to create log - wont log this session"); } // soft fail
@ -863,11 +863,13 @@ namespace ArdupilotMega
} }
} }
DateTime connectButtonUpdate = DateTime.Now;
private void updateConnectIcon() private void updateConnectIcon()
{ {
DateTime menuupdate = DateTime.Now;
if ((DateTime.Now - menuupdate).Milliseconds > 500)
if ((DateTime.Now - connectButtonUpdate).Milliseconds > 500)
{ {
// Console.WriteLine(DateTime.Now.Millisecond); // Console.WriteLine(DateTime.Now.Millisecond);
if (comPort.BaseStream.IsOpen) if (comPort.BaseStream.IsOpen)
@ -896,7 +898,7 @@ namespace ArdupilotMega
}); });
} }
} }
menuupdate = DateTime.Now; connectButtonUpdate = DateTime.Now;
} }
} }

2
Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs
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.0.0.0")] [assembly: AssemblyVersion("1.0.0.0")]
[assembly: AssemblyFileVersion("1.1.54")] [assembly: AssemblyFileVersion("1.1.55")]
[assembly: NeutralResourcesLanguageAttribute("")] [assembly: NeutralResourcesLanguageAttribute("")]

14
Tools/ArdupilotMegaPlanner/Setup/Setup.cs
View File

@ -453,15 +453,17 @@ namespace ArdupilotMega.Setup
CMB_batmontype.SelectedIndex = getIndex(CMB_batmontype,(int)float.Parse(MainV2.comPort.param["BATT_MONITOR"].ToString())); CMB_batmontype.SelectedIndex = getIndex(CMB_batmontype,(int)float.Parse(MainV2.comPort.param["BATT_MONITOR"].ToString()));
} }
if (TXT_ampspervolt.Text == "13.6612") // ignore language re . vs ,
if (TXT_ampspervolt.Text == (13.6612).ToString())
{ {
CMB_batmonsensortype.SelectedIndex = 1; CMB_batmonsensortype.SelectedIndex = 1;
} }
else if (TXT_ampspervolt.Text == "27.3224") else if (TXT_ampspervolt.Text == (27.3224).ToString())
{ {
CMB_batmonsensortype.SelectedIndex = 2; CMB_batmonsensortype.SelectedIndex = 2;
} }
else if (TXT_ampspervolt.Text == "54.64481") else if (TXT_ampspervolt.Text == (54.64481).ToString())
{ {
CMB_batmonsensortype.SelectedIndex = 3; CMB_batmonsensortype.SelectedIndex = 3;
} }
@ -1682,6 +1684,12 @@ namespace ArdupilotMega.Setup
MainV2.cs.ratesensors = backupratesens; MainV2.cs.ratesensors = backupratesens;
if (data.Count < 10)
{
CustomMessageBox.Show("Log does not contain enough data");
return;
}
double[] ans = MagCalib.LeastSq(data); double[] ans = MagCalib.LeastSq(data);
MagCalib.SaveOffsets(ans); MagCalib.SaveOffsets(ans);

BIN
Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb
View File

Binary file not shown.

12
Tools/ArdupilotMegaPlanner/temp.Designer.cs generated
View File

@ -47,6 +47,7 @@
this.BUT_georefimage = new ArdupilotMega.MyButton(); this.BUT_georefimage = new ArdupilotMega.MyButton();
this.BUT_follow_me = new ArdupilotMega.MyButton(); this.BUT_follow_me = new ArdupilotMega.MyButton();
this.BUT_ant_track = new ArdupilotMega.MyButton(); this.BUT_ant_track = new ArdupilotMega.MyButton();
this.BUT_magcalib = new ArdupilotMega.MyButton();
this.SuspendLayout(); this.SuspendLayout();
// //
// button1 // button1
@ -234,11 +235,21 @@
this.BUT_ant_track.UseVisualStyleBackColor = true; this.BUT_ant_track.UseVisualStyleBackColor = true;
this.BUT_ant_track.Click += new System.EventHandler(this.BUT_ant_track_Click); this.BUT_ant_track.Click += new System.EventHandler(this.BUT_ant_track_Click);
// //
// BUT_magcalib
//
this.BUT_magcalib.Location = new System.Drawing.Point(161, 164);
this.BUT_magcalib.Name = "BUT_magcalib";
this.BUT_magcalib.Size = new System.Drawing.Size(96, 23);
this.BUT_magcalib.TabIndex = 19;
this.BUT_magcalib.Text = "Mag Calib";
this.BUT_magcalib.Click += new System.EventHandler(this.BUT_magcalib_Click);
//
// temp // temp
// //
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.ClientSize = new System.Drawing.Size(731, 281); this.ClientSize = new System.Drawing.Size(731, 281);
this.Controls.Add(this.BUT_magcalib);
this.Controls.Add(this.BUT_ant_track); this.Controls.Add(this.BUT_ant_track);
this.Controls.Add(this.BUT_follow_me); this.Controls.Add(this.BUT_follow_me);
this.Controls.Add(this.BUT_georefimage); this.Controls.Add(this.BUT_georefimage);
@ -287,6 +298,7 @@
private MyButton BUT_georefimage; private MyButton BUT_georefimage;
private MyButton BUT_follow_me; private MyButton BUT_follow_me;
private MyButton BUT_ant_track; private MyButton BUT_ant_track;
private MyButton BUT_magcalib;
//private SharpVectors.Renderers.Forms.SvgPictureBox svgPictureBox1; //private SharpVectors.Renderers.Forms.SvgPictureBox svgPictureBox1;
} }

5
Tools/ArdupilotMegaPlanner/temp.cs
View File

@ -886,5 +886,10 @@ namespace ArdupilotMega
{ {
new Antenna.Tracker().Show(); new Antenna.Tracker().Show();
} }
private void BUT_magcalib_Click(object sender, EventArgs e)
{
MagCalib.ProcessLog();
}
} }
} }

1
Tools/autotest/ArduCopter.parm
View File

@ -1,6 +1,7 @@
FRAME 0 FRAME 0
MAG_ENABLE 1 MAG_ENABLE 1
LOG_BITMASK 4095 LOG_BITMASK 4095
COMPASS_DEC 0.211
RC1_MAX 2000.000000 RC1_MAX 2000.000000
RC1_MIN 1000.000000 RC1_MIN 1000.000000
RC1_TRIM 1500.000000 RC1_TRIM 1500.000000

1
Tools/autotest/ArduPlane.parm
View File

@ -9,6 +9,7 @@ ARSPD_FBW_MAX 30
ARSPD_FBW_MIN 10 ARSPD_FBW_MIN 10
KFF_RDDRMIX 0.5 KFF_RDDRMIX 0.5
KFF_PTCHCOMP 0.3 KFF_PTCHCOMP 0.3
COMPASS_DEC 0.211
THR_MAX 100 THR_MAX 100
HDNG2RLL_D 0.5 HDNG2RLL_D 0.5
HDNG2RLL_I 0.01 HDNG2RLL_I 0.01

66
Tools/autotest/pysim/aircraft.py
View File

@ -1,5 +1,5 @@
import euclid, math, util import math, util, rotmat
from rotmat import Vector3, Matrix3
class Aircraft(object): class Aircraft(object):
'''a basic aircraft class''' '''a basic aircraft class'''
@ -14,67 +14,35 @@ class Aircraft(object):
self.longitude = self.home_longitude self.longitude = self.home_longitude
self.altitude = self.home_altitude self.altitude = self.home_altitude
self.pitch = 0.0 # degrees self.dcm = Matrix3()
self.roll = 0.0 # degrees
self.yaw = 0.0 # degrees
# rates in earth frame # rotation rate in body frame
self.pitch_rate = 0.0 # degrees/s self.gyro = Vector3(0,0,0) # rad/s
self.roll_rate = 0.0 # degrees/s
self.yaw_rate = 0.0 # degrees/s
# rates in body frame self.velocity = Vector3(0, 0, 0) # m/s, North, East, Down
self.pDeg = 0.0 # degrees/s self.position = Vector3(0, 0, 0) # m North, East, Down
self.qDeg = 0.0 # degrees/s self.last_velocity = self.velocity.copy()
self.rDeg = 0.0 # degrees/s
self.velocity = euclid.Vector3(0, 0, 0) # m/s, North, East, Up
self.position = euclid.Vector3(0, 0, 0) # m North, East, Up
self.accel = euclid.Vector3(0, 0, 0) # m/s/s North, East, Up
self.mass = 0.0 self.mass = 0.0
self.update_frequency = 50 # in Hz self.update_frequency = 50 # in Hz
self.gravity = 9.8 # m/s/s self.gravity = 9.8 # m/s/s
self.accelerometer = euclid.Vector3(0, 0, -self.gravity) self.accelerometer = Vector3(0, 0, self.gravity)
self.wind = util.Wind('0,0,0') self.wind = util.Wind('0,0,0')
def normalise(self): def update_position(self, delta_time):
'''normalise roll, pitch and yaw
roll between -180 and 180
pitch between -180 and 180
yaw between 0 and 360
'''
def norm(angle, min, max):
while angle > max:
angle -= 360
while angle < min:
angle += 360
return angle
self.roll = norm(self.roll, -180, 180)
self.pitch = norm(self.pitch, -180, 180)
self.yaw = norm(self.yaw, 0, 360)
def update_position(self):
'''update lat/lon/alt from position''' '''update lat/lon/alt from position'''
radius_of_earth = 6378100.0 # in meters radius_of_earth = 6378100.0 # in meters
dlat = math.degrees(math.atan(self.position.x/radius_of_earth)) dlat = math.degrees(math.atan(self.position.x/radius_of_earth))
dlon = math.degrees(math.atan(self.position.y/radius_of_earth))
self.altitude = self.home_altitude + self.position.z
self.latitude = self.home_latitude + dlat self.latitude = self.home_latitude + dlat
lon_scale = math.cos(math.radians(self.latitude));
dlon = math.degrees(math.atan(self.position.y/radius_of_earth))/lon_scale
self.longitude = self.home_longitude + dlon self.longitude = self.home_longitude + dlon
from math import sin, cos, sqrt, radians self.altitude = self.home_altitude - self.position.z
# work out what the accelerometer would see # work out what the accelerometer would see
xAccel = sin(radians(self.pitch)) self.accelerometer = ((self.velocity - self.last_velocity)/delta_time) + Vector3(0,0,self.gravity)
yAccel = -sin(radians(self.roll)) * cos(radians(self.pitch)) # self.accelerometer = Vector3(0,0,-self.gravity)
zAccel = -cos(radians(self.roll)) * cos(radians(self.pitch)) self.accelerometer = self.dcm.transposed() * self.accelerometer
scale = 9.81 / sqrt((xAccel*xAccel)+(yAccel*yAccel)+(zAccel*zAccel)) self.last_velocity = self.velocity.copy()
xAccel *= scale;
yAccel *= scale;
zAccel *= scale;
self.accelerometer = euclid.Vector3(xAccel, yAccel, zAccel)

89
Tools/autotest/pysim/multicopter.py
View File

@ -1,7 +1,9 @@
#!/usr/bin/env python #!/usr/bin/env python
from aircraft import Aircraft from aircraft import Aircraft
import euclid, util, time, math import util, time, math
from math import degrees, radians
from rotmat import Vector3, Matrix3
class Motor(object): class Motor(object):
def __init__(self, angle, clockwise, servo): def __init__(self, angle, clockwise, servo):
@ -83,7 +85,7 @@ class MultiCopter(Aircraft):
self.mass = mass # Kg self.mass = mass # Kg
self.hover_throttle = hover_throttle self.hover_throttle = hover_throttle
self.terminal_velocity = terminal_velocity self.terminal_velocity = terminal_velocity
self.terminal_rotation_rate = 4*360.0 self.terminal_rotation_rate = 4*radians(360.0)
self.frame_height = frame_height self.frame_height = frame_height
# scaling from total motor power to Newtons. Allows the copter # scaling from total motor power to Newtons. Allows the copter
@ -96,6 +98,7 @@ class MultiCopter(Aircraft):
for i in range(0, len(self.motors)): for i in range(0, len(self.motors)):
servo = servos[self.motors[i].servo-1] servo = servos[self.motors[i].servo-1]
if servo <= 0.0: if servo <= 0.0:
self.motor_speed[i] = 0 self.motor_speed[i] = 0
else: else:
self.motor_speed[i] = servo self.motor_speed[i] = servo
@ -107,78 +110,58 @@ class MultiCopter(Aircraft):
delta_time = t - self.last_time delta_time = t - self.last_time
self.last_time = t self.last_time = t
# rotational acceleration, in degrees/s/s, in body frame # rotational acceleration, in rad/s/s, in body frame
roll_accel = 0.0 rot_accel = Vector3(0,0,0)
pitch_accel = 0.0
yaw_accel = 0.0
thrust = 0.0 thrust = 0.0
for i in range(len(self.motors)): for i in range(len(self.motors)):
roll_accel += -5000.0 * math.sin(math.radians(self.motors[i].angle)) * m[i] rot_accel.x += -radians(5000.0) * math.sin(radians(self.motors[i].angle)) * m[i]
pitch_accel += 5000.0 * math.cos(math.radians(self.motors[i].angle)) * m[i] rot_accel.y += radians(5000.0) * math.cos(radians(self.motors[i].angle)) * m[i]
if self.motors[i].clockwise: if self.motors[i].clockwise:
yaw_accel -= m[i] * 400.0 rot_accel.z -= m[i] * radians(400.0)
else: else:
yaw_accel += m[i] * 400.0 rot_accel.z += m[i] * radians(400.0)
thrust += m[i] * self.thrust_scale # newtons thrust += m[i] * self.thrust_scale # newtons
# rotational resistance # rotational air resistance
roll_accel -= (self.pDeg / self.terminal_rotation_rate) * 5000.0 rot_accel.x -= self.gyro.x * radians(5000.0) / self.terminal_rotation_rate
pitch_accel -= (self.qDeg / self.terminal_rotation_rate) * 5000.0 rot_accel.y -= self.gyro.y * radians(5000.0) / self.terminal_rotation_rate
yaw_accel -= (self.rDeg / self.terminal_rotation_rate) * 400.0 rot_accel.z -= self.gyro.z * radians(400.0) / self.terminal_rotation_rate
# update rotational rates in body frame # update rotational rates in body frame
self.pDeg += roll_accel * delta_time self.gyro += rot_accel * delta_time
self.qDeg += pitch_accel * delta_time
self.rDeg += yaw_accel * delta_time
# calculate rates in earth frame # update attitude
(self.roll_rate, self.dcm.rotate(self.gyro * delta_time)
self.pitch_rate,
self.yaw_rate) = util.BodyRatesToEarthRates(self.roll, self.pitch, self.yaw,
self.pDeg, self.qDeg, self.rDeg)
# update rotation
self.roll += self.roll_rate * delta_time
self.pitch += self.pitch_rate * delta_time
self.yaw += self.yaw_rate * delta_time
# air resistance # air resistance
air_resistance = - self.velocity * (self.gravity/self.terminal_velocity) air_resistance = - self.velocity * (self.gravity/self.terminal_velocity)
# normalise rotations accel_body = Vector3(0, 0, -thrust / self.mass)
self.normalise() accel_earth = self.dcm * accel_body
accel_earth += Vector3(0, 0, self.gravity)
accel = thrust / self.mass accel_earth += air_resistance
accel3D = util.RPY_to_XYZ(self.roll, self.pitch, self.yaw, accel)
accel3D += euclid.Vector3(0, 0, -self.gravity)
accel3D += air_resistance
# add in some wind # add in some wind
accel3D += self.wind.accel(self.velocity) accel_earth += self.wind.accel(self.velocity)
# new velocity vector # new velocity vector
self.velocity += accel3D * delta_time self.velocity += accel_earth * delta_time
self.accel = accel3D
# new position vector # new position vector
old_position = self.position.copy() old_position = self.position.copy()
self.position += self.velocity * delta_time self.position += self.velocity * delta_time
# constrain height to the ground # constrain height to the ground
if self.position.z + self.home_altitude < self.ground_level + self.frame_height: if (-self.position.z) + self.home_altitude < self.ground_level + self.frame_height:
if old_position.z + self.home_altitude > self.ground_level + self.frame_height: if (-old_position.z) + self.home_altitude > self.ground_level + self.frame_height:
print("Hit ground at %f m/s" % (-self.velocity.z)) print("Hit ground at %f m/s" % (self.velocity.z))
self.velocity = euclid.Vector3(0, 0, 0)
self.roll_rate = 0 self.velocity = Vector3(0, 0, 0)
self.pitch_rate = 0 # zero roll/pitch, but keep yaw
self.yaw_rate = 0 (r, p, y) = self.dcm.to_euler()
self.roll = 0 self.dcm.from_euler(0, 0, y)
self.pitch = 0
self.accel = euclid.Vector3(0, 0, 0) self.position = Vector3(self.position.x, self.position.y,
self.position = euclid.Vector3(self.position.x, self.position.y, -(self.ground_level + self.frame_height - self.home_altitude))
self.ground_level + self.frame_height - self.home_altitude)
# update lat/lon/altitude # update lat/lon/altitude
self.update_position() self.update_position(delta_time)

268
Tools/autotest/pysim/rotmat.py Normal file
View File

@ -0,0 +1,268 @@
#!/usr/bin/env python
#
# vector3 and rotation matrix classes
# This follows the conventions in the ArduPilot code,
# and is essentially a python version of the AP_Math library
#
# Andrew Tridgell, March 2012
#
# This library is free software; you can redistribute it and/or modify it
# under the terms of the GNU Lesser General Public License as published by the
# Free Software Foundation; either version 2.1 of the License, or (at your
# option) any later version.
#
# This library is distributed in the hope that it will be useful, but WITHOUT
# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
# FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
# for more details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with this library; if not, write to the Free Software Foundation,
# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
'''rotation matrix class
'''
from math import sin, cos, sqrt, asin, atan2, pi, radians, acos
class Vector3:
'''a vector'''
def __init__(self, x=None, y=None, z=None):
if x != None and y != None and z != None:
self.x = x
self.y = y
self.z = z
elif x != None and len(x) == 3:
self.x = x[0]
self.y = x[1]
self.z = x[2]
elif x != None:
raise ValueError('bad initialiser')
else:
self.x = 0
self.y = 0
self.z = 0
def __repr__(self):
return 'Vector3(%.2f, %.2f, %.2f)' % (self.x,
self.y,
self.z)
def __add__(self, v):
return Vector3(self.x + v.x,
self.y + v.y,
self.z + v.z)
__radd__ = __add__
def __sub__(self, v):
return Vector3(self.x - v.x,
self.y - v.y,
self.z - v.z)
def __neg__(self):
return Vector3(-self.x, -self.y, -self.z)
def __rsub__(self, v):
return Vector3(v.x - self.x,
v.y - self.y,
v.z - self.z)
def __mul__(self, v):
if isinstance(v, Vector3):
'''dot product'''
return self.x*v.x + self.y*v.y + self.z*v.z
return Vector3(self.x * v,
self.y * v,
self.z * v)
__rmul__ = __mul__
def __div__(self, v):
return Vector3(self.x / v,
self.y / v,
self.z / v)
def __mod__(self, v):
'''cross product'''
return Vector3(self.y*v.z - self.z*v.y,
self.z*v.x - self.x*v.z,
self.x*v.y - self.y*v.x)
def __copy__(self):
return Vector3(self.x, self.y, self.z)
copy = __copy__
def length(self):
return sqrt(self.x**2 + self.y**2 + self.z**2)
def zero(self):
self.x = self.y = self.z = 0
def angle(self, v):
'''return the angle between this vector and another vector'''
return acos(self * v) / (self.length() * v.length())
def normalized(self):
return self / self.length()
def normalize(self):
v = self.normalized()
self.x = v.x
self.y = v.y
self.z = v.z
class Matrix3:
'''a 3x3 matrix, intended as a rotation matrix'''
def __init__(self, a=None, b=None, c=None):
if a is not None and b is not None and c is not None:
self.a = a.copy()
self.b = b.copy()
self.c = c.copy()
else:
self.identity()
def __repr__(self):
return 'Matrix3((%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f))' % (
self.a.x, self.a.y, self.a.z,
self.b.x, self.b.y, self.b.z,
self.c.x, self.c.y, self.c.z)
def identity(self):
self.a = Vector3(1,0,0)
self.b = Vector3(0,1,0)
self.c = Vector3(0,0,1)
def transposed(self):
return Matrix3(Vector3(self.a.x, self.b.x, self.c.x),
Vector3(self.a.y, self.b.y, self.c.y),
Vector3(self.a.z, self.b.z, self.c.z))
def from_euler(self, roll, pitch, yaw):
'''fill the matrix from Euler angles in radians'''
cp = cos(pitch)
sp = sin(pitch)
sr = sin(roll)
cr = cos(roll)
sy = sin(yaw)
cy = cos(yaw)
self.a.x = cp * cy
self.a.y = (sr * sp * cy) - (cr * sy)
self.a.z = (cr * sp * cy) + (sr * sy)
self.b.x = cp * sy
self.b.y = (sr * sp * sy) + (cr * cy)
self.b.z = (cr * sp * sy) - (sr * cy)
self.c.x = -sp
self.c.y = sr * cp
self.c.z = cr * cp
def to_euler(self):
'''find Euler angles for the matrix'''
if self.c.x >= 1.0:
pitch = pi
elif self.c.x <= -1.0:
pitch = -pi
else:
pitch = -asin(self.c.x)
roll = atan2(self.c.y, self.c.z)
yaw = atan2(self.b.x, self.a.x)
return (roll, pitch, yaw)
def __add__(self, m):
return Matrix3(self.a + m.a, self.b + m.b, self.c + m.c)
__radd__ = __add__
def __sub__(self, m):
return Matrix3(self.a - m.a, self.b - m.b, self.c - m.c)
def __rsub__(self, m):
return Matrix3(m.a - self.a, m.b - self.b, m.c - self.c)
def __mul__(self, other):
if isinstance(other, Vector3):
v = other
return Vector3(self.a.x * v.x + self.a.y * v.y + self.a.z * v.z,
self.b.x * v.x + self.b.y * v.y + self.b.z * v.z,
self.c.x * v.x + self.c.y * v.y + self.c.z * v.z)
elif isinstance(other, Matrix3):
m = other
return Matrix3(Vector3(self.a.x * m.a.x + self.a.y * m.b.x + self.a.z * m.c.x,
self.a.x * m.a.y + self.a.y * m.b.y + self.a.z * m.c.y,
self.a.x * m.a.z + self.a.y * m.b.z + self.a.z * m.c.z),
Vector3(self.b.x * m.a.x + self.b.y * m.b.x + self.b.z * m.c.x,
self.b.x * m.a.y + self.b.y * m.b.y + self.b.z * m.c.y,
self.b.x * m.a.z + self.b.y * m.b.z + self.b.z * m.c.z),
Vector3(self.c.x * m.a.x + self.c.y * m.b.x + self.c.z * m.c.x,
self.c.x * m.a.y + self.c.y * m.b.y + self.c.z * m.c.y,
self.c.x * m.a.z + self.c.y * m.b.z + self.c.z * m.c.z))
v = other
return Matrix3(self.a * v, self.b * v, self.c * v)
def __div__(self, v):
return Matrix3(self.a / v, self.b / v, self.c / v)
def __neg__(self):
return Matrix3(-self.a, -self.b, -self.c)
def __copy__(self):
return Matrix3(self.a, self.b, self.c)
copy = __copy__
def rotate(self, g):
'''rotate the matrix by a given amount on 3 axes'''
temp_matrix = Matrix3()
a = self.a
b = self.b
c = self.c
temp_matrix.a.x = a.y * g.z - a.z * g.y
temp_matrix.a.y = a.z * g.x - a.x * g.z
temp_matrix.a.z = a.x * g.y - a.y * g.x
temp_matrix.b.x = b.y * g.z - b.z * g.y
temp_matrix.b.y = b.z * g.x - b.x * g.z
temp_matrix.b.z = b.x * g.y - b.y * g.x
temp_matrix.c.x = c.y * g.z - c.z * g.y
temp_matrix.c.y = c.z * g.x - c.x * g.z
temp_matrix.c.z = c.x * g.y - c.y * g.x
self.a += temp_matrix.a
self.b += temp_matrix.b
self.c += temp_matrix.c
def normalize(self):
'''re-normalise a rotation matrix'''
error = self.a * self.b
t0 = self.a - (self.b * (0.5 * error))
t1 = self.b - (self.a * (0.5 * error))
t2 = t0 % t1
self.a = t0 * (1.0 / t0.length())
self.b = t1 * (1.0 / t1.length())
self.c = t2 * (1.0 / t2.length())
def trace(self):
'''the trace of the matrix'''
return self.a.x + self.b.y + self.c.z
def test_euler():
'''check that from_euler() and to_euler() are consistent'''
m = Matrix3()
from math import radians, degrees
for r in range(-179, 179, 3):
for p in range(-89, 89, 3):
for y in range(-179, 179, 3):
m.from_euler(radians(r), radians(p), radians(y))
(r2, p2, y2) = m.to_euler()
v1 = Vector3(r,p,y)
v2 = Vector3(degrees(r2),degrees(p2),degrees(y2))
diff = v1 - v2
if diff.length() > 1.0e-12:
print('EULER ERROR:', v1, v2, diff.length())
if __name__ == "__main__":
import doctest
doctest.testmod()
test_euler()

26
Tools/autotest/pysim/sim_multicopter.py
View File

@ -1,7 +1,7 @@
#!/usr/bin/env python #!/usr/bin/env python
from multicopter import MultiCopter from multicopter import MultiCopter
import euclid, util, time, os, sys, math import util, time, os, sys, math
import socket, struct import socket, struct
import select, fgFDM, errno import select, fgFDM, errno
@ -16,16 +16,20 @@ import mavlink
def sim_send(m, a): def sim_send(m, a):
'''send flight information to mavproxy and flightgear''' '''send flight information to mavproxy and flightgear'''
global fdm global fdm
from math import degrees
earth_rates = util.BodyRatesToEarthRates(a.dcm, a.gyro)
(roll, pitch, yaw) = a.dcm.to_euler()
fdm.set('latitude', a.latitude, units='degrees') fdm.set('latitude', a.latitude, units='degrees')
fdm.set('longitude', a.longitude, units='degrees') fdm.set('longitude', a.longitude, units='degrees')
fdm.set('altitude', a.altitude, units='meters') fdm.set('altitude', a.altitude, units='meters')
fdm.set('phi', a.roll, units='degrees') fdm.set('phi', roll, units='radians')
fdm.set('theta', a.pitch, units='degrees') fdm.set('theta', pitch, units='radians')
fdm.set('psi', a.yaw, units='degrees') fdm.set('psi', yaw, units='radians')
fdm.set('phidot', a.roll_rate, units='dps') fdm.set('phidot', earth_rates.x, units='rps')
fdm.set('thetadot', a.pitch_rate, units='dps') fdm.set('thetadot', earth_rates.y, units='rps')
fdm.set('psidot', a.yaw_rate, units='dps') fdm.set('psidot', earth_rates.z, units='rps')
fdm.set('vcas', math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y), units='mps') fdm.set('vcas', math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y), units='mps')
fdm.set('v_north', a.velocity.x, units='mps') fdm.set('v_north', a.velocity.x, units='mps')
fdm.set('v_east', a.velocity.y, units='mps') fdm.set('v_east', a.velocity.y, units='mps')
@ -40,11 +44,11 @@ def sim_send(m, a):
raise raise
buf = struct.pack('<16dI', buf = struct.pack('<16dI',
a.latitude, a.longitude, a.altitude, a.yaw, a.latitude, a.longitude, a.altitude, degrees(yaw),
a.velocity.x, a.velocity.y, a.velocity.x, a.velocity.y,
a.accelerometer.x, a.accelerometer.y, a.accelerometer.z, -a.accelerometer.x, -a.accelerometer.y, -a.accelerometer.z,
a.roll_rate, a.pitch_rate, a.yaw_rate, degrees(earth_rates.x), degrees(earth_rates.y), degrees(earth_rates.z),
a.roll, a.pitch, a.yaw, degrees(roll), degrees(pitch), degrees(yaw),
math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y), math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y),
0x4c56414e) 0x4c56414e)
try: try:

4
Tools/autotest/pysim/testwind.py
View File

@ -1,12 +1,12 @@
#!/usr/bin/env python #!/usr/bin/env python
# simple test of wind generation code # simple test of wind generation code
import util, euclid, time, random import util, time, random
wind = util.Wind('3,90,0.1') wind = util.Wind('3,90,0.1')
t0 = time.time() t0 = time.time()
velocity = euclid.Vector3(0,0,0) velocity = Vector3(0,0,0)
t = 0 t = 0
deltat = 0.01 deltat = 0.01

134
Tools/autotest/pysim/util.py
View File

@ -1,90 +1,8 @@
import euclid, math import math
import os, pexpect, sys, time, random import os, pexpect, sys, time, random
from rotmat import Vector3, Matrix3
from subprocess import call, check_call,Popen, PIPE from subprocess import call, check_call,Popen, PIPE
def RPY_to_XYZ(roll, pitch, yaw, length):
'''convert roll, pitch and yaw in degrees to
Vector3 in X, Y and Z
inputs:
roll, pitch and yaw are in degrees
yaw == 0 when pointing North
roll == zero when horizontal. +ve roll means tilting to the right
pitch == zero when horizontal, +ve pitch means nose is pointing upwards
length is in an arbitrary linear unit.
When RPY is (0, 0, 0) then length represents distance upwards
outputs:
Vector3:
X is in units along latitude. +ve X means going North
Y is in units along longitude +ve Y means going East
Z is altitude in units (+ve is up)
test suite:
>>> RPY_to_XYZ(0, 0, 0, 1)
Vector3(0.00, 0.00, 1.00)
>>> RPY_to_XYZ(0, 0, 0, 2)
Vector3(0.00, 0.00, 2.00)
>>> RPY_to_XYZ(90, 0, 0, 1)
Vector3(0.00, 1.00, 0.00)
>>> RPY_to_XYZ(-90, 0, 0, 1)
Vector3(0.00, -1.00, 0.00)
>>> RPY_to_XYZ(0, 90, 0, 1)
Vector3(-1.00, 0.00, 0.00)
>>> RPY_to_XYZ(0, -90, 0, 1)
Vector3(1.00, 0.00, 0.00)
>>> RPY_to_XYZ(90, 0, 180, 1)
Vector3(-0.00, -1.00, 0.00)
>>> RPY_to_XYZ(-90, 0, 180, 1)
Vector3(0.00, 1.00, 0.00)
>>> RPY_to_XYZ(0, 90, 180, 1)
Vector3(1.00, -0.00, 0.00)
>>> RPY_to_XYZ(0, -90, 180, 1)
Vector3(-1.00, 0.00, 0.00)
>>> RPY_to_XYZ(90, 0, 90, 1)
Vector3(-1.00, 0.00, 0.00)
>>> RPY_to_XYZ(-90, 0, 90, 1)
Vector3(1.00, -0.00, 0.00)
>>> RPY_to_XYZ(90, 0, 270, 1)
Vector3(1.00, -0.00, 0.00)
>>> RPY_to_XYZ(-90, 0, 270, 1)
Vector3(-1.00, 0.00, 0.00)
>>> RPY_to_XYZ(0, 90, 90, 1)
Vector3(-0.00, -1.00, 0.00)
>>> RPY_to_XYZ(0, -90, 90, 1)
Vector3(0.00, 1.00, 0.00)
>>> RPY_to_XYZ(0, 90, 270, 1)
Vector3(0.00, 1.00, 0.00)
>>> RPY_to_XYZ(0, -90, 270, 1)
Vector3(-0.00, -1.00, 0.00)
'''
v = euclid.Vector3(0, 0, length)
v = euclid.Quaternion.new_rotate_euler(-math.radians(pitch), 0, -math.radians(roll)) * v
v = euclid.Quaternion.new_rotate_euler(0, math.radians(yaw), 0) * v
return v
def m2ft(x): def m2ft(x):
'''meters to feet''' '''meters to feet'''
return float(x) / 0.3048 return float(x) / 0.3048
@ -289,53 +207,48 @@ def check_parent(parent_pid=os.getppid()):
sys.exit(1) sys.exit(1)
def EarthRatesToBodyRates(roll, pitch, yaw, def EarthRatesToBodyRates(dcm, earth_rates):
rollRate, pitchRate, yawRate):
'''convert the angular velocities from earth frame to '''convert the angular velocities from earth frame to
body frame. Thanks to James Goppert for the formula body frame. Thanks to James Goppert for the formula
all inputs and outputs are in degrees all inputs and outputs are in radians
returns a tuple, (p,q,r) returns a gyro vector in body frame, in rad/s
''' '''
from math import radians, degrees, sin, cos, tan from math import sin, cos
phi = radians(roll) (phi, theta, psi) = dcm.to_euler()
theta = radians(pitch) phiDot = earth_rates.x
phiDot = radians(rollRate) thetaDot = earth_rates.y
thetaDot = radians(pitchRate) psiDot = earth_rates.z
psiDot = radians(yawRate)
p = phiDot - psiDot*sin(theta) p = phiDot - psiDot*sin(theta)
q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta) q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta)
r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot
return Vector3(p, q, r)
return (degrees(p), degrees(q), degrees(r)) def BodyRatesToEarthRates(dcm, gyro):
def BodyRatesToEarthRates(roll, pitch, yaw, pDeg, qDeg, rDeg):
'''convert the angular velocities from body frame to '''convert the angular velocities from body frame to
earth frame. earth frame.
all inputs and outputs are in degrees all inputs and outputs are in radians/s
returns a tuple, (rollRate,pitchRate,yawRate) returns a earth rate vector
''' '''
from math import radians, degrees, sin, cos, tan, fabs from math import sin, cos, tan, fabs
p = radians(pDeg) p = gyro.x
q = radians(qDeg) q = gyro.y
r = radians(rDeg) r = gyro.z
phi = radians(roll) (phi, theta, psi) = dcm.to_euler()
theta = radians(pitch)
phiDot = p + tan(theta)*(q*sin(phi) + r*cos(phi)) phiDot = p + tan(theta)*(q*sin(phi) + r*cos(phi))
thetaDot = q*cos(phi) - r*sin(phi) thetaDot = q*cos(phi) - r*sin(phi)
if fabs(cos(theta)) < 1.0e-20: if fabs(cos(theta)) < 1.0e-20:
theta += 1.0e-10 theta += 1.0e-10
psiDot = (q*sin(phi) + r*cos(phi))/cos(theta) psiDot = (q*sin(phi) + r*cos(phi))/cos(theta)
return Vector3(phiDot, thetaDot, psiDot)
return (degrees(phiDot), degrees(thetaDot), degrees(psiDot))
class Wind(object): class Wind(object):
@ -382,12 +295,15 @@ class Wind(object):
'''return current wind acceleration in ground frame. The '''return current wind acceleration in ground frame. The
velocity is a Vector3 of the current velocity of the aircraft velocity is a Vector3 of the current velocity of the aircraft
in earth frame, m/s''' in earth frame, m/s'''
from math import radians
(speed, direction) = self.current(deltat=deltat) (speed, direction) = self.current(deltat=deltat)
# wind vector # wind vector
v = euclid.Vector3(speed, 0, 0) v = Vector3(speed, 0, 0)
wind = euclid.Quaternion.new_rotate_euler(0, math.radians(direction), 0) * v m = Matrix3()
m.from_euler(0, 0, radians(direction))
wind = m.transposed() * v
# relative wind vector # relative wind vector
relwind = wind - velocity relwind = wind - velocity

1
libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde
View File

@ -21,6 +21,7 @@
#include <DataFlash.h> #include <DataFlash.h>
#include <APM_RC.h> #include <APM_RC.h>
#include <GCS_MAVLink.h> #include <GCS_MAVLink.h>
#include <Filter.h>
// uncomment this for a APM2 board // uncomment this for a APM2 board
#define APM2_HARDWARE #define APM2_HARDWARE

1
libraries/AP_Math/examples/eulers/eulers.pde
View File

@ -22,6 +22,7 @@ FastSerialPort(Serial, 0);
#include <AP_Baro.h> #include <AP_Baro.h>
#include <AP_Compass.h> #include <AP_Compass.h>
#include <AP_GPS.h> #include <AP_GPS.h>
#include <Filter.h>
Arduino_Mega_ISR_Registry isr_registry; Arduino_Mega_ISR_Registry isr_registry;
AP_Baro_BMP085_HIL barometer; AP_Baro_BMP085_HIL barometer;
AP_Compass_HIL compass; AP_Compass_HIL compass;

29
libraries/AP_Math/matrix3.cpp
View File

@ -153,8 +153,37 @@ void Matrix3<T>::rotate(const Vector3<T> &g)
(*this) += temp_matrix; (*this) += temp_matrix;
} }
// multiplication by a vector
template <typename T>
Vector3<T> Matrix3<T>::operator *(const Vector3<T> &v) const
{
return Vector3<T>(a.x * v.x + a.y * v.y + a.z * v.z,
b.x * v.x + b.y * v.y + b.z * v.z,
c.x * v.x + c.y * v.y + c.z * v.z);
}
// multiplication by another Matrix3<T>
template <typename T>
Matrix3<T> Matrix3<T>::operator *(const Matrix3<T> &m) const
{
Matrix3<T> temp (Vector3<T>(a.x * m.a.x + a.y * m.b.x + a.z * m.c.x,
a.x * m.a.y + a.y * m.b.y + a.z * m.c.y,
a.x * m.a.z + a.y * m.b.z + a.z * m.c.z),
Vector3<T>(b.x * m.a.x + b.y * m.b.x + b.z * m.c.x,
b.x * m.a.y + b.y * m.b.y + b.z * m.c.y,
b.x * m.a.z + b.y * m.b.z + b.z * m.c.z),
Vector3<T>(c.x * m.a.x + c.y * m.b.x + c.z * m.c.x,
c.x * m.a.y + c.y * m.b.y + c.z * m.c.y,
c.x * m.a.z + c.y * m.b.z + c.z * m.c.z));
return temp;
}
// only define for float // only define for float
template void Matrix3<float>::rotation(enum Rotation); template void Matrix3<float>::rotation(enum Rotation);
template void Matrix3<float>::rotate(const Vector3<float> &g); template void Matrix3<float>::rotate(const Vector3<float> &g);
template void Matrix3<float>::from_euler(float roll, float pitch, float yaw); template void Matrix3<float>::from_euler(float roll, float pitch, float yaw);
template void Matrix3<float>::to_euler(float *roll, float *pitch, float *yaw); template void Matrix3<float>::to_euler(float *roll, float *pitch, float *yaw);
template Vector3<float> Matrix3<float>::operator *(const Vector3<float> &v) const;
template Matrix3<float> Matrix3<float>::operator *(const Matrix3<float> &m) const;

22
libraries/AP_Math/matrix3.h
View File

@ -90,27 +90,11 @@ public:
{ return *this = *this / num; } { return *this = *this / num; }
// multiplication by a vector // multiplication by a vector
Vector3<T> operator *(const Vector3<T> &v) const Vector3<T> operator *(const Vector3<T> &v) const;
{
return Vector3<T>(a.x * v.x + a.y * v.y + a.z * v.z,
b.x * v.x + b.y * v.y + b.z * v.z,
c.x * v.x + c.y * v.y + c.z * v.z);
}
// multiplication by another Matrix3<T> // multiplication by another Matrix3<T>
Matrix3<T> operator *(const Matrix3<T> &m) const Matrix3<T> operator *(const Matrix3<T> &m) const;
{
Matrix3<T> temp (Vector3<T>(a.x * m.a.x + a.y * m.b.x + a.z * m.c.x,
a.x * m.a.y + a.y * m.b.y + a.z * m.c.y,
a.x * m.a.z + a.y * m.b.z + a.z * m.c.z),
Vector3<T>(b.x * m.a.x + b.y * m.b.x + b.z * m.c.x,
b.x * m.a.y + b.y * m.b.y + b.z * m.c.y,
b.x * m.a.z + b.y * m.b.z + b.z * m.c.z),
Vector3<T>(c.x * m.a.x + c.y * m.b.x + c.z * m.c.x,
c.x * m.a.y + c.y * m.b.y + c.z * m.c.y,
c.x * m.a.z + c.y * m.b.z + c.z * m.c.z));
return temp;
}
Matrix3<T> &operator *=(const Matrix3<T> &m) Matrix3<T> &operator *=(const Matrix3<T> &m)
{ return *this = *this * m; } { return *this = *this * m; }

2
libraries/Desktop/support/desktop.h
View File

@ -20,7 +20,7 @@ void sitl_input(void);
void sitl_setup(void); void sitl_setup(void);
int sitl_gps_pipe(void); int sitl_gps_pipe(void);
ssize_t sitl_gps_read(int fd, void *buf, size_t count); ssize_t sitl_gps_read(int fd, void *buf, size_t count);
void sitl_update_compass(float heading, float roll, float pitch, float yaw); void sitl_update_compass(float roll, float pitch, float yaw);
void sitl_update_gps(double latitude, double longitude, float altitude, void sitl_update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, bool have_lock); double speedN, double speedE, bool have_lock);
void sitl_update_adc(float roll, float pitch, float yaw, void sitl_update_adc(float roll, float pitch, float yaw,

4
libraries/Desktop/support/sitl.cpp
View File

@ -278,7 +278,7 @@ static void timer_handler(int signum)
sim_state.xAccel, sim_state.yAccel, sim_state.zAccel, sim_state.xAccel, sim_state.yAccel, sim_state.zAccel,
sim_state.airspeed); sim_state.airspeed);
sitl_update_barometer(sim_state.altitude); sitl_update_barometer(sim_state.altitude);
sitl_update_compass(sim_state.heading, sim_state.rollDeg, sim_state.pitchDeg, sim_state.heading); sitl_update_compass(sim_state.rollDeg, sim_state.pitchDeg, sim_state.heading);
sei(); sei();
} }
@ -326,7 +326,7 @@ void sitl_setup(void)
// setup some initial values // setup some initial values
sitl_update_barometer(desktop_state.initial_height); sitl_update_barometer(desktop_state.initial_height);
sitl_update_adc(0, 0, 0, 0, 0, 0, 0, 0, -9.8, 0); sitl_update_adc(0, 0, 0, 0, 0, 0, 0, 0, -9.8, 0);
sitl_update_compass(0, 0, 0, 0); sitl_update_compass(0, 0, 0);
sitl_update_gps(0, 0, 0, 0, 0, false); sitl_update_gps(0, 0, 0, 0, 0, false);
} }

55
libraries/Desktop/support/sitl_compass.cpp
View File

@ -20,40 +20,40 @@
#define MAG_OFS_Y 13.0 #define MAG_OFS_Y 13.0
#define MAG_OFS_Z -18.0 #define MAG_OFS_Z -18.0
// declination in Canberra (degrees)
#define MAG_DECLINATION 12.1
// inclination in Canberra (degrees)
#define MAG_INCLINATION -66
// magnetic field strength in Canberra as observed
// using an APM1 with 5883L compass
#define MAG_FIELD_STRENGTH 818
/* /*
given a magnetic heading, and roll, pitch, yaw values, given a magnetic heading, and roll, pitch, yaw values,
calculate consistent magnetometer components calculate consistent magnetometer components
All angles are in radians All angles are in radians
*/ */
static Vector3f heading_to_mag(float heading, float roll, float pitch, float yaw) static Vector3f heading_to_mag(float roll, float pitch, float yaw)
{ {
Vector3f v; Vector3f Bearth, m;
double headX, headY, cos_roll, sin_roll, cos_pitch, sin_pitch, scale; Matrix3f R;
const double magnitude = 665;
cos_roll = cos(roll); // Bearth is the magnetic field in Canberra. We need to adjust
sin_roll = sin(roll); // it for inclination and declination
cos_pitch = cos(pitch); Bearth(MAG_FIELD_STRENGTH, 0, 0);
sin_pitch = sin(pitch); R.from_euler(0, -ToRad(MAG_INCLINATION), ToRad(MAG_DECLINATION));
Bearth = R * Bearth;
// create a rotation matrix for the given attitude
R.from_euler(roll, pitch, yaw);
headY = -sin(heading); // convert the earth frame magnetic vector to body frame, and
headX = cos(heading); // apply the offsets
m = R.transposed() * Bearth - Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z);
if (fabs(cos_roll) < 1.0e-20) { return m;
cos_roll = 1.0e-10;
}
if (fabs(cos_pitch) < 1.0e-20) {
cos_pitch = 1.0e-10;
}
v.z = -0.6*cos(roll)*cos(pitch);
v.y = (headY + v.z*sin_roll) / cos_roll;
v.x = (headX - (v.y*sin_roll*sin_pitch + v.z*cos_roll*sin_pitch)) / cos_pitch;
scale = magnitude / sqrt((v.x*v.x) + (v.y*v.y) + (v.z*v.z));
v *= scale;
return v;
} }
@ -62,12 +62,11 @@ static Vector3f heading_to_mag(float heading, float roll, float pitch, float yaw
setup the compass with new input setup the compass with new input
all inputs are in degrees all inputs are in degrees
*/ */
void sitl_update_compass(float heading, float roll, float pitch, float yaw) void sitl_update_compass(float roll, float pitch, float yaw)
{ {
extern AP_Compass_HIL compass; extern AP_Compass_HIL compass;
Vector3f m = heading_to_mag(ToRad(heading), Vector3f m = heading_to_mag(ToRad(roll),
ToRad(roll),
ToRad(pitch), ToRad(pitch),
ToRad(yaw)); ToRad(yaw));
compass.setHIL(m.x - MAG_OFS_X, m.y - MAG_OFS_Y, m.z - MAG_OFS_Z); compass.setHIL(m.x, m.y, m.z);
} }