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

Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj

@@ -247,6 +247,7 @@
     <Compile Include="Controls\ImageLabel.Designer.cs">
       <DependentUpon>ImageLabel.cs</DependentUpon>
     </Compile>
+    <Compile Include="HIL\Utils.cs" />
     <Compile Include="ResEdit.cs">
       <SubType>Form</SubType>
     </Compile>
@@ -402,6 +403,7 @@
       <DependentUpon>Splash.cs</DependentUpon>
     </Compile>
     <Compile Include="srtm.cs" />
+    <Compile Include="TCPConsole.cs" />
     <Compile Include="temp.cs">
       <SubType>Form</SubType>
     </Compile>

Tools/ArdupilotMegaPlanner/ArdupilotMega.sln

@@ -5,6 +5,8 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ArdupilotMega", "ArdupilotM
 EndProject
 Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Updater", "Updater\Updater.csproj", "{E64A1A41-A5B0-458E-8284-BB63705354DA}"
 EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ArduinoCPP", "..\..\ArduinoCPP\ArduinoCPP.csproj", "{C38A02C5-3179-4047-8DC3-945341008A74}"
+EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution
 		Debug|Any CPU = Debug|Any CPU
@@ -41,6 +43,18 @@ Global
 		{E64A1A41-A5B0-458E-8284-BB63705354DA}.Release|Win32.ActiveCfg = Release|x86
 		{E64A1A41-A5B0-458E-8284-BB63705354DA}.Release|x86.ActiveCfg = Release|x86
 		{E64A1A41-A5B0-458E-8284-BB63705354DA}.Release|x86.Build.0 = Release|x86
+		{C38A02C5-3179-4047-8DC3-945341008A74}.Debug|Any CPU.ActiveCfg = Debug|x86
+		{C38A02C5-3179-4047-8DC3-945341008A74}.Debug|Mixed Platforms.ActiveCfg = Debug|x86
+		{C38A02C5-3179-4047-8DC3-945341008A74}.Debug|Mixed Platforms.Build.0 = Debug|x86
+		{C38A02C5-3179-4047-8DC3-945341008A74}.Debug|Win32.ActiveCfg = Debug|x86
+		{C38A02C5-3179-4047-8DC3-945341008A74}.Debug|x86.ActiveCfg = Debug|x86
+		{C38A02C5-3179-4047-8DC3-945341008A74}.Debug|x86.Build.0 = Debug|x86
+		{C38A02C5-3179-4047-8DC3-945341008A74}.Release|Any CPU.ActiveCfg = Release|x86
+		{C38A02C5-3179-4047-8DC3-945341008A74}.Release|Mixed Platforms.ActiveCfg = Release|x86
+		{C38A02C5-3179-4047-8DC3-945341008A74}.Release|Mixed Platforms.Build.0 = Release|x86
+		{C38A02C5-3179-4047-8DC3-945341008A74}.Release|Win32.ActiveCfg = Release|x86
+		{C38A02C5-3179-4047-8DC3-945341008A74}.Release|x86.ActiveCfg = Release|x86
+		{C38A02C5-3179-4047-8DC3-945341008A74}.Release|x86.Build.0 = Release|x86
 	EndGlobalSection
 	GlobalSection(SolutionProperties) = preSolution
 		HideSolutionNode = FALSE

Tools/ArdupilotMegaPlanner/Common.cs

@@ -299,7 +299,7 @@ namespace ArdupilotMega
             Console.WriteLine("a = {0}, the slope of the trend line.", Math.Round(a, 2));
             Console.WriteLine("b = {0}, the intercept of the trend line.", Math.Round(b, 2));
 
-            Console.ReadLine();
+            //Console.ReadLine();
         }
        
 		#if MAVLINK10

Tools/ArdupilotMegaPlanner/CurrentState.cs

@@ -360,6 +360,9 @@ namespace ArdupilotMega
                                     break;
                                 case (byte)(Common.ac2modes.CIRCLE):
                                     mode = "Circle";
+                                    break;
+                                        case (byte)(Common.ac2modes.LAND):
+                            mode = "Land";
                             break;
                                 default:
                                     mode = "Unknown";

Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs

