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 f9f7be832f ccfac19cef
commit 90cafe216c
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{
APM_RC.OutputCh(MOT_3, g.rc_2.radio_min);
APM_RC.OutputCh(MOT_2, g.rc_2.radio_min);
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);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
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);
APM_RC.OutputCh(MOT_2, g.rc_1.radio_min);
APM_RC.OutputCh(MOT_1, g.rc_1.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_4, g.rc_4.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_4, g.rc_4.radio_min);
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);
}

4
Tools/ArdupilotMegaPlanner/Common.cs
View File

@ -380,8 +380,8 @@ namespace ArdupilotMega
CH6_OPTFLOW_KD = 19,
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];
int rbufferread = 0;
int retrys = 3;
public int WriteBufferSize { get; set; }
public int WriteTimeout { get; set; }
public int ReceivedBytesThreshold { get; set; }
@ -121,6 +123,14 @@ namespace System.IO.Ports
client.Close();
}
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");
}
}

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

@ -30,8 +30,8 @@
{
this.components = new System.ComponentModel.Container();
System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(Configuration));
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle3 = new System.Windows.Forms.DataGridViewCellStyle();
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle4 = new System.Windows.Forms.DataGridViewCellStyle();
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle1 = new System.Windows.Forms.DataGridViewCellStyle();
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle2 = new System.Windows.Forms.DataGridViewCellStyle();
this.Params = new System.Windows.Forms.DataGridView();
this.Command = 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.label91 = new System.Windows.Forms.Label();
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.CMB_videoresolutions = new System.Windows.Forms.ComboBox();
this.label12 = new System.Windows.Forms.Label();
@ -287,8 +289,8 @@
this.BUT_load = new ArdupilotMega.MyButton();
this.toolTip1 = new System.Windows.Forms.ToolTip(this.components);
this.BUT_compare = new ArdupilotMega.MyButton();
this.label33 = new System.Windows.Forms.Label();
this.CMB_ratesensors = new System.Windows.Forms.ComboBox();
this.myLabel3 = new ArdupilotMega.MyLabel();
this.myLabel4 = new ArdupilotMega.MyLabel();
((System.ComponentModel.ISupportInitialize)(this.Params)).BeginInit();
this.ConfigTabs.SuspendLayout();
this.TabAP.SuspendLayout();
@ -409,14 +411,14 @@
this.Params.AllowUserToAddRows = false;
this.Params.AllowUserToDeleteRows = false;
resources.ApplyResources(this.Params, "Params");
dataGridViewCellStyle3.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft;
dataGridViewCellStyle3.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)));
dataGridViewCellStyle3.ForeColor = System.Drawing.Color.White;
dataGridViewCellStyle3.SelectionBackColor = System.Drawing.SystemColors.Highlight;
dataGridViewCellStyle3.SelectionForeColor = System.Drawing.SystemColors.HighlightText;
dataGridViewCellStyle3.WrapMode = System.Windows.Forms.DataGridViewTriState.True;
this.Params.ColumnHeadersDefaultCellStyle = dataGridViewCellStyle3;
dataGridViewCellStyle1.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft;
dataGridViewCellStyle1.BackColor = System.Drawing.Color.Maroon;
dataGridViewCellStyle1.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0)));
dataGridViewCellStyle1.ForeColor = System.Drawing.Color.White;
dataGridViewCellStyle1.SelectionBackColor = System.Drawing.SystemColors.Highlight;
dataGridViewCellStyle1.SelectionForeColor = System.Drawing.SystemColors.HighlightText;
dataGridViewCellStyle1.WrapMode = System.Windows.Forms.DataGridViewTriState.True;
this.Params.ColumnHeadersDefaultCellStyle = dataGridViewCellStyle1;
this.Params.ColumnHeadersHeightSizeMode = System.Windows.Forms.DataGridViewColumnHeadersHeightSizeMode.AutoSize;
this.Params.Columns.AddRange(new System.Windows.Forms.DataGridViewColumn[] {
this.Command,
@ -425,14 +427,14 @@
this.mavScale,
this.RawValue});
this.Params.Name = "Params";
dataGridViewCellStyle4.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft;
dataGridViewCellStyle4.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)));
dataGridViewCellStyle4.ForeColor = System.Drawing.SystemColors.WindowText;
dataGridViewCellStyle4.SelectionBackColor = System.Drawing.SystemColors.Highlight;
dataGridViewCellStyle4.SelectionForeColor = System.Drawing.SystemColors.HighlightText;
dataGridViewCellStyle4.WrapMode = System.Windows.Forms.DataGridViewTriState.True;
this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle4;
dataGridViewCellStyle2.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft;
dataGridViewCellStyle2.BackColor = System.Drawing.SystemColors.ActiveCaption;
dataGridViewCellStyle2.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0)));
dataGridViewCellStyle2.ForeColor = System.Drawing.SystemColors.WindowText;
dataGridViewCellStyle2.SelectionBackColor = System.Drawing.SystemColors.Highlight;
dataGridViewCellStyle2.SelectionForeColor = System.Drawing.SystemColors.HighlightText;
dataGridViewCellStyle2.WrapMode = System.Windows.Forms.DataGridViewTriState.True;
this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle2;
this.Params.RowHeadersVisible = false;
this.Params.CellValueChanged += new System.Windows.Forms.DataGridViewCellEventHandler(this.Params_CellValueChanged);
//
@ -1093,6 +1095,8 @@
//
// 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_HIGH);
this.TabAC.Controls.Add(this.myLabel2);
@ -1765,6 +1769,25 @@
resources.ApplyResources(this.TabPlanner, "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
//
resources.ApplyResources(this.label26, "label26");
@ -2136,24 +2159,17 @@
this.BUT_compare.UseVisualStyleBackColor = true;
this.BUT_compare.Click += new System.EventHandler(this.BUT_compare_Click);
//
// label33
// myLabel3
//
resources.ApplyResources(this.label33, "label33");
this.label33.Name = "label33";
resources.ApplyResources(this.myLabel3, "myLabel3");
this.myLabel3.Name = "myLabel3";
this.myLabel3.resize = false;
//
// CMB_ratesensors
// myLabel4
//
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);
resources.ApplyResources(this.myLabel4, "myLabel4");
this.myLabel4.Name = "myLabel4";
this.myLabel4.resize = false;
//
// Configuration
//
@ -2546,5 +2562,7 @@
private System.Windows.Forms.NumericUpDown TUNE_HIGH;
private System.Windows.Forms.Label label33;
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.Value = (decimal)(float)param[value];
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;
}
@ -373,6 +375,12 @@ namespace ArdupilotMega.GCSViews
thisctl.DecimalPlaces = 1;
}
if (thisctl.Name.EndsWith("_IMAX"))
{
thisctl.Maximum = 180;
thisctl.Minimum = -180;
}
thisctl.Enabled = true;
thisctl.BackColor = Color.FromArgb(0x43, 0x44, 0x45);
@ -403,7 +411,7 @@ namespace ArdupilotMega.GCSViews
}
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 GMapControl mymap = null;
bool playingLog = false;
public static PointLatLngAlt GuidedModeWP = new PointLatLngAlt();
@ -360,6 +362,11 @@ namespace ArdupilotMega.GCSViews
if (MainV2.comPort.logreadmode)
MainV2.comPort.logreadmode = false;
updatePlayPauseButton(false);
if (!playingLog && MainV2.comPort.logplaybackfile != null)
{
continue;
}
}
@ -1025,10 +1032,11 @@ namespace ArdupilotMega.GCSViews
{
MainV2.comPort.logreadmode = false;
ZedGraphTimer.Stop();
playingLog = false;
}
else
{
BUT_clear_track_Click(sender, e);
// BUT_clear_track_Click(sender, e);
MainV2.comPort.logreadmode = true;
list1.Clear();
list2.Clear();
@ -1045,6 +1053,7 @@ namespace ArdupilotMega.GCSViews
zg1.GraphPane.XAxis.Scale.Min = 0;
zg1.GraphPane.XAxis.Scale.Max = 1;
ZedGraphTimer.Start();
playingLog = true;
}
}

