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

ArduCopter/ArduCopter.pde

@@ -905,8 +905,8 @@ static void fast_loop()
 
 	if(takeoff_complete == false){
 		// reset these I terms to prevent awkward tipping on takeoff
-		reset_rate_I();
+		//reset_rate_I();
-		reset_stability_I();
+		//reset_stability_I();
 	}
 
 	// custom code/exceptions for flight modes

ArduCopter/Log.pde

@@ -222,7 +222,7 @@ void print_latlon(BetterStream *s, int32_t lat_or_lon)
     s->printf("%ld.%07ld",dec_portion,frac_portion);
 }
 
-// Write an GPS packet. Total length : 30 bytes
+// Write an GPS packet. Total length : 31 bytes
 static void Log_Write_GPS()
 {
 	DataFlash.WriteByte(HEAD_BYTE1);
@@ -313,6 +313,7 @@ static void Log_Read_Raw()
 	Serial.println(" ");
 }
 
+// Write an Current data packet. Total length : 16 bytes
 static void Log_Write_Current()
 {
 	DataFlash.WriteByte(HEAD_BYTE1);
@@ -346,6 +347,7 @@ static void Log_Read_Current()
 		temp5);
 }
 
+// Write an Motors packet. Total length : 12 ~ 20 bytes
 static void Log_Write_Motors()
 {
 	DataFlash.WriteByte(HEAD_BYTE1);
@@ -404,7 +406,7 @@ static void Log_Write_Motors()
 	DataFlash.WriteByte(END_BYTE);
 }
 
-// Read a Current packet
+// Read a Motors packet.
 static void Log_Read_Motors()
 {
 	#if FRAME_CONFIG == HEXA_FRAME || FRAME_CONFIG == Y6_FRAME
@@ -472,7 +474,7 @@ static void Log_Read_Motors()
 	#endif
 }
 
-// Write an optical flow packet. Total length : 18 bytes
+// Write an optical flow packet. Total length : 30 bytes
 static void Log_Write_Optflow()
 {
 	#ifdef OPTFLOW_ENABLED
@@ -492,7 +494,7 @@ static void Log_Write_Optflow()
 	#endif
 }
 
-
+// Read an optical flow packet.
 static void Log_Read_Optflow()
 {
 	#ifdef OPTFLOW_ENABLED
@@ -519,6 +521,7 @@ static void Log_Read_Optflow()
 	#endif
 }
 
+// Write an Nav Tuning packet. Total length : 24 bytes
 static void Log_Write_Nav_Tuning()
 {
 	//Matrix3f tempmat = dcm.get_dcm_matrix();
@@ -553,7 +556,7 @@ static void Log_Write_Nav_Tuning()
 	DataFlash.WriteByte(END_BYTE);
 }
 
-
+// Read a Nav Tuning packet.
 static void Log_Read_Nav_Tuning()
 {
 	int16_t temp;
@@ -564,13 +567,13 @@ static void Log_Read_Nav_Tuning()
 		temp = DataFlash.ReadInt();
 		Serial.printf("%d, ", temp);
 	}
-
+	// read 10
 	temp = DataFlash.ReadInt();
 	Serial.printf("%d\n", temp);
 }
 
 
-// Write a control tuning packet. Total length : 22 bytes
+// Write a control tuning packet. Total length : 26 bytes
 static void Log_Write_Control_Tuning()
 {
 	DataFlash.WriteByte(HEAD_BYTE1);
@@ -599,27 +602,26 @@ static void Log_Read_Control_Tuning()
 
 	Serial.printf_P(PSTR("CTUN, "));
 
-	for(int8_t i = 0; i < 11; i++ ){
+	for(int8_t i = 1; i < 11; i++ ){
 		temp = DataFlash.ReadInt();
 		Serial.printf("%d, ", temp);
 	}
-	// read 13
+	// read 11
 	temp = DataFlash.ReadInt();
 	Serial.printf("%d\n", temp);
 }
 
-// Write a performance monitoring packet. Total length : 19 bytes
+// Write a performance monitoring packet. Total length : 9 bytes
 static void Log_Write_Performance()
 {
 	DataFlash.WriteByte(HEAD_BYTE1);
 	DataFlash.WriteByte(HEAD_BYTE2);
 	DataFlash.WriteByte(LOG_PERFORMANCE_MSG);
-
+	DataFlash.WriteByte(	dcm.gyro_sat_count);				//1
-	DataFlash.WriteByte(	dcm.gyro_sat_count);				//2
+	DataFlash.WriteByte(	imu.adc_constraints);				//2
-	DataFlash.WriteByte(	imu.adc_constraints);				//3
+	DataFlash.WriteByte(	dcm.renorm_sqrt_count);				//3
-	DataFlash.WriteByte(	dcm.renorm_sqrt_count);				//4
+	DataFlash.WriteByte(	dcm.renorm_blowup_count);			//4
-	DataFlash.WriteByte(	dcm.renorm_blowup_count);			//5
+	DataFlash.WriteByte(	gps_fix_count);						//5
-	DataFlash.WriteByte(	gps_fix_count);						//6
 	DataFlash.WriteByte(END_BYTE);
 }
 
@@ -641,7 +643,7 @@ static void Log_Read_Performance()
 		temp5);
 }
 
-// Write a command processing packet.
+// Write a command processing packet.  Total length : 21 bytes
 static void Log_Write_Cmd(byte num, struct Location *wp)
 {
 	DataFlash.WriteByte(HEAD_BYTE1);
@@ -686,19 +688,19 @@ static void Log_Read_Cmd()
                     temp8);
 }
 
-// Write an attitude packet. Total length : 10 bytes
+// Write an attitude packet. Total length : 16 bytes
 static void Log_Write_Attitude()
 {
 	DataFlash.WriteByte(HEAD_BYTE1);
 	DataFlash.WriteByte(HEAD_BYTE2);
 	DataFlash.WriteByte(LOG_ATTITUDE_MSG);
 
-	DataFlash.WriteInt(g.rc_1.control_in);			// 0
+	DataFlash.WriteInt(g.rc_1.control_in);			// 1
-	DataFlash.WriteInt((int)dcm.roll_sensor);		// 1
+	DataFlash.WriteInt((int)dcm.roll_sensor);		// 2
-	DataFlash.WriteInt(g.rc_2.control_in);			// 2
+	DataFlash.WriteInt(g.rc_2.control_in);			// 3
-	DataFlash.WriteInt((int)dcm.pitch_sensor);		// 3
+	DataFlash.WriteInt((int)dcm.pitch_sensor);		// 4
-	DataFlash.WriteInt(g.rc_4.control_in);			// 4
+	DataFlash.WriteInt(g.rc_4.control_in);			// 5
-	DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);	// 5
+	DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);	// 6
 
 	DataFlash.WriteByte(END_BYTE);
 }
@@ -714,7 +716,7 @@ static void Log_Read_Attitude()
 	uint16_t temp6 	= DataFlash.ReadInt();
 
 							// 1   2   3    4   5   6
-	Serial.printf_P(PSTR("ATT, %d, %d, %u, %d, %d, %d\n"),
+	Serial.printf_P(PSTR("ATT, %d, %d, %d, %d, %d, %u\n"),
 		temp1,
 		temp2,
 		temp3,
@@ -723,7 +725,7 @@ static void Log_Read_Attitude()
 		temp6);
 }
 
-// Write a mode packet. Total length : 5 bytes
+// Write a mode packet. Total length : 7 bytes
 static void Log_Write_Mode(byte mode)
 {
 	DataFlash.WriteByte(HEAD_BYTE1);
@@ -742,6 +744,7 @@ static void Log_Read_Mode()
 	Serial.printf_P(PSTR(", %d\n"),DataFlash.ReadInt());
 }
 
+// Write Startup packet. Total length : 4 bytes
 static void Log_Write_Startup()
 {
 	DataFlash.WriteByte(HEAD_BYTE1);
@@ -750,7 +753,7 @@ static void Log_Write_Startup()
 	DataFlash.WriteByte(END_BYTE);
 }
 
-// Read a mode packet
+// Read a startup packet
 static void Log_Read_Startup()
 {
 	Serial.printf_P(PSTR("START UP\n"));

ArduCopter/heli.pde

@@ -145,7 +145,7 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
 		// rudder feed forward based on collective
 		#if HIL_MODE == HIL_MODE_DISABLED  // don't do rudder feed forward in simulator
 		if( !g.heli_ext_gyro_enabled ) {
-			yaw_offset = g.heli_coll_yaw_effect * (coll_out_scaled - g.heli_coll_mid);
+			yaw_offset = g.heli_coll_yaw_effect * abs(coll_out_scaled - g.heli_coll_mid);
 		}
 		#endif
 	}

ArduCopter/system.pde

@@ -205,6 +205,7 @@ static void init_ardupilot()
 
     // identify ourselves correctly with the ground station
 	mavlink_system.sysid = g.sysid_this_mav;
+    mavlink_system.type = 2; //MAV_QUADROTOR;
         
 #if LOGGING_ENABLED == ENABLED
     DataFlash.Init();