@@ -550,13 +550,13 @@ namespace ArdupilotMega.GCSViews
             {
                 if (text.Length > 0)
                 {
-                        if (sender.GetType() == typeof(NumericUpDown))
+                    if (text[0].GetType() == typeof(NumericUpDown))
                         {
                             decimal option = (decimal)(float.Parse(Params[e.ColumnIndex, e.RowIndex].Value.ToString()));
                             ((NumericUpDown)text[0]).Value = option;
                             ((NumericUpDown)text[0]).BackColor = Color.Green;
                         }
-                        else if (sender.GetType() == typeof(ComboBox))
+                    else if (text[0].GetType() == typeof(ComboBox))
                         {
                             int option = (int)(float.Parse(Params[e.ColumnIndex, e.RowIndex].Value.ToString()));
                             ((ComboBox)text[0]).SelectedIndex = option;
@@ -685,7 +685,7 @@ namespace ArdupilotMega.GCSViews
                         Control[] text = this.Controls.Find(value, true);
                         if (text.Length > 0)
                         {
-                            ((NumericUpDown)text[0]).BackColor = Color.FromArgb(0x43, 0x44, 0x45);
+                            ((Control)text[0]).BackColor = Color.FromArgb(0x43, 0x44, 0x45);
                         }
                     }
                     catch { }

Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs

@@ -3,7 +3,6 @@ using System.Collections.Generic;
 using System.ComponentModel;
 using System.Data;
 using System.Drawing;
-using System.Linq;
 using System.Text;
 using System.Windows.Forms;
 using System.Text.RegularExpressions;
@@ -591,7 +590,7 @@ namespace ArdupilotMega.GCSViews
             UploadFlash(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"firmware.hex", board);
         }
 
-        void UploadFlash(string filename, string board)
+        public void UploadFlash(string filename, string board)
         {
             byte[] FLASH = new byte[1];
             StreamReader sr = null;

Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs

@@ -8,8 +8,8 @@
         {
             this.components = new System.ComponentModel.Container();
             System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(FlightData));
-            System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle7 = new System.Windows.Forms.DataGridViewCellStyle();
+            System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle1 = new System.Windows.Forms.DataGridViewCellStyle();
-            System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle8 = new System.Windows.Forms.DataGridViewCellStyle();
+            System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle2 = new System.Windows.Forms.DataGridViewCellStyle();
             this.contextMenuStrip1 = new System.Windows.Forms.ContextMenuStrip(this.components);
             this.goHereToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem();
             this.pointCameraHereToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem();
@@ -1217,8 +1217,8 @@
             // 
             // dataGridViewImageColumn1
             // 
-            dataGridViewCellStyle7.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleCenter;
+            dataGridViewCellStyle1.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleCenter;
-            this.dataGridViewImageColumn1.DefaultCellStyle = dataGridViewCellStyle7;
+            this.dataGridViewImageColumn1.DefaultCellStyle = dataGridViewCellStyle1;
             resources.ApplyResources(this.dataGridViewImageColumn1, "dataGridViewImageColumn1");
             this.dataGridViewImageColumn1.Image = global::ArdupilotMega.Properties.Resources.up;
             this.dataGridViewImageColumn1.ImageLayout = System.Windows.Forms.DataGridViewImageCellLayout.Stretch;
@@ -1226,8 +1226,8 @@
             // 
             // dataGridViewImageColumn2
             // 
-            dataGridViewCellStyle8.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleCenter;
+            dataGridViewCellStyle2.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleCenter;
-            this.dataGridViewImageColumn2.DefaultCellStyle = dataGridViewCellStyle8;
+            this.dataGridViewImageColumn2.DefaultCellStyle = dataGridViewCellStyle2;
             resources.ApplyResources(this.dataGridViewImageColumn2, "dataGridViewImageColumn2");
             this.dataGridViewImageColumn2.Image = global::ArdupilotMega.Properties.Resources.down;
             this.dataGridViewImageColumn2.ImageLayout = System.Windows.Forms.DataGridViewImageCellLayout.Stretch;

Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs

@@ -270,9 +270,11 @@ namespace ArdupilotMega.GCSViews
                             System.Threading.Thread.Sleep(2000);
 
                             SITLSEND = new UdpClient(simIP, 5501);
-                        }
 
                             SetupTcpJSBSim(); // old style
+                        }
+
+
                         SetupUDPXplanes(); // fg udp style
                         SetupUDPMavLink(); // pass traffic - raw
                     }