9
Tools/ArdupilotMegaPlanner/Joystick.cs
View File

@ -320,6 +320,15 @@ namespace ArdupilotMega
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);
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)
{
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);
comPort.Write("erase\r");
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;
CHK_logs.Items.Clear();
}

2
Tools/ArdupilotMegaPlanner/MagCalib.cs
View File

@ -227,7 +227,7 @@ namespace ArdupilotMega
/// <param name="ofs">offsets</param>
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
{

10
Tools/ArdupilotMegaPlanner/MainV2.cs
View File

@ -547,7 +547,7 @@ namespace ArdupilotMega
try
{
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
@ -863,11 +863,13 @@ namespace ArdupilotMega
}
}
DateTime connectButtonUpdate = DateTime.Now;
private void updateConnectIcon()
{
DateTime menuupdate = DateTime.Now;
if ((DateTime.Now - menuupdate).Milliseconds > 500)
if ((DateTime.Now - connectButtonUpdate).Milliseconds > 500)
{
// Console.WriteLine(DateTime.Now.Millisecond);
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:
// [assembly: AssemblyVersion("1.0.*")]
[assembly: AssemblyVersion("1.0.0.0")]
[assembly: AssemblyFileVersion("1.1.54")]
[assembly: AssemblyFileVersion("1.1.55")]
[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()));
}
if (TXT_ampspervolt.Text == "13.6612")
// ignore language re . vs ,
if (TXT_ampspervolt.Text == (13.6612).ToString())
{
CMB_batmonsensortype.SelectedIndex = 1;
}
else if (TXT_ampspervolt.Text == "27.3224")
else if (TXT_ampspervolt.Text == (27.3224).ToString())
{
CMB_batmonsensortype.SelectedIndex = 2;
}
else if (TXT_ampspervolt.Text == "54.64481")
else if (TXT_ampspervolt.Text == (54.64481).ToString())
{
CMB_batmonsensortype.SelectedIndex = 3;
}
@ -1682,6 +1684,12 @@ namespace ArdupilotMega.Setup
MainV2.cs.ratesensors = backupratesens;
if (data.Count < 10)
{
CustomMessageBox.Show("Log does not contain enough data");
return;
}
double[] ans = MagCalib.LeastSq(data);
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_follow_me = new ArdupilotMega.MyButton();
this.BUT_ant_track = new ArdupilotMega.MyButton();
this.BUT_magcalib = new ArdupilotMega.MyButton();
this.SuspendLayout();
//
// button1
@ -234,11 +235,21 @@
this.BUT_ant_track.UseVisualStyleBackColor = true;
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
//
this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F);
this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font;
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_follow_me);
this.Controls.Add(this.BUT_georefimage);
@ -287,6 +298,7 @@
private MyButton BUT_georefimage;
private MyButton BUT_follow_me;
private MyButton BUT_ant_track;
private MyButton BUT_magcalib;
//private SharpVectors.Renderers.Forms.SvgPictureBox svgPictureBox1;
}