@@ -626,6 +627,9 @@ check_startup_for_CLI()
 static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
 {
     switch (rate) {
+    case 1:    return 1200;
+    case 2:    return 2400;
+    case 4:    return 4800;
     case 9:    return 9600;
     case 19:   return 19200;
     case 38:   return 38400;

ArduPlane/system.pde

@@ -544,6 +544,9 @@ static void resetPerfData(void) {
 static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
 {
     switch (rate) {
+    case 1:    return 1200;
+    case 2:    return 2400;
+    case 4:    return 4800;
     case 9:    return 9600;
     case 19:   return 19200;
     case 38:   return 38400;

Tools/ArdupilotMegaPlanner/Controls/AGauge.cs

@@ -221,13 +221,13 @@ namespace AGaugeApp
 
 #region properties
         [System.ComponentModel.Browsable(true)]
-        public Single Value0 { get { return m_value[0]; } set { m_NeedIdx = 0; Value = value; } }
+        public Single Value0 { get { return m_value[0]; } set { m_NeedIdx = 0; Value = value; this.Invalidate(); } }
         [System.ComponentModel.Browsable(true)]
-        public Single Value1 { get { return m_value[1]; } set { m_NeedIdx = 1; Value = value; } }
+        public Single Value1 { get { return m_value[1]; } set { m_NeedIdx = 1; Value = value; this.Invalidate(); } }
         [System.ComponentModel.Browsable(true)]
-        public Single Value2 { get { return m_value[2]; } set { m_NeedIdx = 2; Value = value; } }
+        public Single Value2 { get { return m_value[2]; } set { m_NeedIdx = 2; Value = value; this.Invalidate(); } }
         [System.ComponentModel.Browsable(true)]
-        public Single Value3 { get { return m_value[3]; } set { m_NeedIdx = 3; Value = value; } }
+        public Single Value3 { get { return m_value[3]; } set { m_NeedIdx = 3; Value = value; this.Invalidate(); } }
 
         [System.ComponentModel.Browsable(true),
         System.ComponentModel.Category("AGauge"),

Tools/ArdupilotMegaPlanner/CurrentState.cs

@@ -256,7 +256,7 @@ namespace ArdupilotMega
                     if (ind != -1)
                         logdata = logdata.Substring(0, ind);
 
-                    if (messages.Count > 5)
+                    while (messages.Count > 5)
                     {
                         messages.RemoveAt(0);
                     }

Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs

@@ -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 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.Command = new System.Windows.Forms.DataGridViewTextBoxColumn();
             this.Value = new System.Windows.Forms.DataGridViewTextBoxColumn();
@@ -141,31 +141,23 @@
             this.RLL2SRV_P = new System.Windows.Forms.NumericUpDown();
             this.label52 = new System.Windows.Forms.Label();
             this.TabAC = new System.Windows.Forms.TabPage();
+            this.myLabel2 = new ArdupilotMega.MyLabel();
+            this.TUNE = new System.Windows.Forms.ComboBox();
             this.myLabel1 = new ArdupilotMega.MyLabel();
             this.CH7_OPT = new System.Windows.Forms.ComboBox();
-            this.groupBox17 = new System.Windows.Forms.GroupBox();
-            this.ACRO_PIT_IMAX = new System.Windows.Forms.NumericUpDown();
-            this.label27 = new System.Windows.Forms.Label();
-            this.ACRO_PIT_I = new System.Windows.Forms.NumericUpDown();
-            this.label29 = new System.Windows.Forms.Label();
-            this.ACRO_PIT_P = new System.Windows.Forms.NumericUpDown();
-            this.label33 = new System.Windows.Forms.Label();
             this.groupBox5 = new System.Windows.Forms.GroupBox();
+            this.THR_RATE_D = new System.Windows.Forms.NumericUpDown();
+            this.label29 = new System.Windows.Forms.Label();
             this.label14 = new System.Windows.Forms.Label();
             this.THR_RATE_IMAX = new System.Windows.Forms.NumericUpDown();
             this.THR_RATE_I = new System.Windows.Forms.NumericUpDown();
             this.label20 = new System.Windows.Forms.Label();
             this.THR_RATE_P = new System.Windows.Forms.NumericUpDown();
             this.label25 = new System.Windows.Forms.Label();
-            this.groupBox18 = new System.Windows.Forms.GroupBox();
-            this.ACRO_RLL_IMAX = new System.Windows.Forms.NumericUpDown();
-            this.label40 = new System.Windows.Forms.Label();
-            this.ACRO_RLL_I = new System.Windows.Forms.NumericUpDown();
-            this.label44 = new System.Windows.Forms.Label();
-            this.ACRO_RLL_P = new System.Windows.Forms.NumericUpDown();
-            this.label48 = new System.Windows.Forms.Label();
             this.CHK_lockrollpitch = new System.Windows.Forms.CheckBox();
             this.groupBox4 = new System.Windows.Forms.GroupBox();
+            this.NAV_LAT_D = new System.Windows.Forms.NumericUpDown();
+            this.label27 = new System.Windows.Forms.Label();
             this.WP_SPEED_MAX = new System.Windows.Forms.NumericUpDown();
             this.label9 = new System.Windows.Forms.Label();
             this.NAV_LAT_IMAX = new System.Windows.Forms.NumericUpDown();
@@ -199,6 +191,8 @@
             this.STB_YAW_P = new System.Windows.Forms.NumericUpDown();
             this.label35 = new System.Windows.Forms.Label();
             this.groupBox21 = new System.Windows.Forms.GroupBox();
+            this.STAB_D = new System.Windows.Forms.NumericUpDown();
+            this.lblSTAB_D = new System.Windows.Forms.Label();
             this.STB_PIT_IMAX = new System.Windows.Forms.NumericUpDown();
             this.label36 = new System.Windows.Forms.Label();
             this.STB_PIT_I = new System.Windows.Forms.NumericUpDown();
@@ -213,6 +207,8 @@
             this.STB_RLL_P = new System.Windows.Forms.NumericUpDown();
             this.label46 = new System.Windows.Forms.Label();
             this.groupBox23 = new System.Windows.Forms.GroupBox();
+            this.RATE_YAW_D = new System.Windows.Forms.NumericUpDown();
+            this.label10 = new System.Windows.Forms.Label();
             this.RATE_YAW_IMAX = new System.Windows.Forms.NumericUpDown();
             this.label47 = new System.Windows.Forms.Label();
             this.RATE_YAW_I = new System.Windows.Forms.NumericUpDown();
@@ -220,6 +216,8 @@
             this.RATE_YAW_P = new System.Windows.Forms.NumericUpDown();
             this.label82 = new System.Windows.Forms.Label();
             this.groupBox24 = new System.Windows.Forms.GroupBox();
+            this.RATE_PIT_D = new System.Windows.Forms.NumericUpDown();
+            this.label11 = new System.Windows.Forms.Label();
             this.RATE_PIT_IMAX = new System.Windows.Forms.NumericUpDown();
             this.label84 = new System.Windows.Forms.Label();
             this.RATE_PIT_I = new System.Windows.Forms.NumericUpDown();
@@ -227,6 +225,8 @@
             this.RATE_PIT_P = new System.Windows.Forms.NumericUpDown();
             this.label87 = new System.Windows.Forms.Label();
             this.groupBox25 = new System.Windows.Forms.GroupBox();
+            this.RATE_RLL_D = new System.Windows.Forms.NumericUpDown();
+            this.label17 = new System.Windows.Forms.Label();
             this.RATE_RLL_IMAX = new System.Windows.Forms.NumericUpDown();
             this.label88 = new System.Windows.Forms.Label();
             this.RATE_RLL_I = new System.Windows.Forms.NumericUpDown();
@@ -285,8 +285,6 @@
             this.BUT_load = new ArdupilotMega.MyButton();
             this.toolTip1 = new System.Windows.Forms.ToolTip(this.components);
             this.BUT_compare = new ArdupilotMega.MyButton();
-            this.myLabel2 = new ArdupilotMega.MyLabel();
-            this.TUNE = new System.Windows.Forms.ComboBox();
             ((System.ComponentModel.ISupportInitialize)(this.Params)).BeginInit();
             this.ConfigTabs.SuspendLayout();
             this.TabAP.SuspendLayout();
@@ -347,19 +345,13 @@
             ((System.ComponentModel.ISupportInitialize)(this.RLL2SRV_I)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.RLL2SRV_P)).BeginInit();
             this.TabAC.SuspendLayout();
-            this.groupBox17.SuspendLayout();
-            ((System.ComponentModel.ISupportInitialize)(this.ACRO_PIT_IMAX)).BeginInit();
-            ((System.ComponentModel.ISupportInitialize)(this.ACRO_PIT_I)).BeginInit();
-            ((System.ComponentModel.ISupportInitialize)(this.ACRO_PIT_P)).BeginInit();
             this.groupBox5.SuspendLayout();
+            ((System.ComponentModel.ISupportInitialize)(this.THR_RATE_D)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.THR_RATE_IMAX)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.THR_RATE_I)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.THR_RATE_P)).BeginInit();
-            this.groupBox18.SuspendLayout();
-            ((System.ComponentModel.ISupportInitialize)(this.ACRO_RLL_IMAX)).BeginInit();
-            ((System.ComponentModel.ISupportInitialize)(this.ACRO_RLL_I)).BeginInit();
-            ((System.ComponentModel.ISupportInitialize)(this.ACRO_RLL_P)).BeginInit();
             this.groupBox4.SuspendLayout();
+            ((System.ComponentModel.ISupportInitialize)(this.NAV_LAT_D)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.WP_SPEED_MAX)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.NAV_LAT_IMAX)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.NAV_LAT_I)).BeginInit();
@@ -379,6 +371,7 @@
             ((System.ComponentModel.ISupportInitialize)(this.STB_YAW_I)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.STB_YAW_P)).BeginInit();
             this.groupBox21.SuspendLayout();
+            ((System.ComponentModel.ISupportInitialize)(this.STAB_D)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.STB_PIT_IMAX)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.STB_PIT_I)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.STB_PIT_P)).BeginInit();
@@ -387,14 +380,17 @@
             ((System.ComponentModel.ISupportInitialize)(this.STB_RLL_I)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.STB_RLL_P)).BeginInit();
             this.groupBox23.SuspendLayout();
+            ((System.ComponentModel.ISupportInitialize)(this.RATE_YAW_D)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.RATE_YAW_IMAX)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.RATE_YAW_I)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.RATE_YAW_P)).BeginInit();
             this.groupBox24.SuspendLayout();
+            ((System.ComponentModel.ISupportInitialize)(this.RATE_PIT_D)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.RATE_PIT_IMAX)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.RATE_PIT_I)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.RATE_PIT_P)).BeginInit();
             this.groupBox25.SuspendLayout();
+            ((System.ComponentModel.ISupportInitialize)(this.RATE_RLL_D)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.RATE_RLL_IMAX)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.RATE_RLL_I)).BeginInit();
             ((System.ComponentModel.ISupportInitialize)(this.RATE_RLL_P)).BeginInit();
@@ -407,14 +403,14 @@
             this.Params.AllowUserToAddRows = false;
             this.Params.AllowUserToDeleteRows = false;
             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.Columns.AddRange(new System.Windows.Forms.DataGridViewColumn[] {
             this.Command,
@@ -423,14 +419,14 @@
             this.mavScale,
             this.RawValue});
             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.CellValueChanged += new System.Windows.Forms.DataGridViewCellEventHandler(this.Params_CellValueChanged);
             // 
@@ -1095,9 +1091,7 @@
             this.TabAC.Controls.Add(this.TUNE);
             this.TabAC.Controls.Add(this.myLabel1);
             this.TabAC.Controls.Add(this.CH7_OPT);
-            this.TabAC.Controls.Add(this.groupBox17);
             this.TabAC.Controls.Add(this.groupBox5);
-            this.TabAC.Controls.Add(this.groupBox18);
             this.TabAC.Controls.Add(this.CHK_lockrollpitch);
             this.TabAC.Controls.Add(this.groupBox4);
             this.TabAC.Controls.Add(this.groupBox6);
@@ -1112,6 +1106,41 @@
             resources.ApplyResources(this.TabAC, "TabAC");
             this.TabAC.Name = "TabAC";
             // 
+            // myLabel2
+            // 
+            resources.ApplyResources(this.myLabel2, "myLabel2");
+            this.myLabel2.Name = "myLabel2";
+            this.myLabel2.resize = false;
+            // 
+            // TUNE
+            // 
+            this.TUNE.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList;
+            this.TUNE.DropDownWidth = 150;
+            this.TUNE.FormattingEnabled = true;
+            this.TUNE.Items.AddRange(new object[] {
+            resources.GetString("TUNE.Items"),
+            resources.GetString("TUNE.Items1"),
+            resources.GetString("TUNE.Items2"),
+            resources.GetString("TUNE.Items3"),
+            resources.GetString("TUNE.Items4"),
+            resources.GetString("TUNE.Items5"),
+            resources.GetString("TUNE.Items6"),
+            resources.GetString("TUNE.Items7"),
+            resources.GetString("TUNE.Items8"),
+            resources.GetString("TUNE.Items9"),
+            resources.GetString("TUNE.Items10"),
+            resources.GetString("TUNE.Items11"),
+            resources.GetString("TUNE.Items12"),
+            resources.GetString("TUNE.Items13"),
+            resources.GetString("TUNE.Items14"),
+            resources.GetString("TUNE.Items15"),
+            resources.GetString("TUNE.Items16"),
+            resources.GetString("TUNE.Items17"),
+            resources.GetString("TUNE.Items18"),
+            resources.GetString("TUNE.Items19")});
+            resources.ApplyResources(this.TUNE, "TUNE");
+            this.TUNE.Name = "TUNE";
+            // 
             // myLabel1
             // 
             resources.ApplyResources(this.myLabel1, "myLabel1");