@@ -591,7 +593,7 @@ namespace ArdupilotMega.GCSViews
 
                 if (hzcounttime.Second != DateTime.Now.Second)
                 {
-                                        Console.WriteLine("SIM hz {0}", hzcount);
+//                                        Console.WriteLine("SIM hz {0}", hzcount);
                     hzcount = 0;
                     hzcounttime = DateTime.Now;
                 }
@@ -660,84 +662,6 @@ namespace ArdupilotMega.GCSViews
             MavLink = new UdpClient("127.0.0.1", 14550);
         }
 
-        /// <summary>
-        /// From http://code.google.com/p/gentlenav/source/browse/trunk/Tools/XP_UDB_HILSIM/utility.cpp
-        /// Converts from xplanes to fixed body ref
-        /// </summary>
-        /// <param name="x"></param>
-        /// <param name="y"></param>
-        /// <param name="z"></param>
-        /// <param name="alpha"></param>
-        /// <param name="beta"></param>
-        public static void FLIGHTtoBCBF(ref float x, ref float y, ref float z, float alpha, float beta)
-        {
-            float Ca = (float)Math.Cos(alpha);
-             float Cb = (float)Math.Cos(beta);
-             float Sa = (float)Math.Sin(alpha);
-             float Sb = (float)Math.Sin(beta);
-
-             float X_plane = (x * Ca * Cb) - (z * Sa * Cb) - (y * Sb);
-             float Y_plane = (z * Sa * Sb) - (x * Ca * Sb) - (y * Cb);
-             float Z_plane = (x * Sa) + (z * Ca);
-
-             x = X_plane;
-            y = Y_plane;
-            z = Z_plane;
-        }
-
-        void OGLtoBCBF(ref float x, ref float y, ref float z, float phi, float theta, float psi)
-        {
-            float x_NED, y_NED, z_NED;
-            float Cr, Cp, Cy;
-            float Sr, Sp, Sy;
-
-            //Accelerations in X-Plane are expressed in the local OpenGL reference frame, for whatever reason. 
-            //This coordinate system is defined as follows (taken from the X-Plane SDK Wiki):
-
-            //	The origin 0,0,0 is on the surface of the earth at sea level at some "reference point".
-            //	The +X axis points east from the reference point.
-            //	The +Z axis points south from the reference point.
-            //	The +Y axis points straight up away from the center of the earth at the reference point.
-
-            // First we shall convert from this East Up South frame, to a more conventional NED (North East Down) frame.
-            x_NED = -1.0f * z;
-            y_NED = x;
-            z_NED = -1.0f * y;
-
-            // Next calculate cos & sin of angles for use in the transformation matrix.
-            // r, p & y subscripts stand for roll pitch and yaw.
-
-            Cr = (float)Math.Cos(phi);
-            Cp = (float)Math.Cos(theta);
-            Cy = (float)Math.Cos(psi);
-            Sr = (float)Math.Sin(phi);
-            Sp = (float)Math.Sin(theta);
-            Sy = (float)Math.Sin(psi);
-
-            // Next we need to rotate our accelerations from the NED reference frame, into the body fixed reference frame
-
-            // THANKS TO GEORGE M SIOURIS WHOSE "MISSILE GUIDANCE AND CONTROL SYSTEMS" BOOK SEEMS TO BE THE ONLY EASY TO FIND REFERENCE THAT
-            // ACTUALLY GETS THE NED TO BODY FRAME ROTATION MATRIX CORRECT!!
-
-            // CpCy, CpSy, -Sp					| local_ax
-            // SrSpCy-CrSy, SrSpSy+CrCy, SrCp	| local_ay
-            // CrSpCy+SrSy, CrSpSy-SrCy, CrCp	| local_az
-
-            x = (x_NED * Cp * Cy) + (y_NED * Cp * Sy) - (z_NED * Sp);
-            y = (x_NED * ((Sr * Sp * Cy) - (Cr * Sy))) + (y_NED * ((Sr * Sp * Sy) + (Cr * Cy))) + (z_NED * Sr * Cp);
-            z = (x_NED * ((Cr * Sp * Cy) + (Sr * Sy))) + (y_NED * ((Cr * Sp * Sy) - (Sr * Cy))) + (z_NED * Cr * Cp);
-        }
-
-        double sin(double rad)
-        {
-            return Math.Sin(rad);
-        }
-
-        double cos(double rad)
-        {
-            return Math.Cos(rad);
-        }
-
         float oldax =0, olday =0, oldaz = 0;
         DateTime oldtime = DateTime.Now;
 #if MAVLINK10