5
Tools/ArdupilotMegaPlanner/temp.cs
View File

@ -886,5 +886,10 @@ namespace ArdupilotMega
{
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
MAG_ENABLE 1
LOG_BITMASK 4095
COMPASS_DEC 0.211
RC1_MAX 2000.000000
RC1_MIN 1000.000000
RC1_TRIM 1500.000000

1
Tools/autotest/ArduPlane.parm
View File

@ -9,6 +9,7 @@ ARSPD_FBW_MAX 30
ARSPD_FBW_MIN 10
KFF_RDDRMIX 0.5
KFF_PTCHCOMP 0.3
COMPASS_DEC 0.211
THR_MAX 100
HDNG2RLL_D 0.5
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):
'''a basic aircraft class'''
@ -14,67 +14,35 @@ class Aircraft(object):
self.longitude = self.home_longitude
self.altitude = self.home_altitude
self.pitch = 0.0 # degrees
self.roll = 0.0 # degrees
self.yaw = 0.0 # degrees
self.dcm = Matrix3()
# rates in earth frame
self.pitch_rate = 0.0 # degrees/s
self.roll_rate = 0.0 # degrees/s
self.yaw_rate = 0.0 # degrees/s
# rotation rate in body frame
self.gyro = Vector3(0,0,0) # rad/s
# rates in body frame
self.pDeg = 0.0 # degrees/s
self.qDeg = 0.0 # degrees/s
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.velocity = Vector3(0, 0, 0) # m/s, North, East, Down
self.position = Vector3(0, 0, 0) # m North, East, Down
self.last_velocity = self.velocity.copy()
self.mass = 0.0
self.update_frequency = 50 # in Hz
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')
def normalise(self):
'''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):
def update_position(self, delta_time):
'''update lat/lon/alt from position'''
radius_of_earth = 6378100.0 # in meters
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
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
from math import sin, cos, sqrt, radians
self.altitude = self.home_altitude - self.position.z
# work out what the accelerometer would see
xAccel = sin(radians(self.pitch))
yAccel = -sin(radians(self.roll)) * cos(radians(self.pitch))
zAccel = -cos(radians(self.roll)) * cos(radians(self.pitch))
scale = 9.81 / sqrt((xAccel*xAccel)+(yAccel*yAccel)+(zAccel*zAccel))
xAccel *= scale;
yAccel *= scale;
zAccel *= scale;
self.accelerometer = euclid.Vector3(xAccel, yAccel, zAccel)
self.accelerometer = ((self.velocity - self.last_velocity)/delta_time) + Vector3(0,0,self.gravity)
# self.accelerometer = Vector3(0,0,-self.gravity)
self.accelerometer = self.dcm.transposed() * self.accelerometer
self.last_velocity = self.velocity.copy()

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

@ -1,7 +1,9 @@
#!/usr/bin/env python
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):
def __init__(self, angle, clockwise, servo):
@ -83,7 +85,7 @@ class MultiCopter(Aircraft):
self.mass = mass # Kg
self.hover_throttle = hover_throttle
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
# 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)):
servo = servos[self.motors[i].servo-1]
if servo <= 0.0:
self.motor_speed[i] = 0
else:
self.motor_speed[i] = servo
@ -107,78 +110,58 @@ class MultiCopter(Aircraft):
delta_time = t - self.last_time
self.last_time = t
# rotational acceleration, in degrees/s/s, in body frame
roll_accel = 0.0
pitch_accel = 0.0
yaw_accel = 0.0
# rotational acceleration, in rad/s/s, in body frame
rot_accel = Vector3(0,0,0)
thrust = 0.0
for i in range(len(self.motors)):
roll_accel += -5000.0 * math.sin(math.radians(self.motors[i].angle)) * m[i]
pitch_accel += 5000.0 * math.cos(math.radians(self.motors[i].angle)) * m[i]
rot_accel.x += -radians(5000.0) * math.sin(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:
yaw_accel -= m[i] * 400.0
rot_accel.z -= m[i] * radians(400.0)
else:
yaw_accel += m[i] * 400.0
rot_accel.z += m[i] * radians(400.0)
thrust += m[i] * self.thrust_scale # newtons
# rotational resistance
roll_accel -= (self.pDeg / self.terminal_rotation_rate) * 5000.0
pitch_accel -= (self.qDeg / self.terminal_rotation_rate) * 5000.0
yaw_accel -= (self.rDeg / self.terminal_rotation_rate) * 400.0
# rotational air resistance
rot_accel.x -= self.gyro.x * radians(5000.0) / self.terminal_rotation_rate
rot_accel.y -= self.gyro.y * radians(5000.0) / self.terminal_rotation_rate
rot_accel.z -= self.gyro.z * radians(400.0) / self.terminal_rotation_rate
# update rotational rates in body frame
self.pDeg += roll_accel * delta_time
self.qDeg += pitch_accel * delta_time
self.rDeg += yaw_accel * delta_time
self.gyro += rot_accel * delta_time
# calculate rates in earth frame
(self.roll_rate,
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
# update attitude
self.dcm.rotate(self.gyro * delta_time)
# air resistance
air_resistance = - self.velocity * (self.gravity/self.terminal_velocity)
# normalise rotations
self.normalise()
accel = thrust / self.mass
accel3D = util.RPY_to_XYZ(self.roll, self.pitch, self.yaw, accel)
accel3D += euclid.Vector3(0, 0, -self.gravity)
accel3D += air_resistance
accel_body = Vector3(0, 0, -thrust / self.mass)
accel_earth = self.dcm * accel_body
accel_earth += Vector3(0, 0, self.gravity)
accel_earth += air_resistance
# add in some wind
accel3D += self.wind.accel(self.velocity)
accel_earth += self.wind.accel(self.velocity)
# new velocity vector
self.velocity += accel3D * delta_time
self.accel = accel3D
self.velocity += accel_earth * delta_time
# new position vector
old_position = self.position.copy()
self.position += self.velocity * delta_time
# constrain height to the ground
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:
print("Hit ground at %f m/s" % (-self.velocity.z))
self.velocity = euclid.Vector3(0, 0, 0)
self.roll_rate = 0
self.pitch_rate = 0
self.yaw_rate = 0
self.roll = 0
self.pitch = 0
self.accel = euclid.Vector3(0, 0, 0)
self.position = euclid.Vector3(self.position.x, self.position.y,
self.ground_level + self.frame_height - self.home_altitude)
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:
print("Hit ground at %f m/s" % (self.velocity.z))
self.velocity = Vector3(0, 0, 0)
# zero roll/pitch, but keep yaw
(r, p, y) = self.dcm.to_euler()
self.dcm.from_euler(0, 0, y)
self.position = Vector3(self.position.x, self.position.y,
-(self.ground_level + self.frame_height - self.home_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
from multicopter import MultiCopter
import euclid, util, time, os, sys, math
import util, time, os, sys, math
import socket, struct
import select, fgFDM, errno
@ -16,16 +16,20 @@ import mavlink
def sim_send(m, a):
'''send flight information to mavproxy and flightgear'''
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('longitude', a.longitude, units='degrees')
fdm.set('altitude', a.altitude, units='meters')
fdm.set('phi', a.roll, units='degrees')
fdm.set('theta', a.pitch, units='degrees')
fdm.set('psi', a.yaw, units='degrees')
fdm.set('phidot', a.roll_rate, units='dps')
fdm.set('thetadot', a.pitch_rate, units='dps')
fdm.set('psidot', a.yaw_rate, units='dps')
fdm.set('phi', roll, units='radians')
fdm.set('theta', pitch, units='radians')
fdm.set('psi', yaw, units='radians')
fdm.set('phidot', earth_rates.x, units='rps')
fdm.set('thetadot', earth_rates.y, units='rps')
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('v_north', a.velocity.x, units='mps')
fdm.set('v_east', a.velocity.y, units='mps')
@ -40,11 +44,11 @@ def sim_send(m, a):
raise
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.accelerometer.x, a.accelerometer.y, a.accelerometer.z,
a.roll_rate, a.pitch_rate, a.yaw_rate,
a.roll, a.pitch, a.yaw,
-a.accelerometer.x, -a.accelerometer.y, -a.accelerometer.z,
degrees(earth_rates.x), degrees(earth_rates.y), degrees(earth_rates.z),
degrees(roll), degrees(pitch), degrees(yaw),
math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y),
0x4c56414e)
try:

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

@ -1,12 +1,12 @@
#!/usr/bin/env python
# simple test of wind generation code
import util, euclid, time, random
import util, time, random
wind = util.Wind('3,90,0.1')
t0 = time.time()
velocity = euclid.Vector3(0,0,0)
velocity = Vector3(0,0,0)
t = 0
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
from rotmat import Vector3, Matrix3
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):
'''meters to feet'''
return float(x) / 0.3048
@ -289,53 +207,48 @@ def check_parent(parent_pid=os.getppid()):
sys.exit(1)
def EarthRatesToBodyRates(roll, pitch, yaw,
rollRate, pitchRate, yawRate):
def EarthRatesToBodyRates(dcm, earth_rates):
'''convert the angular velocities from earth frame to
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)
theta = radians(pitch)
phiDot = radians(rollRate)
thetaDot = radians(pitchRate)
psiDot = radians(yawRate)
(phi, theta, psi) = dcm.to_euler()
phiDot = earth_rates.x
thetaDot = earth_rates.y
psiDot = earth_rates.z
p = phiDot - psiDot*sin(theta)
q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta)
r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot
return Vector3(p, q, r)
return (degrees(p), degrees(q), degrees(r))
def BodyRatesToEarthRates(roll, pitch, yaw, pDeg, qDeg, rDeg):
def BodyRatesToEarthRates(dcm, gyro):
'''convert the angular velocities from body frame to
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)
q = radians(qDeg)
r = radians(rDeg)
p = gyro.x
q = gyro.y
r = gyro.z
phi = radians(roll)
theta = radians(pitch)
(phi, theta, psi) = dcm.to_euler()
phiDot = p + tan(theta)*(q*sin(phi) + r*cos(phi))
thetaDot = q*cos(phi) - r*sin(phi)
if fabs(cos(theta)) < 1.0e-20:
theta += 1.0e-10
psiDot = (q*sin(phi) + r*cos(phi))/cos(theta)
return (degrees(phiDot), degrees(thetaDot), degrees(psiDot))
return Vector3(phiDot, thetaDot, psiDot)
class Wind(object):
@ -382,12 +295,15 @@ class Wind(object):
'''return current wind acceleration in ground frame. The
velocity is a Vector3 of the current velocity of the aircraft
in earth frame, m/s'''
from math import radians
(speed, direction) = self.current(deltat=deltat)
# wind vector
v = euclid.Vector3(speed, 0, 0)
wind = euclid.Quaternion.new_rotate_euler(0, math.radians(direction), 0) * v
v = Vector3(speed, 0, 0)
m = Matrix3()
m.from_euler(0, 0, radians(direction))
wind = m.transposed() * v
# relative wind vector
relwind = wind - velocity

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

@ -21,6 +21,7 @@
#include <DataFlash.h>
#include <APM_RC.h>
#include <GCS_MAVLink.h>
#include <Filter.h>
// uncomment this for a APM2 board
#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_Compass.h>
#include <AP_GPS.h>
#include <Filter.h>
Arduino_Mega_ISR_Registry isr_registry;
AP_Baro_BMP085_HIL barometer;
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;
}
// 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
template void Matrix3<float>::rotation(enum Rotation);
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>::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; }
// multiplication by a vector
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);
}
Vector3<T> operator *(const Vector3<T> &v) const;
// multiplication by another 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;
}
Matrix3<T> operator *(const Matrix3<T> &m) const;
Matrix3<T> &operator *=(const Matrix3<T> &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);
int sitl_gps_pipe(void);
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,
double speedN, double speedE, bool have_lock);
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.airspeed);
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();
}
@ -326,7 +326,7 @@ void sitl_setup(void)
// setup some initial values
sitl_update_barometer(desktop_state.initial_height);
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);
}

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

@ -20,40 +20,40 @@
#define MAG_OFS_Y 13.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,
calculate consistent magnetometer components
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;
double headX, headY, cos_roll, sin_roll, cos_pitch, sin_pitch, scale;
const double magnitude = 665;
Vector3f Bearth, m;
Matrix3f R;
cos_roll = cos(roll);
sin_roll = sin(roll);
cos_pitch = cos(pitch);
sin_pitch = sin(pitch);
// Bearth is the magnetic field in Canberra. We need to adjust
// it for inclination and declination
Bearth(MAG_FIELD_STRENGTH, 0, 0);
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);
headX = cos(heading);
if (fabs(cos_roll) < 1.0e-20) {
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;
// convert the earth frame magnetic vector to body frame, and
// apply the offsets
m = R.transposed() * Bearth - Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z);
return m;
}
@ -62,12 +62,11 @@ static Vector3f heading_to_mag(float heading, float roll, float pitch, float yaw
setup the compass with new input
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;
Vector3f m = heading_to_mag(ToRad(heading),
ToRad(roll),
Vector3f m = heading_to_mag(ToRad(roll),
ToRad(pitch),
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);
}