@@ -1121,6 +1150,7 @@
             // CH7_OPT
             // 
             this.CH7_OPT.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList;
+            this.CH7_OPT.DropDownWidth = 150;
             this.CH7_OPT.FormattingEnabled = true;
             this.CH7_OPT.Items.AddRange(new object[] {
             resources.GetString("CH7_OPT.Items"),
@@ -1134,50 +1164,10 @@
             resources.ApplyResources(this.CH7_OPT, "CH7_OPT");
             this.CH7_OPT.Name = "CH7_OPT";
             // 
-            // groupBox17
-            // 
-            this.groupBox17.Controls.Add(this.ACRO_PIT_IMAX);
-            this.groupBox17.Controls.Add(this.label27);
-            this.groupBox17.Controls.Add(this.ACRO_PIT_I);
-            this.groupBox17.Controls.Add(this.label29);
-            this.groupBox17.Controls.Add(this.ACRO_PIT_P);
-            this.groupBox17.Controls.Add(this.label33);
-            resources.ApplyResources(this.groupBox17, "groupBox17");
-            this.groupBox17.Name = "groupBox17";
-            this.groupBox17.TabStop = false;
-            // 
-            // ACRO_PIT_IMAX
-            // 
-            resources.ApplyResources(this.ACRO_PIT_IMAX, "ACRO_PIT_IMAX");
-            this.ACRO_PIT_IMAX.Name = "ACRO_PIT_IMAX";
-            // 
-            // label27
-            // 
-            resources.ApplyResources(this.label27, "label27");
-            this.label27.Name = "label27";
-            // 
-            // ACRO_PIT_I
-            // 
-            resources.ApplyResources(this.ACRO_PIT_I, "ACRO_PIT_I");
-            this.ACRO_PIT_I.Name = "ACRO_PIT_I";
-            // 
-            // label29
-            // 
-            resources.ApplyResources(this.label29, "label29");
-            this.label29.Name = "label29";
-            // 
-            // ACRO_PIT_P
-            // 
-            resources.ApplyResources(this.ACRO_PIT_P, "ACRO_PIT_P");
-            this.ACRO_PIT_P.Name = "ACRO_PIT_P";
-            // 
-            // label33
-            // 
-            resources.ApplyResources(this.label33, "label33");
-            this.label33.Name = "label33";
-            // 
             // groupBox5
             // 
+            this.groupBox5.Controls.Add(this.THR_RATE_D);
+            this.groupBox5.Controls.Add(this.label29);
             this.groupBox5.Controls.Add(this.label14);
             this.groupBox5.Controls.Add(this.THR_RATE_IMAX);
             this.groupBox5.Controls.Add(this.THR_RATE_I);
@@ -1188,6 +1178,16 @@
             this.groupBox5.Name = "groupBox5";
             this.groupBox5.TabStop = false;
             // 
+            // THR_RATE_D
+            // 
+            resources.ApplyResources(this.THR_RATE_D, "THR_RATE_D");
+            this.THR_RATE_D.Name = "THR_RATE_D";
+            // 
+            // label29
+            // 
+            resources.ApplyResources(this.label29, "label29");
+            this.label29.Name = "label29";
+            // 
             // label14
             // 
             resources.ApplyResources(this.label14, "label14");
@@ -1218,48 +1218,6 @@
             resources.ApplyResources(this.label25, "label25");
             this.label25.Name = "label25";
             // 
-            // groupBox18
-            // 
-            this.groupBox18.Controls.Add(this.ACRO_RLL_IMAX);
-            this.groupBox18.Controls.Add(this.label40);
-            this.groupBox18.Controls.Add(this.ACRO_RLL_I);
-            this.groupBox18.Controls.Add(this.label44);
-            this.groupBox18.Controls.Add(this.ACRO_RLL_P);
-            this.groupBox18.Controls.Add(this.label48);
-            resources.ApplyResources(this.groupBox18, "groupBox18");
-            this.groupBox18.Name = "groupBox18";
-            this.groupBox18.TabStop = false;
-            // 
-            // ACRO_RLL_IMAX
-            // 
-            resources.ApplyResources(this.ACRO_RLL_IMAX, "ACRO_RLL_IMAX");
-            this.ACRO_RLL_IMAX.Name = "ACRO_RLL_IMAX";
-            // 
-            // label40
-            // 
-            resources.ApplyResources(this.label40, "label40");
-            this.label40.Name = "label40";
-            // 
-            // ACRO_RLL_I
-            // 
-            resources.ApplyResources(this.ACRO_RLL_I, "ACRO_RLL_I");
-            this.ACRO_RLL_I.Name = "ACRO_RLL_I";
-            // 
-            // label44
-            // 
-            resources.ApplyResources(this.label44, "label44");
-            this.label44.Name = "label44";
-            // 
-            // ACRO_RLL_P
-            // 
-            resources.ApplyResources(this.ACRO_RLL_P, "ACRO_RLL_P");
-            this.ACRO_RLL_P.Name = "ACRO_RLL_P";
-            // 
-            // label48
-            // 
-            resources.ApplyResources(this.label48, "label48");
-            this.label48.Name = "label48";
-            // 
             // CHK_lockrollpitch
             // 
             resources.ApplyResources(this.CHK_lockrollpitch, "CHK_lockrollpitch");
@@ -1271,6 +1229,8 @@
             // 
             // groupBox4
             // 
+            this.groupBox4.Controls.Add(this.NAV_LAT_D);
+            this.groupBox4.Controls.Add(this.label27);
             this.groupBox4.Controls.Add(this.WP_SPEED_MAX);
             this.groupBox4.Controls.Add(this.label9);
             this.groupBox4.Controls.Add(this.NAV_LAT_IMAX);
@@ -1283,6 +1243,16 @@
             this.groupBox4.Name = "groupBox4";
             this.groupBox4.TabStop = false;
             // 
+            // NAV_LAT_D
+            // 
+            resources.ApplyResources(this.NAV_LAT_D, "NAV_LAT_D");
+            this.NAV_LAT_D.Name = "NAV_LAT_D";
+            // 
+            // label27
+            // 
+            resources.ApplyResources(this.label27, "label27");
+            this.label27.Name = "label27";
+            // 
             // WP_SPEED_MAX
             // 
             resources.ApplyResources(this.WP_SPEED_MAX, "WP_SPEED_MAX");
@@ -1469,6 +1439,8 @@
             // 
             // groupBox21
             // 
+            this.groupBox21.Controls.Add(this.STAB_D);
+            this.groupBox21.Controls.Add(this.lblSTAB_D);
             this.groupBox21.Controls.Add(this.STB_PIT_IMAX);
             this.groupBox21.Controls.Add(this.label36);
             this.groupBox21.Controls.Add(this.STB_PIT_I);
@@ -1479,6 +1451,16 @@
             this.groupBox21.Name = "groupBox21";
             this.groupBox21.TabStop = false;
             // 
+            // STAB_D
+            // 
+            resources.ApplyResources(this.STAB_D, "STAB_D");
+            this.STAB_D.Name = "STAB_D";
+            // 
+            // lblSTAB_D
+            // 
+            resources.ApplyResources(this.lblSTAB_D, "lblSTAB_D");
+            this.lblSTAB_D.Name = "lblSTAB_D";
+            // 
             // STB_PIT_IMAX
             // 
             resources.ApplyResources(this.STB_PIT_IMAX, "STB_PIT_IMAX");
@@ -1553,6 +1535,8 @@
             // 
             // groupBox23
             // 
+            this.groupBox23.Controls.Add(this.RATE_YAW_D);
+            this.groupBox23.Controls.Add(this.label10);
             this.groupBox23.Controls.Add(this.RATE_YAW_IMAX);
             this.groupBox23.Controls.Add(this.label47);
             this.groupBox23.Controls.Add(this.RATE_YAW_I);
@@ -1563,6 +1547,16 @@
             this.groupBox23.Name = "groupBox23";
             this.groupBox23.TabStop = false;
             // 
+            // RATE_YAW_D
+            // 
+            resources.ApplyResources(this.RATE_YAW_D, "RATE_YAW_D");
+            this.RATE_YAW_D.Name = "RATE_YAW_D";
+            // 
+            // label10
+            // 
+            resources.ApplyResources(this.label10, "label10");
+            this.label10.Name = "label10";
+            // 
             // RATE_YAW_IMAX
             // 
             resources.ApplyResources(this.RATE_YAW_IMAX, "RATE_YAW_IMAX");
@@ -1595,6 +1589,8 @@
             // 
             // groupBox24
             // 
+            this.groupBox24.Controls.Add(this.RATE_PIT_D);
+            this.groupBox24.Controls.Add(this.label11);
             this.groupBox24.Controls.Add(this.RATE_PIT_IMAX);
             this.groupBox24.Controls.Add(this.label84);
             this.groupBox24.Controls.Add(this.RATE_PIT_I);
@@ -1605,6 +1601,16 @@
             this.groupBox24.Name = "groupBox24";
             this.groupBox24.TabStop = false;
             // 
+            // RATE_PIT_D
+            // 
+            resources.ApplyResources(this.RATE_PIT_D, "RATE_PIT_D");
+            this.RATE_PIT_D.Name = "RATE_PIT_D";
+            // 
+            // label11
+            // 
+            resources.ApplyResources(this.label11, "label11");
+            this.label11.Name = "label11";
+            // 
             // RATE_PIT_IMAX
             // 
             resources.ApplyResources(this.RATE_PIT_IMAX, "RATE_PIT_IMAX");
@@ -1637,6 +1643,8 @@
             // 
             // groupBox25
             // 
+            this.groupBox25.Controls.Add(this.RATE_RLL_D);
+            this.groupBox25.Controls.Add(this.label17);
             this.groupBox25.Controls.Add(this.RATE_RLL_IMAX);
             this.groupBox25.Controls.Add(this.label88);
             this.groupBox25.Controls.Add(this.RATE_RLL_I);
@@ -1647,6 +1655,16 @@
             this.groupBox25.Name = "groupBox25";
             this.groupBox25.TabStop = false;
             // 
+            // RATE_RLL_D
+            // 
+            resources.ApplyResources(this.RATE_RLL_D, "RATE_RLL_D");
+            this.RATE_RLL_D.Name = "RATE_RLL_D";
+            // 
+            // label17
+            // 
+            resources.ApplyResources(this.label17, "label17");
+            this.label17.Name = "label17";
+            // 
             // RATE_RLL_IMAX
             // 
             resources.ApplyResources(this.RATE_RLL_IMAX, "RATE_RLL_IMAX");
@@ -2096,40 +2114,6 @@
             this.BUT_compare.UseVisualStyleBackColor = true;
             this.BUT_compare.Click += new System.EventHandler(this.BUT_compare_Click);
             // 
-            // myLabel2
-            // 
-            resources.ApplyResources(this.myLabel2, "myLabel2");
-            this.myLabel2.Name = "myLabel2";
-            this.myLabel2.resize = false;
-            // 
-            // TUNE
-            // 
-            this.TUNE.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList;
-            this.TUNE.FormattingEnabled = true;
-            this.TUNE.Items.AddRange(new object[] {
-            resources.GetString("TUNE.Items"),
-            resources.GetString("TUNE.Items1"),
-            resources.GetString("TUNE.Items2"),
-            resources.GetString("TUNE.Items3"),
-            resources.GetString("TUNE.Items4"),
-            resources.GetString("TUNE.Items5"),
-            resources.GetString("TUNE.Items6"),
-            resources.GetString("TUNE.Items7"),
-            resources.GetString("TUNE.Items8"),
-            resources.GetString("TUNE.Items9"),
-            resources.GetString("TUNE.Items10"),
-            resources.GetString("TUNE.Items11"),
-            resources.GetString("TUNE.Items12"),
-            resources.GetString("TUNE.Items13"),
-            resources.GetString("TUNE.Items14"),
-            resources.GetString("TUNE.Items15"),
-            resources.GetString("TUNE.Items16"),
-            resources.GetString("TUNE.Items17"),
-            resources.GetString("TUNE.Items18"),
-            resources.GetString("TUNE.Items19")});
-            resources.ApplyResources(this.TUNE, "TUNE");
-            this.TUNE.Name = "TUNE";
-            // 
             // Configuration
             // 
             resources.ApplyResources(this, "$this");
@@ -2205,19 +2189,13 @@
             ((System.ComponentModel.ISupportInitialize)(this.RLL2SRV_P)).EndInit();
             this.TabAC.ResumeLayout(false);
             this.TabAC.PerformLayout();
-            this.groupBox17.ResumeLayout(false);
-            ((System.ComponentModel.ISupportInitialize)(this.ACRO_PIT_IMAX)).EndInit();
-            ((System.ComponentModel.ISupportInitialize)(this.ACRO_PIT_I)).EndInit();
-            ((System.ComponentModel.ISupportInitialize)(this.ACRO_PIT_P)).EndInit();
             this.groupBox5.ResumeLayout(false);
+            ((System.ComponentModel.ISupportInitialize)(this.THR_RATE_D)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.THR_RATE_IMAX)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.THR_RATE_I)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.THR_RATE_P)).EndInit();
-            this.groupBox18.ResumeLayout(false);
-            ((System.ComponentModel.ISupportInitialize)(this.ACRO_RLL_IMAX)).EndInit();
-            ((System.ComponentModel.ISupportInitialize)(this.ACRO_RLL_I)).EndInit();
-            ((System.ComponentModel.ISupportInitialize)(this.ACRO_RLL_P)).EndInit();
             this.groupBox4.ResumeLayout(false);