Tools/ArdupilotMegaPlanner/HIL/Aircraft.cs

@@ -2,11 +2,13 @@
 using System.Collections.Generic;
 using System.Text;
 using YLScsDrawing.Drawing3d;
+using ArdupilotMega.HIL;
 
 namespace ArdupilotMega.HIL
 {
-    public class Aircraft
+    public class Aircraft : Utils
     {
+        Aircraft self;
         public double home_latitude = 0;
         public double home_longitude = 0;
         public double home_altitude = 0;
@@ -20,15 +22,27 @@ namespace ArdupilotMega.HIL
         public double pitch = 0.0;     //# degrees
         public double roll = 0.0;    //# degrees
         public double yaw = 0.0;   //# degrees
+
         public double pitch_rate = 0.0;  //# degrees/s
         public double roll_rate = 0.0;  //# degrees/s
         public double yaw_rate = 0.0;  //# degrees/s
+
+        public double pDeg = 0.0;   // degrees/s
+        public double qDeg = 0.0;   // degrees/s
+        public double rDeg = 0.0;   // degrees/s
+
         public Vector3d velocity = new Vector3d(0, 0, 0); //# m/s, North, East, Up
         public Vector3d position = new Vector3d(0, 0, 0); //# m North, East, Up
         public Vector3d accel = new Vector3d(0, 0, 0); //# m/s/s North, East, Up
         public double mass = 0.0;
         public double update_frequency = 50;//# in Hz
         public double gravity = 9.8;//# m/s/s
+        public Vector3d accelerometer = new Vector3d(0, 0, -9.8);
+
+        public Aircraft()
+        {
+            self = this;
+        }
 
         public void normalise()
         {
@@ -46,7 +60,8 @@ namespace ArdupilotMega.HIL
             return angle;
         }
 
-            public void update_position() {
+        public void update_position()
+        {
             //'''update lat/lon/alt from position'''
 
             double radius_of_earth = 6378100.0;// # in meters
@@ -56,9 +71,20 @@ namespace ArdupilotMega.HIL
             altitude = home_altitude + position.Z;
             latitude = home_latitude + dlat;
             longitude = home_longitude + dlon;
+
+            // work out what the accelerometer would see
+            double xAccel = sin(radians(self.pitch)) * cos(radians(self.roll));
+            double yAccel = -sin(radians(self.roll)) * cos(radians(self.pitch));
+            double zAccel = -cos(radians(self.roll)) * cos(radians(self.pitch));
+            double scale = 9.81 / sqrt((xAccel * xAccel) + (yAccel * yAccel) + (zAccel * zAccel));
+            xAccel *= scale;
+            yAccel *= scale;
+            zAccel *= scale;
+            self.accelerometer = new Vector3d(xAccel, yAccel, zAccel);
+
         }
 
-            const float rad2deg = (float)(180 / Math.PI);
+
-            const float deg2rad = (float)(1.0 / rad2deg);
+
     }
 }

Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs

@@ -3,15 +3,105 @@ using System.Collections.Generic;
 using System.Linq;
 using System.Text;
 using YLScsDrawing.Drawing3d;
+using ArdupilotMega.HIL;
 
 namespace ArdupilotMega.HIL
 {
+    public class Motor : Utils
+    {
+        const bool True = true;
+        const bool False = false;
+
+        public Motor self;
+        public double angle;
+        public bool clockwise;
+        public double servo;
+
+        public Motor(double angle, bool clockwise, double servo)
+        {
+            self = this;
+            self.angle = angle;
+            self.clockwise = clockwise;
+            self.servo = servo;
+        }
+
+        public static Motor[] build_motors(string frame)
+        {
+            Motor[] motors = new HIL.Motor[8];
+            frame = frame.ToLower();
+            if (frame.Contains("quad") || frame.Contains("quadx"))
+            {
+                motors = new HIL.Motor[] {
+            new Motor(90,  False,  1),
+            new Motor(270, False,  2),
+            new Motor(0,   True,   3),
+            new Motor(180, True,   4),
+                };
+                if (frame.Contains("quadx"))
+                {
+                    foreach (int i in range(4))
+                        motors[i].angle -= 45.0;
+                }
+            }
+
+            else if (frame.Contains("y6"))
+            {
+                motors = new HIL.Motor[] {
+            new Motor(60,   False, 1),
+            new Motor(60,   True,  7),
+            new Motor(180,  True,  4),
+            new Motor(180,  False, 8),
+            new Motor(-60,  True,  2),
+            new Motor(-60,  False, 3),
+            };
+            }
+            else if (frame.Contains("hexa") || frame.Contains("hexax"))
+            {
+                motors = new HIL.Motor[] {
+            new Motor(0,   True,  1),
+            new Motor(60,  False, 4),
+            new Motor(120, True,  8),
+            new Motor(180, False, 2),
+            new Motor(240, True,  3),
+            new Motor(300, False, 7),
+           };
+            }
+            else if (frame.Contains("hexax"))
+            {
+                motors = new HIL.Motor[] {
+            new Motor(30,  False,  7),
+            new Motor(90,  True,   1),
+            new Motor(150, False,  4),
+            new Motor(210, True,   8),
+            new Motor(270, False,  2),
+            new Motor(330, True,   3),
+           };
+            }
+            else if (frame.Contains("octa") || frame.Contains("octax"))
+            {
+                motors = new HIL.Motor[] {
+            new Motor(0,    True,  1),
+            new Motor(180,  True,  2),
+            new Motor(45,   False, 3),
+            new Motor(135,  False, 4),
+            new Motor(-45,  False, 7),
+            new Motor(-135, False, 8),
+            new Motor(270,  True, 10),
+            new Motor(90,   True, 11),
+        };
+                if (frame.Contains("octax"))
+                {
+                    foreach (int i in range(8))
+                        motors[i].angle += 22.5;
+                }
+            }
+            return motors;
+        }
+    }
+
     public class QuadCopter : Aircraft
     {
-        const float ft2m = (float)(1.0 / 3.2808399);
+        QuadCopter self;
-        const float rad2deg = (float)(180 / Math.PI);
-        const float deg2rad = (float)(1.0 / rad2deg);
-        const float kts2fps = (float)1.68780986;
 
         int framecount = 0;
         DateTime seconds = DateTime.Now;
@@ -21,6 +111,7 @@ namespace ArdupilotMega.HIL
         double hover_throttle;
         double terminal_velocity;
         double terminal_rotation_rate;
+        Motor[] motors;
 
         Vector3d old_position;
 
@@ -31,16 +122,20 @@ namespace ArdupilotMega.HIL
 
         DateTime last_time;
 
-        public QuadCopter()
+        public QuadCopter(string frame = "quad")
         {
+            self = this;
+
+            motors = Motor.build_motors(frame);
+
             mass = 1.0;// # Kg
             frame_height = 0.1;
-            motor_speed = new double[] { 0.0, 0.0, 0.0, 0.0 };
+            motor_speed = new double[motors.Length];
             hover_throttle = 0.37;
             terminal_velocity = 30.0;
             terminal_rotation_rate = 4 * 360.0;
 
-            thrust_scale = (mass * gravity) / (4.0 * hover_throttle);
+            thrust_scale = (mass * gravity) / (motors.Length * hover_throttle);
 
             last_time = DateTime.Now;
         }
@@ -109,25 +204,53 @@ namespace ArdupilotMega.HIL
                 delta_time = new TimeSpan(0, 0, 0, 0, 20);
             }
 
-            //# rotational acceleration, in degrees/s/s
+             // rotational acceleration, in degrees/s/s, in body frame
-            double roll_accel = (m[1] - m[0]) * 5000.0;
+            double roll_accel = 0.0;
-            double pitch_accel = (m[2] - m[3]) * 5000.0;
+            double pitch_accel = 0.0;
-            double yaw_accel = -((m[2] + m[3]) - (m[0] + m[1])) * 400.0;
+            double yaw_accel = 0.0;
+            double thrust = 0.0;
 
-            //Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll);
+            foreach (var i in range((self.motors.Length)))
+            {
+                roll_accel += -5000.0 * sin(radians(self.motors[i].angle)) * m[i];
+                pitch_accel += 5000.0 * cos(radians(self.motors[i].angle)) * m[i];
+                if (self.motors[i].clockwise)
+                {
+                    yaw_accel -= m[i] * 400.0;
+                }
+                else
+                {
+                    yaw_accel += m[i] * 400.0;
+                }
+                thrust += m[i] * self.thrust_scale; // newtons
+            }
 
         // rotational resistance
-            roll_accel -= (roll_rate / terminal_rotation_rate) * 5000.0;
+        roll_accel  -= (self.pDeg / self.terminal_rotation_rate) * 5000.0;
-            pitch_accel -= (pitch_rate / terminal_rotation_rate) * 5000.0;
+        pitch_accel -= (self.qDeg / self.terminal_rotation_rate) * 5000.0;
-            yaw_accel -= (yaw_rate / terminal_rotation_rate) * 400.0;
+        yaw_accel -= (self.rDeg / self.terminal_rotation_rate) * 400.0;
-
-            //# update rotational rates
-            roll_rate += roll_accel * delta_time.TotalSeconds;
-            pitch_rate += pitch_accel * delta_time.TotalSeconds;
-            yaw_rate += yaw_accel * delta_time.TotalSeconds;
 
             //Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll);
 
+            //# update rotational rates in body frame
+        self.pDeg  += roll_accel * delta_time.TotalSeconds;
+        self.qDeg  += pitch_accel * delta_time.TotalSeconds;
+        self.rDeg += yaw_accel * delta_time.TotalSeconds;
+
+            // Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll);
+
+                    // calculate rates in earth frame
+
+             var answer =  BodyRatesToEarthRates(self.roll, self.pitch, self.yaw,
+                                                      self.pDeg, self.qDeg, self.rDeg);
+                    self.roll_rate = answer.Item1;
+         self.pitch_rate = answer.Item2;
+         self.yaw_rate = answer.Item3;
+
+         //self.roll_rate = pDeg;
+         //self.pitch_rate = qDeg;
+         //self.yaw_rate = rDeg;
+
             //# update rotation
             roll += roll_rate * delta_time.TotalSeconds;
             pitch += pitch_rate * delta_time.TotalSeconds;
@@ -143,7 +266,6 @@ namespace ArdupilotMega.HIL
             //# normalise rotations
             normalise();
 
-            double thrust = (m[0] + m[1] + m[2] + m[3]) * thrust_scale;//# Newtons
             double accel = thrust / mass;
 
             //Console.WriteLine("in {0:0.000000} {1:0.000000} {2:0.000000} {3:0.000000}", roll, pitch, yaw, accel);

Tools/ArdupilotMegaPlanner/HIL/Utils.cs

@@ -0,0 +1,104 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+
+namespace ArdupilotMega.HIL
+{
+    public class Utils
+    {
+        public const float rad2deg = (float)(180 / System.Math.PI);
+        public const float deg2rad = (float)(1.0 / rad2deg);
+        public const float ft2m = (float)(1.0 / 3.2808399);
+        public const float kts2fps = (float)1.68780986;
+
+        public static double sin(double val)
+        {
+            return System.Math.Sin(val);
+        }
+        public static double cos(double val)
+        {
+            return System.Math.Cos(val);
+        }
+        public static double radians(double val)
+        {
+            return val * deg2rad;
+        }
+                public static double degrees(double val)
+        {
+            return val * rad2deg;
+        }
+        public static double sqrt(double val)
+        {
+            return System.Math.Sqrt(val);
+        }
+
+        public static int[] range(int no)
+        {
+            int[] range = new int[no];
+            for (int a = 0; a < no; a++)
+            {
+                range[a] = a;
+            }
+            return range;
+        }
+
+        public static double fabs(double val)
+        {
+            return System.Math.Abs(val);
+        }
+
+        public static double tan(double val)
+        {
+            return System.Math.Tan(val);
+        }
+
+        
+public static Tuple<double,double,double> EarthRatesToBodyRates(double roll,double pitch,double yaw,
+                         double rollRate,double pitchRate,double yawRate) {
+    //convert the angular velocities from earth frame to
+    //body frame. Thanks to James Goppert for the formula
+
+    //all inputs and outputs are in degrees
+
+    //returns a tuple, (p,q,r)
+   
+
+    var phi      = radians(roll);
+    var theta    = radians(pitch);
+    var phiDot   = radians(rollRate);
+    var thetaDot = radians(pitchRate);
+    var psiDot   = radians(yawRate);
+
+    var p = phiDot - psiDot*sin(theta);
+    var q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta);
+    var r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot;
+
+    return new Tuple<double,double,double> (degrees(p), degrees(q), degrees(r));
+}
+
+public static Tuple<double,double,double> BodyRatesToEarthRates(double roll, double pitch, double yaw,double  pDeg, double qDeg,double  rDeg){
+    //convert the angular velocities from body frame to
+    //earth frame.
+
+    //all inputs and outputs are in degrees
+
+    //returns a tuple, (rollRate,pitchRate,yawRate)
+
+    var p      = radians(pDeg);
+    var q      = radians(qDeg);
+    var r      = radians(rDeg);
+
+    var phi      = radians(roll);
+    var theta    = radians(pitch);
+
+    var phiDot   = p + tan(theta)*(q*sin(phi) + r*cos(phi));
+    var thetaDot = q*cos(phi) - r*sin(phi);
+    if (fabs(cos(theta)) < 1.0e-20)
+        theta += 1.0e-10;
+    var psiDot   = (q*sin(phi) + r*cos(phi))/cos(theta);
+
+    return new Tuple<double,double,double> (degrees(phiDot), degrees(thetaDot), degrees(psiDot));
+    }
+    }
+}

Tools/ArdupilotMegaPlanner/HUD.cs

@@ -27,6 +27,7 @@ namespace hud
         object paintlock = new object();
         object streamlock = new object();
         MemoryStream _streamjpg = new MemoryStream();
+        [System.ComponentModel.Browsable(false)]
         public MemoryStream streamjpg { get { lock (streamlock) { return _streamjpg; } } set { lock (streamlock) { _streamjpg = value; } } }
         /// <summary>
         /// this is to reduce cpu usage
@@ -44,7 +45,10 @@ namespace hud
         public HUD()
         {
             if (this.DesignMode)
-                return;
+            {
+                opengl = false;
+                //return;
+            }
 
             InitializeComponent();
 
@@ -237,7 +241,7 @@ namespace hud
             {
                 e.Graphics.Clear(this.BackColor);
                 e.Graphics.Flush();
-                return;
+                //return;
             }
 
             if ((DateTime.Now - starttime).TotalMilliseconds < 30 && (_bgimage == null))
@@ -277,7 +281,7 @@ namespace hud
             if (DateTime.Now.Second != countdate.Second)
             {
                 countdate = DateTime.Now;
-                Console.WriteLine("HUD " + count + " hz drawtime " + (huddrawtime / count) + " gl " + opengl);
+               // Console.WriteLine("HUD " + count + " hz drawtime " + (huddrawtime / count) + " gl " + opengl);
                 if ((huddrawtime / count) > 1000)
                     opengl = false;
 
@@ -1202,19 +1206,19 @@ namespace hud
 
                 if (gpsfix == 0)
                 {
-                    gps = resources.GetString("GPS: No GPS.Text");
+                    gps = ("GPS: No GPS");
                 }
                 else if (gpsfix == 1)
                 {
-                    gps = resources.GetString("GPS: No Fix.Text");
+                    gps = ("GPS: No Fix");
                 }
                 else if (gpsfix == 2)
                 {
-                    gps = resources.GetString("GPS: 2D Fix.Text");
+                    gps = ("GPS: 3D Fix");
                 }
                 else if (gpsfix == 3)
                 {
-                    gps = resources.GetString("GPS: 3D Fix.Text");
+                    gps = ("GPS: 3D Fix");
                 }
 
                 drawstring(graphicsObject, gps, font, fontsize + 2, whiteBrush, this.Width - 10 * fontsize, this.Height - 30 - fontoffset);

Tools/ArdupilotMegaPlanner/Log.cs

@@ -347,6 +347,8 @@ namespace ArdupilotMega
 
                     if (items.Length == 11 && items[6] == "0.0000")
                         alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US"));
+                    if (items.Length == 11 && items[6] == "0")
+                        alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US"));
 
 
                     position[positionindex].Add(new Point3D(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")), double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), alt));

Tools/ArdupilotMegaPlanner/MAVLink.cs

@@ -974,7 +974,7 @@ namespace ArdupilotMega
                 actionid == MAV_ACTION.MAV_ACTION_REBOOT)
             {
                 retrys = 1;
-                timeout = 6000;
+                timeout = 20000;
             }
 
             while (true)
@@ -2001,8 +2001,9 @@ namespace ArdupilotMega
 
                     if (temp[0] != 254 && temp[0] != 'U' || lastbad[0] == 'I' && lastbad[1] == 'M') // out of sync
                     {
-                        if (temp[0] >= 0x20 && temp[0] <= 127 || temp[0] == '\n')
+                        if (temp[0] >= 0x20 && temp[0] <= 127 || temp[0] == '\n' || temp[0] == '\r')
                         {
+                            TCPConsole.Write(temp[0]);
                             Console.Write((char)temp[0]);
                         }
                         count = 0;

Tools/ArdupilotMegaPlanner/Program.cs

@@ -4,6 +4,7 @@ using System.Windows.Forms;
 using System.Net;
 using System.IO;
 using System.Text;
+using System.Threading;
 
 
 namespace ArdupilotMega
@@ -32,9 +33,7 @@ namespace ArdupilotMega
             Application.SetCompatibleTextRenderingDefault(false);
             try
             {
-
                 Application.Run(new MainV2());             
-
             }
             catch (Exception ex) { Console.WriteLine(ex.ToString()); }
         }

Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs

@@ -11,7 +11,7 @@ using System.Resources;
 [assembly: AssemblyConfiguration("")]
 [assembly: AssemblyCompany("Michael Oborne")]
 [assembly: AssemblyProduct("ArdupilotMega Planner")]
-[assembly: AssemblyCopyright("Copyright © Michael Oborne 2010-2011")]
+[assembly: AssemblyCopyright("Copyright © Michael Oborne 2010-2012")]
 [assembly: AssemblyTrademark("")]
 [assembly: AssemblyCulture("")]
 
@@ -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.20")]
+[assembly: AssemblyFileVersion("1.1.21")]
 [assembly: NeutralResourcesLanguageAttribute("")]

Tools/ArdupilotMegaPlanner/TCPConsole.cs

@@ -0,0 +1,71 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Net;
+using System.Net.Sockets;
+
+namespace ArdupilotMega
+{
+    static class TCPConsole
+    {
+        static TcpListener listener;
+        static TcpClient client;
+        static bool started = false;
+
+        static void startup()
+        {
+            started = true;
+            try
+            {
+                listener = new TcpListener(IPAddress.Any, 2300);
+
+                listener.Start();
+
+                listener.BeginAcceptTcpClient(
+      new AsyncCallback(DoAcceptTcpClientCallback),
+      listener);
+            }
+            catch { Console.WriteLine("TCP Console fail: port 2300"); return; }
+
+        }
+
+        // Process the client connection.
+        static void DoAcceptTcpClientCallback(IAsyncResult ar)
+        {
+            // Get the listener that handles the client request.
+            TcpListener listener = (TcpListener)ar.AsyncState;
+
+            // End the operation and display the received data on 
+            // the console.
+            client = listener.EndAcceptTcpClient(ar);
+
+            // setup for next listener
+            listener.BeginAcceptTcpClient(
+new AsyncCallback(DoAcceptTcpClientCallback),
+listener);
+
+            // Process the connection here. (Add the client to a
+            // server table, read data, etc.)
+            Console.WriteLine("Client connected completed");
+
+            client.Client.NoDelay = true;
+        }
+
+        public static void Write(byte thing)
+        {
+            if (!started)
+                startup();
+
+            if (client == null)
+                return;
+            if (!client.Connected)
+                return;
+            try
+            {
+                client.GetStream().WriteByte(thing);
+            }
+            catch { }
+        }
+    }
+}

Tools/ArdupilotMegaPlanner/app.config

@@ -3,5 +3,5 @@
   <configSections>
   </configSections>
   <startup useLegacyV2RuntimeActivationPolicy="true">
-</startup>
+<supportedRuntime version="v2.0.50727"/></startup>
 </configuration>

Tools/ArdupilotMegaPlanner/bin/Release/ArduinoCPP.exe.config

@@ -0,0 +1,3 @@
+<?xml version="1.0"?>
+<configuration>
+<startup><supportedRuntime version="v2.0.50727"/></startup></configuration>

Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.exe.config

@@ -3,5 +3,5 @@
   <configSections>
   </configSections>
   <startup useLegacyV2RuntimeActivationPolicy="true">
-</startup>
+<supportedRuntime version="v2.0.50727"/></startup>
 </configuration>

Tools/ArdupilotMegaPlanner/georefimage.cs

@@ -51,7 +51,7 @@ namespace ArdupilotMega
                 }
                 catch (JpegProcessingException e)
                 {
-                    Console.Error.WriteLine(e.Message);
+                    Console.WriteLine(e.Message);
                     return dtaken;
                 }