+            ((System.ComponentModel.ISupportInitialize)(this.NAV_LAT_D)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.WP_SPEED_MAX)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.NAV_LAT_IMAX)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.NAV_LAT_I)).EndInit();
@@ -2237,6 +2215,7 @@
             ((System.ComponentModel.ISupportInitialize)(this.STB_YAW_I)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.STB_YAW_P)).EndInit();
             this.groupBox21.ResumeLayout(false);
+            ((System.ComponentModel.ISupportInitialize)(this.STAB_D)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.STB_PIT_IMAX)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.STB_PIT_I)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.STB_PIT_P)).EndInit();
@@ -2245,14 +2224,17 @@
             ((System.ComponentModel.ISupportInitialize)(this.STB_RLL_I)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.STB_RLL_P)).EndInit();
             this.groupBox23.ResumeLayout(false);
+            ((System.ComponentModel.ISupportInitialize)(this.RATE_YAW_D)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.RATE_YAW_IMAX)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.RATE_YAW_I)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.RATE_YAW_P)).EndInit();
             this.groupBox24.ResumeLayout(false);
+            ((System.ComponentModel.ISupportInitialize)(this.RATE_PIT_D)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.RATE_PIT_IMAX)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.RATE_PIT_I)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.RATE_PIT_P)).EndInit();
             this.groupBox25.ResumeLayout(false);
+            ((System.ComponentModel.ISupportInitialize)(this.RATE_RLL_D)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.RATE_RLL_IMAX)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.RATE_RLL_I)).EndInit();
             ((System.ComponentModel.ISupportInitialize)(this.RATE_RLL_P)).EndInit();
@@ -2501,23 +2483,21 @@
         private System.Windows.Forms.Label label109;
         private System.Windows.Forms.Label label14;
         private System.Windows.Forms.Label label26;
-        private System.Windows.Forms.GroupBox groupBox17;
-        private System.Windows.Forms.NumericUpDown ACRO_PIT_IMAX;
-        private System.Windows.Forms.Label label27;
-        private System.Windows.Forms.NumericUpDown ACRO_PIT_I;
-        private System.Windows.Forms.Label label29;
-        private System.Windows.Forms.NumericUpDown ACRO_PIT_P;
-        private System.Windows.Forms.Label label33;
-        private System.Windows.Forms.GroupBox groupBox18;
-        private System.Windows.Forms.NumericUpDown ACRO_RLL_IMAX;
-        private System.Windows.Forms.Label label40;
-        private System.Windows.Forms.NumericUpDown ACRO_RLL_I;
-        private System.Windows.Forms.Label label44;
-        private System.Windows.Forms.NumericUpDown ACRO_RLL_P;
-        private System.Windows.Forms.Label label48;
         private MyLabel myLabel1;
         private System.Windows.Forms.ComboBox CH7_OPT;
         private MyLabel myLabel2;
         private System.Windows.Forms.ComboBox TUNE;
+        private System.Windows.Forms.NumericUpDown RATE_YAW_D;
+        private System.Windows.Forms.Label label10;
+        private System.Windows.Forms.NumericUpDown RATE_PIT_D;
+        private System.Windows.Forms.Label label11;
+        private System.Windows.Forms.NumericUpDown RATE_RLL_D;
+        private System.Windows.Forms.Label label17;
+        private System.Windows.Forms.NumericUpDown NAV_LAT_D;
+        private System.Windows.Forms.Label label27;
+        private System.Windows.Forms.NumericUpDown THR_RATE_D;
+        private System.Windows.Forms.Label label29;
+        private System.Windows.Forms.NumericUpDown STAB_D;
+        private System.Windows.Forms.Label lblSTAB_D;
     }
 }

Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs

@@ -805,7 +805,14 @@ namespace ArdupilotMega.GCSViews
 
             // Add the video device
             hr = m_FilterGraph.AddSourceFilterForMoniker(capDevices[CMB_videosources.SelectedIndex].Mon, null, "Video input", out capFilter);
-            DsError.ThrowExceptionForHR(hr);
+            try
+            {
+                DsError.ThrowExceptionForHR(hr);
+            }
+            catch (Exception ex) {
+                MessageBox.Show("Can not add video source\n" + ex.ToString()); 
+                return; 
+            }
 
             // Find the stream config interface
             hr = capGraph.FindInterface(PinCategory.Capture, MediaType.Video, capFilter, typeof(IAMStreamConfig).GUID, out o);

Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs

@@ -279,16 +279,24 @@ namespace ArdupilotMega.GCSViews
             {
                 cell = Commands.Rows[selectedrow].Cells[Alt.Index] as DataGridViewTextBoxCell;
 
-                cell.Value = TXT_DefaultAlt.Text;
-
-                float result;
-                float.TryParse(TXT_homealt.Text, out result);
-
-                if (result == 0)
                 {
-                    MessageBox.Show("You must have a home altitude");
+                    float result;
+                    float.TryParse(TXT_homealt.Text, out result);
+
+                    if (result == 0)
+                    {
+                        MessageBox.Show("You must have a home altitude");
+                    }
+
+                    if (!float.TryParse(TXT_DefaultAlt.Text, out result))
+                    {
+                        MessageBox.Show("Your default alt is not valid");
+                        return;
+                    }
                 }
 
+                cell.Value = TXT_DefaultAlt.Text;
+
                 float ans;
                 if (float.TryParse(cell.Value.ToString(), out ans))
                 {
@@ -784,7 +792,7 @@ namespace ArdupilotMega.GCSViews
                     cmd = Commands[Command.Index, selectedrow].Value.ToString();
                 }
                 catch { cmd = option; }
-                Console.WriteLine("editformat " + option + " value " + cmd);
+                //Console.WriteLine("editformat " + option + " value " + cmd);
                 ChangeColumnHeader(cmd);
             }
             catch (Exception ex) { MessageBox.Show(ex.ToString()); }
@@ -916,7 +924,7 @@ namespace ArdupilotMega.GCSViews
                 drawnpolygons.Markers.Add(m);
                 drawnpolygons.Markers.Add(mBorders);
             }
-            catch (Exception) { }
+            catch (Exception ex) { Console.WriteLine(ex.ToString()); }
         }
 
         /// <summary>
@@ -1086,11 +1094,16 @@ namespace ArdupilotMega.GCSViews
                 else if (home.Length > 5 && usable == 0)
                 {
                     lookat = "<LookAt>     <longitude>" + TXT_homelng.Text.ToString(new System.Globalization.CultureInfo("en-US")) + "</longitude>     <latitude>" + TXT_homelat.Text.ToString(new System.Globalization.CultureInfo("en-US")) + "</latitude> <range>4000</range> </LookAt>";
-                    MainMap.HoldInvalidation = true;
+
-                    MainMap.ZoomAndCenterMarkers("objects");
+                    RectLatLng? rect = MainMap.GetRectOfAllMarkers("objects");
-                    MainMap.Zoom -= 2;
+                    if (rect.HasValue)
+                    {
+                        MainMap.Position = rect.Value.LocationMiddle;
+                    }
+
+                    MainMap.Zoom = 17;
+
                     MainMap_OnMapZoomChanged();
-                    MainMap.HoldInvalidation = false;
                 }
 
                 RegeneratePolygon();
@@ -1933,8 +1946,13 @@ namespace ArdupilotMega.GCSViews
                     {
                         if (CurentRectMarker.InnerMarker.Tag.ToString().Contains("grid"))
                         {
-                            drawnpolygon.Points[int.Parse(CurentRectMarker.InnerMarker.Tag.ToString().Replace("grid", "")) - 1] = new PointLatLng(end.Lat, end.Lng);
+                            try
-                            MainMap.UpdatePolygonLocalPosition(drawnpolygon);
+                            {
+                                drawnpolygon.Points[int.Parse(CurentRectMarker.InnerMarker.Tag.ToString().Replace("grid", "")) - 1] = new PointLatLng(end.Lat, end.Lng);
+                                MainMap.UpdatePolygonLocalPosition(drawnpolygon);
+                                MainMap.Invalidate();
+                            }
+                            catch { }
                         }
                         else
                         {
@@ -2002,6 +2020,7 @@ namespace ArdupilotMega.GCSViews
                         {
                             drawnpolygon.Points[int.Parse(CurentRectMarker.InnerMarker.Tag.ToString().Replace("grid", "")) - 1] = new PointLatLng(point.Lat, point.Lng);
                             MainMap.UpdatePolygonLocalPosition(drawnpolygon);
+                            MainMap.Invalidate();
                         }
                     }
                     catch { }
@@ -2446,6 +2465,24 @@ namespace ArdupilotMega.GCSViews
                 string angle = (90).ToString("0");
                 Common.InputBox("Angle", "Enter the line direction (0-180)", ref angle);
 
+                double tryme = 0;
+
+                if (!double.TryParse(angle, out tryme))
+                {
+                    MessageBox.Show("Invalid Angle");
+                    return;
+                }
+                if (!double.TryParse(alt, out tryme))
+                {
+                    MessageBox.Show("Invalid Alt");
+                    return;
+                }
+                if (!double.TryParse(distance, out tryme))
+                {
+                    MessageBox.Show("Invalid Distance");
+                    return;
+                }
+
 #if DEBUG
                 //Commands.Rows.Clear();
 #endif

Tools/ArdupilotMegaPlanner/GCSViews/Simulation.Designer.cs

@@ -83,7 +83,7 @@
             this.label17 = new ArdupilotMega.MyLabel();
             this.panel5 = new System.Windows.Forms.Panel();
             this.zg1 = new ZedGraph.ZedGraphControl();
-            this.timer1 = new System.Windows.Forms.Timer(this.components);
+            this.timer_servo_graph = new System.Windows.Forms.Timer(this.components);
             this.panel6 = new System.Windows.Forms.Panel();
             this.label28 = new ArdupilotMega.MyLabel();
             this.label29 = new ArdupilotMega.MyLabel();
@@ -501,9 +501,9 @@
             this.zg1.ScrollMinY = 0D;
             this.zg1.ScrollMinY2 = 0D;
             // 
-            // timer1
+            // timer_servo_graph
             // 
-            this.timer1.Tick += new System.EventHandler(this.timer1_Tick);
+            this.timer_servo_graph.Tick += new System.EventHandler(this.timer1_Tick);
             // 
             // panel6
             // 
@@ -692,7 +692,6 @@
             this.RAD_aerosimrc.Name = "RAD_aerosimrc";
             this.toolTip1.SetToolTip(this.RAD_aerosimrc, resources.GetString("RAD_aerosimrc.ToolTip"));
             this.RAD_aerosimrc.UseVisualStyleBackColor = true;
-            this.RAD_aerosimrc.CheckedChanged += new System.EventHandler(this.RAD_aerosimrc_CheckedChanged);
             // 
             // RAD_JSBSim
             // 
@@ -700,7 +699,6 @@
             this.RAD_JSBSim.Name = "RAD_JSBSim";
             this.toolTip1.SetToolTip(this.RAD_JSBSim, resources.GetString("RAD_JSBSim.ToolTip"));
             this.RAD_JSBSim.UseVisualStyleBackColor = true;
-            this.RAD_JSBSim.CheckedChanged += new System.EventHandler(this.RAD_JSBSim_CheckedChanged);
             // 
             // CHK_xplane10
             // 
@@ -811,7 +809,7 @@
         private ArdupilotMega.MyLabel label19;
         private ArdupilotMega.MyLabel TXT_control_mode;
         private ZedGraph.ZedGraphControl zg1;
-        private System.Windows.Forms.Timer timer1;
+        private System.Windows.Forms.Timer timer_servo_graph;
         private System.Windows.Forms.Panel panel6;
         private System.Windows.Forms.TextBox TXT_ruddergain;
         private System.Windows.Forms.TextBox TXT_pitchgain;

Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs

@@ -307,12 +307,12 @@ namespace ArdupilotMega.GCSViews
                 };
                 t11.Start();
                 MainV2.threads.Add(t11);
-                timer1.Start();
+                timer_servo_graph.Start();
             }
             else
             {
 
-                timer1.Stop();
+                timer_servo_graph.Stop();
                 threadrun = 0;
                 if (SimulatorRECV != null)
                     SimulatorRECV.Close();
@@ -569,7 +569,8 @@ namespace ArdupilotMega.GCSViews
                             RECVprocess(udpdata, recv, comPort);
                         }
                     }
-                    catch { //OutputLog.AppendText("Xplanes Data Problem - You need DATA IN/OUT 3, 4, 17, 18, 19, 20\n" + ex.Message + "\n");
+                    catch
+                    { //OutputLog.AppendText("Xplanes Data Problem - You need DATA IN/OUT 3, 4, 17, 18, 19, 20\n" + ex.Message + "\n");
                     }
                 }
                 if (MavLink != null && MavLink.Client != null && MavLink.Client.Connected && MavLink.Available > 0)
@@ -606,7 +607,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;
                 }
@@ -675,11 +676,11 @@ namespace ArdupilotMega.GCSViews
             MavLink = new UdpClient("127.0.0.1", 14550);
         }
 
-        float oldax =0, olday =0, oldaz = 0;
+        float oldax = 0, olday = 0, oldaz = 0;
         DateTime oldtime = DateTime.Now;
 #if MAVLINK10
         ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t oldgps = new MAVLink.__mavlink_gps_raw_int_t();
-		#endif
+#endif
 
         ArdupilotMega.MAVLink.__mavlink_attitude_t oldatt = new ArdupilotMega.MAVLink.__mavlink_attitude_t();
 
@@ -740,7 +741,9 @@ namespace ArdupilotMega.GCSViews
                     att.pitchspeed = (DATA[17][0]);
                     att.rollspeed = (DATA[17][1]);
                     att.yawspeed = (DATA[17][2]);
-                } else {
+                }
+                else
+                {
                     att.pitch = (DATA[17][0] * deg2rad);
                     att.roll = (DATA[17][1] * deg2rad);
                     att.yaw = (DATA[17][2] * deg2rad);
@@ -799,9 +802,9 @@ namespace ArdupilotMega.GCSViews
                 double head = DATA[18][2] - 90;
 #if MAVLINK10
                 imu.time_usec = ((ulong)DateTime.Now.ToBinary());
-				#else
+#else
-				imu.usec = ((ulong)DateTime.Now.ToBinary());
+                imu.usec = ((ulong)DateTime.Now.ToBinary());
-				#endif
+#endif
                 imu.xgyro = xgyro; // roll - yes
                 imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
                 imu.ygyro = ygyro; // pitch - yes
@@ -817,7 +820,14 @@ namespace ArdupilotMega.GCSViews
 #if MAVLINK10
                 gps.alt = (int)(DATA[20][2] * ft2m * 1000);
                 gps.fix_type = 3;
-                gps.cog = (ushort)(DATA[19][2] * 100);
+                                if (xplane9)
+                {
+                    gps.cog = ((float)DATA[19][2]);
+                }
+                else
+                {
+                    gps.cog = ((float)DATA[18][2]);
+                }
                 gps.lat = (int)(DATA[20][0] * 1.0e7);
                 gps.lon = (int)(DATA[20][1] * 1.0e7);
                 gps.time_usec = ((ulong)0);
@@ -825,7 +835,14 @@ namespace ArdupilotMega.GCSViews
 #else
                 gps.alt = ((float)(DATA[20][2] * ft2m));
                 gps.fix_type = 3;
-                gps.hdg = ((float)DATA[19][2]);
+                if (xplane9)
+                {
+                    gps.hdg = ((float)DATA[19][2]);
+                }
+                else
+                {
+                    gps.hdg = ((float)DATA[18][2]);
+                }
                 gps.lat = ((float)DATA[20][0]);
                 gps.lon = ((float)DATA[20][1]);
                 gps.usec = ((ulong)0);
@@ -860,9 +877,9 @@ namespace ArdupilotMega.GCSViews
 
 #if MAVLINK10
                 imu.time_usec = ((ulong)DateTime.Now.ToBinary());
-				#else
+#else
-				imu.usec = ((ulong)DateTime.Now.ToBinary());
+                imu.usec = ((ulong)DateTime.Now.ToBinary());
-				#endif
+#endif
 
                 imu.xacc = ((Int16)(imudata2.accelX * 9808 / 32.2));
                 imu.xgyro = ((Int16)(imudata2.rateRoll * 17.453293));
@@ -927,9 +944,9 @@ namespace ArdupilotMega.GCSViews
 
 #if MAVLINK10
                 imu.time_usec = ((ulong)DateTime.Now.ToBinary());
-				#else
+#else
-				imu.usec = ((ulong)DateTime.Now.ToBinary());
+                imu.usec = ((ulong)DateTime.Now.ToBinary());
-				#endif
+#endif
                 imu.xgyro = (short)(aeroin.Model_fAngVel_Body_X * 1000); // roll - yes
                 //imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
                 imu.ygyro = (short)(aeroin.Model_fAngVel_Body_Y * 1000); // pitch - yes
@@ -943,7 +960,7 @@ namespace ArdupilotMega.GCSViews
                 imu.yacc = (Int16)((accel3D.Y + aeroin.Model_fAccel_Body_Y) * 1000); // roll
                 imu.zacc = (Int16)((accel3D.Z + aeroin.Model_fAccel_Body_Z) * 1000);
 
-//                Console.WriteLine("x {0} y {1} z {2}", imu.xacc, imu.yacc, imu.zacc);
+                //                Console.WriteLine("x {0} y {1} z {2}", imu.xacc, imu.yacc, imu.zacc);
 
 #if MAVLINK10
                 gps.alt = ((int)(aeroin.Model_fPosZ) * 1000);
@@ -988,9 +1005,9 @@ namespace ArdupilotMega.GCSViews
 
 #if MAVLINK10
                 imu.time_usec = ((ulong)DateTime.Now.ToBinary());
-				#else
+#else
-				imu.usec = ((ulong)DateTime.Now.ToBinary());
+                imu.usec = ((ulong)DateTime.Now.ToBinary());
-				#endif
+#endif
                 imu.xgyro = (short)(fdm.phidot); // roll - yes
                 //imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
                 imu.ygyro = (short)(fdm.thetadot); // pitch - yes
@@ -1134,7 +1151,7 @@ namespace ArdupilotMega.GCSViews
                 Array.Copy(BitConverter.GetBytes((double)lastfdmdata.psi * rad2deg), 0, sitlout, a += 8, 8);
                 Array.Copy(BitConverter.GetBytes((double)lastfdmdata.vcas * ft2m), 0, sitlout, a += 8, 8);
 
-//                Console.WriteLine(lastfdmdata.theta);
+                //                Console.WriteLine(lastfdmdata.theta);
 
                 Array.Copy(BitConverter.GetBytes((int)0x4c56414e), 0, sitlout, a += 8, 4);
 
@@ -1176,27 +1193,27 @@ namespace ArdupilotMega.GCSViews
 
             comPort.sendPacket(asp);
 			
-			#else
+#else
 
             if (chkSensor.Checked == false) // attitude
             {
-                comPort.sendPacket( att);
+                comPort.sendPacket(att);
 
-                comPort.sendPacket( asp);
+                comPort.sendPacket(asp);
             }
             else // raw imu
             {
                 // imudata
 
-                comPort.sendPacket( imu);
+                comPort.sendPacket(imu);
 
-			#endif
+#endif
 
-            MAVLink.__mavlink_raw_pressure_t pres = new MAVLink.__mavlink_raw_pressure_t();
+                MAVLink.__mavlink_raw_pressure_t pres = new MAVLink.__mavlink_raw_pressure_t();
-            double calc = (101325 * Math.Pow(1 - 2.25577 * Math.Pow(10, -5) * gps.alt, 5.25588)); // updated from valid gps
+                double calc = (101325 * Math.Pow(1 - 2.25577 * Math.Pow(10, -5) * gps.alt, 5.25588)); // updated from valid gps
-            pres.press_diff1 = (short)(int)(calc - 101325); // 0 alt is 0 pa
+                pres.press_diff1 = (short)(int)(calc - 101325); // 0 alt is 0 pa
 
-            comPort.sendPacket(pres);
+                comPort.sendPacket(pres);
 #if !MAVLINK10
                 comPort.sendPacket(asp);
             }
@@ -1228,7 +1245,7 @@ namespace ArdupilotMega.GCSViews
         /// <param name="pitch_out">-1 to 1</param>
         /// <param name="rudder_out">-1 to 1</param>
         /// <param name="throttle_out">0 to 1</param>
-        private void updateScreenDisplay(double lat,double lng,double alt,double roll,double pitch,double heading, double yaw,double roll_out,double pitch_out, double rudder_out, double throttle_out)
+        private void updateScreenDisplay(double lat, double lng, double alt, double roll, double pitch, double heading, double yaw, double roll_out, double pitch_out, double rudder_out, double throttle_out)
         {
             try
             {
@@ -1329,7 +1346,7 @@ namespace ArdupilotMega.GCSViews
                 }
                 catch (Exception) { Console.WriteLine("Socket Write failed, FG closed?"); }
 
-                updateScreenDisplay(lastfdmdata.latitude,lastfdmdata.longitude,lastfdmdata.altitude * .3048,lastfdmdata.phi,lastfdmdata.theta,lastfdmdata.psi,lastfdmdata.psi,m[0],m[1],m[2],m[3]);
+                updateScreenDisplay(lastfdmdata.latitude, lastfdmdata.longitude, lastfdmdata.altitude * .3048, lastfdmdata.phi, lastfdmdata.theta, lastfdmdata.psi, lastfdmdata.psi, m[0], m[1], m[2], m[3]);
 
                 return;
 
@@ -1350,10 +1367,9 @@ namespace ArdupilotMega.GCSViews
             }
             else
             {
-
                 roll_out = (float)MainV2.cs.hilch1 / rollgain;
                 pitch_out = (float)MainV2.cs.hilch2 / pitchgain;
-                throttle_out = ((float)MainV2.cs.hilch3 / 2 + 5000) / throttlegain;
+                throttle_out = ((float)MainV2.cs.hilch3) / throttlegain;
                 rudder_out = (float)MainV2.cs.hilch4 / ruddergain;
 
                 if (RAD_aerosimrc.Checked && CHK_quad.Checked)
@@ -1415,7 +1431,9 @@ namespace ArdupilotMega.GCSViews
                         if (xplane9)
                         {
                             updateScreenDisplay(DATA[20][0] * deg2rad, DATA[20][1] * deg2rad, DATA[20][2] * .3048, DATA[18][1] * deg2rad, DATA[18][0] * deg2rad, DATA[19][2] * deg2rad, DATA[18][2] * deg2rad, roll_out, pitch_out, rudder_out, throttle_out);
-                        } else {
+                        }
+                        else
+                        {
 
                             updateScreenDisplay(DATA[20][0] * deg2rad, DATA[20][1] * deg2rad, DATA[20][2] * .3048, DATA[17][1] * deg2rad, DATA[17][0] * deg2rad, DATA[18][2] * deg2rad, DATA[17][2] * deg2rad, roll_out, pitch_out, rudder_out, throttle_out);
                         }
@@ -1444,7 +1462,7 @@ namespace ArdupilotMega.GCSViews
                 Array.Copy(BitConverter.GetBytes((double)(roll_out * REV_roll)), 0, AeroSimRC, 0, 8);
                 Array.Copy(BitConverter.GetBytes((double)(pitch_out * REV_pitch * -1)), 0, AeroSimRC, 8, 8);
                 Array.Copy(BitConverter.GetBytes((double)(rudder_out * REV_rudder)), 0, AeroSimRC, 16, 8);
-                Array.Copy(BitConverter.GetBytes((double)((throttle_out *2) -1)), 0, AeroSimRC, 24, 8);
+                Array.Copy(BitConverter.GetBytes((double)((throttle_out * 2) - 1)), 0, AeroSimRC, 24, 8);
 
                 if (heli)
                 {
@@ -1487,12 +1505,12 @@ namespace ArdupilotMega.GCSViews
             if (RAD_JSBSim.Checked)
             {
                 roll_out = Constrain(roll_out * REV_roll, -1f, 1f);
-                pitch_out = Constrain(-pitch_out * REV_pitch, -1f,1f);
+                pitch_out = Constrain(-pitch_out * REV_pitch, -1f, 1f);
                 rudder_out = Constrain(rudder_out * REV_rudder, -1f, 1f);
 
                 throttle_out = Constrain(throttle_out, -0.0f, 1f);
 
-                string cmd = string.Format("set fcs/aileron-cmd-norm {0}\r\nset fcs/elevator-cmd-norm {1}\r\nset fcs/rudder-cmd-norm {2}\r\nset fcs/throttle-cmd-norm {3}\r\n",roll_out,pitch_out,rudder_out,throttle_out);
+                string cmd = string.Format("set fcs/aileron-cmd-norm {0}\r\nset fcs/elevator-cmd-norm {1}\r\nset fcs/rudder-cmd-norm {2}\r\nset fcs/throttle-cmd-norm {3}\r\n", roll_out, pitch_out, rudder_out, throttle_out);
 
                 //Console.Write(cmd);
                 byte[] data = System.Text.Encoding.ASCII.GetBytes(cmd);
@@ -1736,9 +1754,9 @@ namespace ArdupilotMega.GCSViews
             //myPane.Chart.Fill = new Fill(Color.White, Color.LightGray, 45.0f);
 
             // Sample at 50ms intervals
-            timer1.Interval = 50;
+            timer_servo_graph.Interval = 50;
-            timer1.Enabled = true;
+            timer_servo_graph.Enabled = true;
-            timer1.Start();
+            timer_servo_graph.Start();
 
 
             // Calculate the Axis Scale Ranges
@@ -1778,14 +1796,14 @@ namespace ArdupilotMega.GCSViews
                 xScale.Max = time + xScale.MajorStep;
                 xScale.Min = xScale.Max - 30.0;
             }
-                // Make sure the Y axis is rescaled to accommodate actual data
+            // Make sure the Y axis is rescaled to accommodate actual data
-                try
+            try
-                {
+            {
-                    zg1.AxisChange();
+                zg1.AxisChange();
-                }
+            }
-                catch { }
+            catch { }
-                // Force a redraw
+            // Force a redraw
-                zg1.Invalidate();
+            zg1.Invalidate();
         }
 
         private void SaveSettings_Click(object sender, EventArgs e)
@@ -1965,7 +1983,7 @@ namespace ArdupilotMega.GCSViews
 
                 if (!MainV2.MONO)
                 {
-                    extra = " --fg-root=\"" + Path.GetDirectoryName(ofd.FileName.ToLower().Replace("bin\\win32\\","")) + "\\data\"";
+                    extra = " --fg-root=\"" + Path.GetDirectoryName(ofd.FileName.ToLower().Replace("bin\\win32\\", "")) + "\\data\"";
                 }
 
                 System.Diagnostics.Process P = new System.Diagnostics.Process();
@@ -2044,7 +2062,7 @@ namespace ArdupilotMega.GCSViews
             if (displayfull)
             {
                 //this.Width = 651;
-                timer1.Start();
+                timer_servo_graph.Start();
                 zg1.Visible = true;
 
 
@@ -2059,7 +2077,7 @@ namespace ArdupilotMega.GCSViews
                 //this.Width = 651;
                 //this.Height = 457;
 
-                timer1.Stop();
+                timer_servo_graph.Stop();
                 zg1.Visible = false;
 
                 CHKgraphpitch.Visible = false;
@@ -2069,14 +2087,5 @@ namespace ArdupilotMega.GCSViews
             }
         }
 
-        private void RAD_aerosimrc_CheckedChanged(object sender, EventArgs e)
-        {
-
-        }
-
-        private void RAD_JSBSim_CheckedChanged(object sender, EventArgs e)
-        {
-
-        }
     }
 }

Tools/ArdupilotMegaPlanner/GCSViews/Simulation.resx

@@ -1356,7 +1356,7 @@
   <data name="&gt;&gt;zg1.ZOrder" xml:space="preserve">
     <value>17</value>
   </data>
-  <metadata name="timer1.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
+  <metadata name="timer_servo_graph.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
     <value>17, 17</value>
   </metadata>
   <data name="label28.Location" type="System.Drawing.Point, System.Drawing">
@@ -2142,10 +2142,10 @@
   <data name="&gt;&gt;currentStateBindingSource.Type" xml:space="preserve">
     <value>System.Windows.Forms.BindingSource, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
   </data>
-  <data name="&gt;&gt;timer1.Name" xml:space="preserve">
+  <data name="&gt;&gt;timer_servo_graph.Name" xml:space="preserve">
-    <value>timer1</value>
+    <value>timer_servo_graph</value>
   </data>
-  <data name="&gt;&gt;timer1.Type" xml:space="preserve">
+  <data name="&gt;&gt;timer_servo_graph.Type" xml:space="preserve">
     <value>System.Windows.Forms.Timer, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
   </data>
   <data name="&gt;&gt;toolTip1.Name" xml:space="preserve">

Tools/ArdupilotMegaPlanner/MAVLink.cs

@@ -2001,12 +2001,13 @@ namespace ArdupilotMega
                         else
                         {
                             MainV2.cs.datetime = DateTime.Now;
-                            temp[count] = (byte)BaseStream.ReadByte();
+                            if (BaseStream.IsOpen)
+                                temp[count] = (byte)BaseStream.ReadByte();
                         }
                     }
                     catch (Exception e) { Console.WriteLine("MAVLink readpacket read error: " + e.Message); break; }
 
-                    if (temp[0] != 254 && temp[0] != 'U' || lastbad[0] == 'I' && lastbad[1] == 'M' || lastbad[1] == 'G') // out of sync
+                    if (temp[0] != 254 && temp[0] != 'U' || lastbad[0] == 'I' && lastbad[1] == 'M' || lastbad[1] == 'G' || lastbad[1] == 'A') // out of sync "AUTO" "GUIDED" "IMU"
                     {
                         if (temp[0] >= 0x20 && temp[0] <= 127 || temp[0] == '\n' || temp[0] == '\r')
                         {
@@ -2058,7 +2059,10 @@ namespace ArdupilotMega
 
                                         //Console.WriteLine("data " + 0 + " " + length + " aval " + BaseStream.BytesToRead);
                                     }
-                                    int read = BaseStream.Read(temp, 6, length - 4);
+                                    if (BaseStream.IsOpen)
+                                    {
+                                        int read = BaseStream.Read(temp, 6, length - 4);
+                                    }
                                 }
                                 //Console.WriteLine("data " + read + " " + length + " aval " + this.BytesToRead);
                                 count = length + 2;
@@ -2113,6 +2117,10 @@ namespace ArdupilotMega
             if (temp.Length > 5 && temp[1] != MAVLINK_MESSAGE_LENGTHS[temp[5]])
             {
                 Console.WriteLine("Mavlink Bad Packet (Len Fail) len {0} pkno {1}", temp.Length, temp[5]);
+#if MAVLINK10
+                if (temp.Length == 11 && temp[0] == 'U' && temp[5] == 0)
+                    throw new Exception("Mavlink 0.9 Heartbeat, Please upgrade your AP, This planner is for Mavlink 1.0\n\n");
+#endif
                 return new byte[0];
             }
 

Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs

@@ -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.30")]
+[assembly: AssemblyFileVersion("1.1.33")]
 [assembly: NeutralResourcesLanguageAttribute("")]

Tools/ArdupilotMegaPlanner/ResEdit.cs

@@ -196,6 +196,12 @@ namespace resedit
 
         private void button3_Click(object sender, EventArgs e)
         {
+            if (!File.Exists("translation/output.html"))
+            {
+                MessageBox.Show("No existing translation has been done");
+                return;
+            }
+
             StreamReader sr1 = new StreamReader("translation/output.html");
 
             string file = sr1.ReadToEnd();

Tools/ArdupilotMegaPlanner/Setup/Setup.cs

@@ -220,7 +220,7 @@ namespace ArdupilotMega.Setup
                 }
             }
 
-            MessageBox.Show("Ensure all your sticks are centered, and click ok to continue");
+            MessageBox.Show("Ensure all your sticks are centered and throttle is down, and click ok to continue");
 
             MainV2.cs.UpdateCurrentSettings(currentStateBindingSource, true, MainV2.comPort);
 
@@ -800,13 +800,18 @@ namespace ArdupilotMega.Setup
         {
             if (startup || ((TextBox)sender).Enabled == false)
                 return;
-            float measuredvoltage = float.Parse(TXT_measuredvoltage.Text);
+            try
-            float voltage = float.Parse(TXT_voltage.Text);
+            {
-            float divider = float.Parse(TXT_divider.Text);
+                float measuredvoltage = float.Parse(TXT_measuredvoltage.Text);
-            if (voltage == 0)
+                float voltage = float.Parse(TXT_voltage.Text);
-                return;
+                float divider = float.Parse(TXT_divider.Text);
-            float new_divider = (measuredvoltage * divider) / voltage;
+                if (voltage == 0)
-            TXT_divider.Text = new_divider.ToString();
+                    return;
+                float new_divider = (measuredvoltage * divider) / voltage;
+                TXT_divider.Text = new_divider.ToString();
+            }
+            catch { MessageBox.Show("Invalid number entered"); return; }
+
             try
             {
                 if (MainV2.comPort.param["VOLT_DIVIDER"] == null)

Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml

@@ -13,12 +13,12 @@
       <F8>CRS</F8>
     </GPS>
     <ATT>
-      <F1>Roll</F1>
+      <F1>Roll IN</F1>
-      <F2>Pitch</F2>
+      <F2>Roll</F2>
-      <F3>Yaw</F3>
+      <F3>Pitch IN</F3>
-      <F4>CH1 out</F4>
+      <F4>Pitch</F4>
-      <F5>CH2 out</F5>
+      <F5>Yaw IN</F5>
-      <F6>CH4 out</F6>
+      <F6>Yaw</F6>
     </ATT>
     <NTUN>
       <F1>WP Dist</F1>
@@ -27,26 +27,23 @@
       <F4>Lat Err</F4>
       <F5>nav lon</F5>
       <F6>nav lat</F6>
-      <F7>nav lon I</F7>
+	  <F7>X Speed</F7>
-      <F8>nav lat I</F8>
+	  <F8>Y Speed</F8>
-      <F9>Loiter Lon I</F9>
+      <F9>nav lon I</F9>
-      <F10>Loiter Lat I</F10>
+      <F10>nav lat I</F10>
     </NTUN>
     <CTUN>
-      <F1>Roll IN</F1>
+      <F1>Thr IN</F1>
-      <F2>Pitch IN</F2>
+      <F2>Sonar Alt</F2>
-      <F3>Thr IN</F3>
+      <F3>Baro Alt</F3>
-      <F4>Yaw IN</F4>
+      <F4>WP Alt</F4>
-      <F5>Sonar Alt</F5>
+      <F5>Nav Throttle</F5>
-      <F6>Baro Alt</F6>
+      <F6>Angle boost</F6>
-      <F7>Next WP Alt</F7>
+      <F7>Manual boost</F7>
-      <F8>Nav Throttle</F8>
+      <F8>Climb Rate</F8>
-      <F9>Angle boost</F9>
+      <F9>rc3 servo out</F9>
-      <F10>Manual boost</F10>
+      <F10>alt hold int</F10>
-      <F11>Climb Rate</F11>
+      <F11>Thr int</F11>
-      <F12>rc3 servo out</F12>
-      <F13>alt hold int</F13>
-      <F14>Thr int</F14>
     </CTUN>
     <PM>
       <F1>Perf Timer</F1>
@@ -65,8 +62,8 @@
       <F6>Accel Z</F6>
     </RAW>
     <CURR>
-      <F1>Throttle in</F1>
+      <F1>Thr IN</F1>
-      <F2>Throttle intergrator</F2>
+      <F2>Thr int</F2>
       <F3>Voltage</F3>
       <F4>Current</F4>
       <F5>Current total</F5>
@@ -76,8 +73,22 @@
       <F2>Current #</F2>
       <F3>ID</F3>
       <F4>options</F4>
-      <F5></F5>
+      <F5>p1</F5>
+	  <F6>Alt</F6>
+	  <F7>Lat</F7>
+	  <F8>Long</F8>
     </CMD>
+    <OF>
+      <F1>X raw</F1>
+      <F2>Y raw</F2>
+      <F3>SurfaceQual</F3>
+      <F4>X cm</F4>
+      <F5>Y cm</F5>
+	  <F6>Lat</F6>
+	  <F7>Long</F7>
+	  <F8>Roll Cmd</F8>
+	  <F9>Pitch Cmd</F9>
+    </OF>
     <MOD>
       <F1>FlightMode</F1>
       <F2>Thr Cruise</F2>

Tools/ArdupilotMegaPlanner/dataflashlog.xml

@@ -13,12 +13,12 @@
       <F8>CRS</F8>
     </GPS>
     <ATT>
-      <F1>Roll</F1>
+      <F1>Roll IN</F1>
-      <F2>Pitch</F2>
+      <F2>Roll</F2>
-      <F3>Yaw</F3>
+      <F3>Pitch IN</F3>
-      <F4>CH1 out</F4>
+      <F4>Pitch</F4>
-      <F5>CH2 out</F5>
+      <F5>Yaw IN</F5>
-      <F6>CH4 out</F6>
+      <F6>Yaw</F6>
     </ATT>
     <NTUN>
       <F1>WP Dist</F1>
@@ -27,26 +27,23 @@
       <F4>Lat Err</F4>
       <F5>nav lon</F5>
       <F6>nav lat</F6>
-      <F7>nav lon I</F7>
+	  <F7>X Speed</F7>
-      <F8>nav lat I</F8>
+	  <F8>Y Speed</F8>
-      <F9>Loiter Lon I</F9>
+      <F9>nav lon I</F9>
-      <F10>Loiter Lat I</F10>
+      <F10>nav lat I</F10>
     </NTUN>
     <CTUN>
-      <F1>Roll IN</F1>
+      <F1>Thr IN</F1>
-      <F2>Pitch IN</F2>
+      <F2>Sonar Alt</F2>
-      <F3>Thr IN</F3>
+      <F3>Baro Alt</F3>
-      <F4>Yaw IN</F4>
+      <F4>WP Alt</F4>
-      <F5>Sonar Alt</F5>
+      <F5>Nav Throttle</F5>
-      <F6>Baro Alt</F6>
+      <F6>Angle boost</F6>
-      <F7>Next WP Alt</F7>
+      <F7>Manual boost</F7>
-      <F8>Nav Throttle</F8>
+      <F8>Climb Rate</F8>
-      <F9>Angle boost</F9>
+      <F9>rc3 servo out</F9>
-      <F10>Manual boost</F10>
+      <F10>alt hold int</F10>
-      <F11>Climb Rate</F11>
+      <F11>Thr int</F11>
-      <F12>rc3 servo out</F12>
-      <F13>alt hold int</F13>
-      <F14>Thr int</F14>
     </CTUN>
     <PM>
       <F1>Perf Timer</F1>
@@ -65,8 +62,8 @@
       <F6>Accel Z</F6>
     </RAW>
     <CURR>
-      <F1>Throttle in</F1>
+      <F1>Thr IN</F1>
-      <F2>Throttle intergrator</F2>
+      <F2>Thr int</F2>
       <F3>Voltage</F3>
       <F4>Current</F4>
       <F5>Current total</F5>
@@ -76,8 +73,22 @@
       <F2>Current #</F2>
       <F3>ID</F3>
       <F4>options</F4>
-      <F5></F5>
+      <F5>p1</F5>
+	  <F6>Alt</F6>
+	  <F7>Lat</F7>
+	  <F8>Long</F8>
     </CMD>
+    <OF>
+      <F1>X raw</F1>
+      <F2>Y raw</F2>
+      <F3>SurfaceQual</F3>
+      <F4>X cm</F4>
+      <F5>Y cm</F5>
+	  <F6>Lat</F6>
+	  <F7>Long</F7>
+	  <F8>Roll Cmd</F8>
+	  <F9>Pitch Cmd</F9>
+    </OF>
     <MOD>
       <F1>FlightMode</F1>
       <F2>Thr Cruise</F2>

libraries/AC_PID/examples/AC_PID_test/AC_PID_test.pde

@@ -0,0 +1,64 @@
+/*
+	Example of PID library.
+	2012 Code by Jason Short, Randy Mackay. DIYDrones.com
+*/
+
+// includes
+#include <Arduino_Mega_ISR_Registry.h>
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <APM_RC.h> // ArduPilot RC Library
+#include <AC_PID.h> // ArduPilot Mega RC Library
+
+// default PID values
+#define TEST_P 1.0
+#define TEST_I 0.01
+#define TEST_D 0.2
+#define TEST_IMAX 10
+
+// Serial ports
+FastSerialPort0(Serial);        // FTDI/console
+
+// Global variables
+Arduino_Mega_ISR_Registry isr_registry;
+APM_RC_APM1 APM_RC;
+
+// setup function
+void setup()
+{
+	Serial.begin(115200);
+	Serial.println("ArduPilot Mega AC_PID library test");
+
+	isr_registry.init();
+	APM_RC.Init(&isr_registry);	 // APM Radio initialization
+
+	delay(1000);
+}
+
+// main loop
+void loop()
+{
+	// setup (unfortunately must be done here as we cannot create a global AC_PID object)
+	AC_PID pid(0, PSTR("TESTPID_"),  TEST_P, TEST_I, TEST_D, TEST_IMAX * 100);
+	uint16_t radio_in;
+	uint16_t radio_trim;
+	int error;
+	int control;
+	float dt = 1000/50;
+
+	// display PID gains
+	Serial.printf("P %f  I %f  D %f  imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax());
+
+	// capture radio trim
+	radio_trim = APM_RC.InputCh(0);
+
+	while( true ) {
+		radio_in = APM_RC.InputCh(0);
+		error = radio_in - radio_trim;
+		control = pid.get_pid(error, dt);
+
+		// display pid results
+		Serial.printf("radio: %d\t err: %d\t pid:%d\n", radio_in, error, control);
+		delay(50);
+	}
+}

libraries/AC_PID/examples/AC_PID_test/Makefile

@@ -0,0 +1,2 @@
+BOARD	=	mega
+include ../../../AP_Common/Arduino.mk

libraries/APM_PerfMon/APM_PerfMon.cpp

@@ -1,201 +0,0 @@
-
-extern "C" {
-  // AVR LibC Includes
-  #include "WConstants.h"
-}
-
-#include "APM_PerfMon.h"
-
-// don't know why i need these
-// see http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&p=410870
-int __cxa_guard_acquire(__guard *g) {return !*(char *)(g);}; 
-void __cxa_guard_release (__guard *g) {*(char *)g = 1;}; 
-void __cxa_guard_abort (__guard *) {};
-
-// static class variable definitions
-int APM_PerfMon::nextFuncNum;
-char APM_PerfMon::functionNames[PERFMON_MAX_FUNCTIONS][PERFMON_FUNCTION_NAME_LENGTH];
-unsigned long APM_PerfMon::time[PERFMON_MAX_FUNCTIONS];
-unsigned long APM_PerfMon::numCalls[PERFMON_MAX_FUNCTIONS];
-unsigned long APM_PerfMon::allStartTime;
-unsigned long APM_PerfMon::allEndTime;
-APM_PerfMon* APM_PerfMon::lastCreated = NULL;
-
-// constructor
-APM_PerfMon::APM_PerfMon(int funcNum)
-{
-    // stop recording time from parent
-	if( lastCreated != NULL )
-	    lastCreated->stop();
-
-    // check global start time
-	if( allStartTime == 0 )
-	    allStartTime = micros();
-		
-    _funcNum = funcNum;		// record which function we should assign time to
-	_parent = lastCreated;  // add pointer to parent
-	lastCreated = this;     // record myself as the last created instance
-	
-	numCalls[_funcNum]++;   // record that this function has been called
-	start();  				// start recording time spent in this function
-}
-
-// destructor
-APM_PerfMon::~APM_PerfMon()
-{
-    stop();   // stop recording time spent in this function
-	lastCreated = _parent;  // make my parent the last created instance
-	
-	// restart recording time for parent
-	if( lastCreated != NULL )
-	    lastCreated->start();
-}
-
-// stop recording time
-void APM_PerfMon::stop()
-{
-    time[_funcNum] += (micros()-_startTime);   
-}
-
-// stop recording time
-void APM_PerfMon::start()
-{
-    _startTime = micros();  // restart recording time spent in this function  
-}
-
-// record function name in static list
-int APM_PerfMon::recordFunctionName(const char funcName[])
-{
-    int nextNum = nextFuncNum++;
-    int i;
-    
-    // clear existing function name (if any)
-    functionNames[nextNum][0] = 0;
-    
-    // store function name
-    for( i=0; i<PERFMON_FUNCTION_NAME_LENGTH-1 && funcName[i] != 0; i++ )
-    {
-        functionNames[nextNum][i] = funcName[i];      
-    }
-    functionNames[nextNum][i] = 0;
-    
-    return nextNum;
-}
-
-// ClearAll - clears all data from static members
-void APM_PerfMon::ClearAll()
-{
-    int i;
-	APM_PerfMon *p = lastCreated;
-	
-	for(i=0; i<PERFMON_MAX_FUNCTIONS; i++)
-	{
-		time[i] = 0;  // reset times
-		numCalls[i] = 0;  // reset num times called
-	}
-
-	// reset start time to now
-	allStartTime = micros();	
-	allEndTime = 0;
-	
-	// reset start times of any active counters
-	while( p != NULL ) {
-	    p->_startTime = allStartTime;
-		p = p->_parent;
-	}
-}
-
-// ClearAll - clears all data from static members
-void APM_PerfMon::DisplayResults(HardwareSerial* aSerial)
-{
-    int i,j,k,padding,changed;
-	float hz;
-	float pct;
-	unsigned long totalTime;
-	unsigned long sumOfTime = 0;
-	unsigned long unExplainedTime;
-	int order[PERFMON_MAX_FUNCTIONS];
-	APM_PerfMon* p = lastCreated;
-	
-	// record end time
-	if( allEndTime == 0 )
-	    allEndTime = micros();
-		
-    // turn off any time recording
-	if( lastCreated != NULL )
-	    lastCreated->stop();
-	
-	// reorder results
-	for(i=0; i<nextFuncNum; i++)
-	    order[i] = i;
-	changed=0;
-	do{
-	    changed = 0;
-	    for(i=0; i<nextFuncNum-1; i++)
-		    if(time[order[i]]<time[order[i+1]])
-			{
-			    j = order[i];
-				order[i] = order[i+1];
-				order[i+1] = j;
-				changed = 1;
-			}
-	}while(changed != 0);
-	// sort results
-	
-	totalTime = allEndTime - allStartTime;
-	
-	aSerial->print("PerfMon start:");
-	aSerial->print(allStartTime/1000);
-	aSerial->print(" (mils)   end:");
-	aSerial->print(allEndTime/1000);
-	aSerial->print(" (mils)  elapsed:");
-	aSerial->print(totalTime/1000);
-	aSerial->print(" (mils)");
-	aSerial->println();
-	aSerial->println("PerfMon:           \tcpu%\tmils\t#called\tHz");
-	for( i=0; i<nextFuncNum; i++ )
-	{
-	    j=order[i];
-	    sumOfTime += time[j];
-	    hz = numCalls[j]/(totalTime/1000000);
-		pct = ((float)time[j] / (float)totalTime) * 100.0;
-		padding = PERFMON_FUNCTION_NAME_LENGTH - strLen(functionNames[j]);
-		
-		aSerial->print(functionNames[j]);
-		for(k=0;k<padding;k++)
-		    aSerial->print(" ");
-		aSerial->print("\t");
-		aSerial->print(pct);
-		aSerial->print("%\t");
-		aSerial->print(time[j]/1000);
-		aSerial->print("\t");
-		aSerial->print(numCalls[j]);
-		aSerial->print("\t");		
-		aSerial->print(hz,0);
-		aSerial->print("hz");
-		aSerial->println();
-	}
-	// display unexplained time
-	if( sumOfTime >= totalTime )
-	    unExplainedTime = 0;
-	else
-	    unExplainedTime = totalTime - sumOfTime;
-	pct = ((float)unExplainedTime / (float)totalTime) * 100.0;
-	aSerial->print("unexplained        \t");
-	aSerial->print(pct);
-	aSerial->print("%\t");
-	aSerial->print(unExplainedTime/1000);
-	aSerial->println();
-	
-	// turn back on any time recording
-	if( lastCreated != NULL )
-	    lastCreated->start();
-}
-
-int APM_PerfMon::strLen(char* str)
-{
-    int i = 0;
-    while( str[i] != 0 )
-	    i++;
-    return i;
-}

libraries/APM_PerfMon/APM_PerfMon.h

@@ -1,49 +0,0 @@
-#ifndef APM_PerfMon_h
-#define APM_PerfMon_h
-
-// macros to make integrating into code easier
-#define APM_PERFMON_REGISTER static int myFunc = APM_PerfMon::recordFunctionName(__func__); APM_PerfMon perfMon(myFunc);
-#define APM_PERFMON_REGISTER_NAME(functionName) static int myFunc = APM_PerfMon::recordFunctionName(functionName); APM_PerfMon perfMon(myFunc);
-
-#define PERFMON_MAX_FUNCTIONS 50
-#define PERFMON_FUNCTION_NAME_LENGTH 20
-
-__extension__ typedef int __guard __attribute__((mode (__DI__))); 
-
-extern "C" int __cxa_guard_acquire(__guard *); 
-extern "C" void __cxa_guard_release (__guard *); 
-extern "C" void __cxa_guard_abort (__guard *);
-
-#include "HardwareSerial.h"
-
-class APM_PerfMon
-{
-    public:
-        // static variables
-        static int nextFuncNum;
-		static char functionNames[PERFMON_MAX_FUNCTIONS][PERFMON_FUNCTION_NAME_LENGTH];
-        static unsigned long time[PERFMON_MAX_FUNCTIONS];
-		static unsigned long numCalls[PERFMON_MAX_FUNCTIONS];
-		static unsigned long allStartTime;
-		static unsigned long allEndTime;
-		static APM_PerfMon* lastCreated;
-
-		// static methods
-        static int recordFunctionName(const char funcName[]);
-        static void DisplayResults(HardwareSerial* aSerial);		
-		static void ClearAll();
-		static int strLen(char* str);
-		
-        // normal variables
-        int _funcNum;
-        unsigned long _startTime;
-		APM_PerfMon* _parent;
-
-		// normal methods
-		APM_PerfMon(int funcNum);  // Constructor - records function start time
-		~APM_PerfMon();  // Destructor - records function end time
-		void stop();  // stops recording time spent in this function - meant to be called by a child.
-		void start();  // restarts recording time spent in this function
-};
-
-#endif  // APM_PerfMon_h

libraries/APM_PerfMon/APM_PerfMon_test/APM_PerfMon_test.pde

@@ -1,45 +0,0 @@
-/*
-  APM_PerfMon
-  Code by Randy Mackay
-*/
-
-#include "APM_PerfMon.h"        // PerfMonitor library
-
-void setup()
-{
-  APM_PERFMON_REGISTER_NAME("setupA")
-  Serial.begin(115200);
-  Serial.println("Performance Monitor test v1.0");  
-}
-
-void loop()
-{
-    APM_PERFMON_REGISTER
-    
-    int i = 0;
-    
-    for( i=0; i<100; i++ )
-    {
-        testFunction();
-    }
-    
-    APM_PerfMon::DisplayResults(&Serial);
- 
-    delay(10000); 
-    
-    APM_PerfMon::ClearAll();
-}
-
-void testFunction()
-{
-    APM_PERFMON_REGISTER
-    delay(10);
-    testFunction2();
-    delay(10);
-}
-
-void testFunction2()
-{
-    APM_PERFMON_REGISTER
-    delay(10);
-}