From 69d9c1583509c8d019c04f74e5e8ed7c97d3dc03 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Fri, 23 Sep 2011 07:33:25 +0800 Subject: [PATCH] APM Planner 1.0.75 add heli hil (beta) suggested sim gains 5000,5000,500,1000 fix simple mode error add AZ to measure distance --- Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs | 21 +++-- Tools/ArdupilotMegaPlanner/CurrentState.cs | 10 ++- .../GCSViews/FlightPlanner.cs | 6 +- .../GCSViews/Simulation.Designer.cs | 9 ++ .../GCSViews/Simulation.cs | 70 +++++++++------ .../GCSViews/Simulation.resx | 88 +++++++++++++------ Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs | 4 +- Tools/ArdupilotMegaPlanner/MainV2.cs | 7 +- .../Properties/AssemblyInfo.cs | 2 +- Tools/ArdupilotMegaPlanner/Setup/Setup.cs | 3 +- .../bin/Release/GCSViews/Simulation.resx | 88 +++++++++++++------ 11 files changed, 205 insertions(+), 103 deletions(-) diff --git a/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs b/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs index a10f2f7489..fac3ab0cdd 100644 --- a/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs +++ b/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs @@ -79,17 +79,16 @@ namespace System.IO.Ports client = new UdpClient(int.Parse(Port)); - int timeout = 5; - while (timeout > 0) - { - if (BytesToRead > 0) - break; - System.Threading.Thread.Sleep(1000); - timeout--; - } - - if (BytesToRead == 0) - return; + int timeout = 5; + while (timeout > 0) + { + if (BytesToRead > 0) + break; + System.Threading.Thread.Sleep(1000); + timeout--; + } + if (BytesToRead == 0) + return; try { diff --git a/Tools/ArdupilotMegaPlanner/CurrentState.cs b/Tools/ArdupilotMegaPlanner/CurrentState.cs index 303e0b969f..4bba5e8269 100644 --- a/Tools/ArdupilotMegaPlanner/CurrentState.cs +++ b/Tools/ArdupilotMegaPlanner/CurrentState.cs @@ -136,7 +136,11 @@ namespace ArdupilotMega public int hilch1 { get; set; } public int hilch2 { get; set; } public int hilch3 { get; set; } - public int hilch4 { get; set; } + public int hilch4 { get; set; } + public int hilch5; + public int hilch6; + public int hilch7; + public int hilch8; // rc override public ushort rcoverridech1 { get; set; } @@ -230,6 +234,10 @@ namespace ArdupilotMega hilch2 = hil.chan2_scaled; hilch3 = hil.chan3_scaled; hilch4 = hil.chan4_scaled; + hilch5 = hil.chan5_scaled; + hilch6 = hil.chan6_scaled; + hilch7 = hil.chan7_scaled; + hilch8 = hil.chan8_scaled; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] = null; } diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs index 496882201c..2aa13247c7 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs @@ -622,7 +622,11 @@ namespace ArdupilotMega.GCSViews case MAVLink.MAV_CMD.LAND: Commands.Columns[1].HeaderText = "N/A"; if (MainV2.APMFirmware != MainV2.Firmwares.ArduPlane) + { Commands.Columns[2].HeaderText = "N/A"; + Commands.Columns[3].HeaderText = "N/A"; + Commands.Columns[4].HeaderText = "N/A"; + } break; case MAVLink.MAV_CMD.TAKEOFF: if (MainV2.APMFirmware != MainV2.Firmwares.ArduPlane) @@ -2430,7 +2434,7 @@ namespace ArdupilotMega.GCSViews polygons.Markers.Add(new GMapMarkerGoogleRed(start)); MainMap.Invalidate(); - MessageBox.Show("Distance: " + FormatDistance(MainMap.Manager.GetDistance(startmeasure, start), true)); + MessageBox.Show("Distance: " + FormatDistance(MainMap.Manager.GetDistance(startmeasure, start),true) + " AZ: " + (MainMap.Manager.GetBearing(startmeasure, start).ToString("0"))); polygons.Polygons.Remove(line); polygons.Markers.Clear(); startmeasure = new PointLatLng(); diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.Designer.cs index 17ffbb85a9..997088d9e7 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.Designer.cs @@ -109,6 +109,7 @@ this.BUT_startfgquad = new ArdupilotMega.MyButton(); this.BUT_startfgplane = new ArdupilotMega.MyButton(); this.BUT_startxplane = new ArdupilotMega.MyButton(); + this.CHK_heli = new System.Windows.Forms.CheckBox(); ((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).BeginInit(); this.panel1.SuspendLayout(); this.panel2.SuspendLayout(); @@ -673,10 +674,17 @@ this.BUT_startxplane.UseVisualStyleBackColor = true; this.BUT_startxplane.Click += new System.EventHandler(this.BUT_startxplane_Click); // + // CHK_heli + // + resources.ApplyResources(this.CHK_heli, "CHK_heli"); + this.CHK_heli.Name = "CHK_heli"; + this.CHK_heli.UseVisualStyleBackColor = true; + // // Simulation // resources.ApplyResources(this, "$this"); this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; + this.Controls.Add(this.CHK_heli); this.Controls.Add(this.BUT_startxplane); this.Controls.Add(this.BUT_startfgplane); this.Controls.Add(this.BUT_startfgquad); @@ -801,5 +809,6 @@ private MyButton BUT_startfgquad; private MyButton BUT_startfgplane; private MyButton BUT_startxplane; + private System.Windows.Forms.CheckBox CHK_heli; } } diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs index bfd1cd91a0..2c0469d8ef 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs @@ -124,6 +124,8 @@ namespace ArdupilotMega.GCSViews try { + quad = new HIL.QuadCopter(); + SetupUDPRecv(); if (RAD_softXplanes.Checked) @@ -149,6 +151,7 @@ namespace ArdupilotMega.GCSViews }; t11.Start(); MainV2.threads.Add(t11); + timer1.Start(); } else { @@ -618,6 +621,9 @@ namespace ArdupilotMega.GCSViews att.pitch = (DATA[18][0] * deg2rad); att.roll = (DATA[18][1] * deg2rad); att.yaw = (DATA[18][2] * deg2rad); + att.pitchspeed = (DATA[17][0]); + att.rollspeed = (DATA[17][1]); + att.yawspeed = (DATA[17][2]); TimeSpan timediff = DateTime.Now - oldtime; @@ -679,7 +685,7 @@ namespace ArdupilotMega.GCSViews object imudata = new fgIMUData(); - MAVLink.ByteArrayToStructure(data, ref imudata, 0); + MAVLink.ByteArrayToStructureEndian(data, ref imudata, 0); imudata = (fgIMUData)(imudata); @@ -725,7 +731,7 @@ namespace ArdupilotMega.GCSViews object temp = fdm; - MAVLink.ByteArrayToStructure(data, ref temp, 0); + MAVLink.ByteArrayToStructureEndian(data, ref temp, 0); fdm = (FGNetFDM)(temp); @@ -904,12 +910,9 @@ namespace ArdupilotMega.GCSViews private void processArduPilot() { - // Console.WriteLine("sim "+DateTime.Now.Millisecond); + bool heli = CHK_heli.Checked; - float roll_out = (float)MainV2.cs.hilch1 / rollgain; - float pitch_out = (float)MainV2.cs.hilch2 / pitchgain; - float throttle_out = ((float)MainV2.cs.hilch3 + 5000) / throttlegain; - float rudder_out = (float)MainV2.cs.hilch4 / ruddergain; + // Console.WriteLine("sim "+DateTime.Now.Millisecond); if (CHK_quad.Checked) { @@ -931,6 +934,9 @@ namespace ArdupilotMega.GCSViews try { + if (lastfdmdata.version == 0) + return; + quad.update(ref m, lastfdmdata); } catch (Exception e) { Console.WriteLine("Quad hill error " + e.ToString()); } @@ -1010,12 +1016,37 @@ namespace ArdupilotMega.GCSViews } + float roll_out, pitch_out, throttle_out, rudder_out, collective_out; + + collective_out = 0; + + if (heli) + { + roll_out = (float)MainV2.cs.hilch1 / rollgain; + pitch_out = (float)MainV2.cs.hilch2 / pitchgain; + throttle_out = 1; + rudder_out = (float)MainV2.cs.hilch4 / -ruddergain; + + collective_out = (float)(MainV2.cs.hilch3 - 1000) / throttlegain; + } + else + { + + roll_out = (float)MainV2.cs.hilch1 / rollgain; + pitch_out = (float)MainV2.cs.hilch2 / pitchgain; + throttle_out = ((float)MainV2.cs.hilch3 + 5000) / throttlegain; + rudder_out = (float)MainV2.cs.hilch4 / ruddergain; + } if ((roll_out == -1 || roll_out == 1) && (pitch_out == -1 || pitch_out == 1)) { this.Invoke((MethodInvoker)delegate { - //OutputLog.AppendText("Please check your radio setup - CLI -> setup -> radio!!!\n"); + try + { + OutputLog.AppendText("Please check your radio setup - CLI -> setup -> radio!!!\n"); + } + catch { } }); } @@ -1158,15 +1189,9 @@ namespace ArdupilotMega.GCSViews { // sending only 1 packet instead of many. - bool helicopter = false; - -#if !DEBUG - helicopter = false; -#endif - byte[] Xplane = new byte[5 + 36 + 36]; - if (helicopter) + if (heli) { Xplane = new byte[5 + 36 + 36 + 36]; } @@ -1184,14 +1209,6 @@ namespace ArdupilotMega.GCSViews Array.Copy(BitConverter.GetBytes((float)throttle_out), 0, Xplane, 17, 4); Array.Copy(BitConverter.GetBytes((float)throttle_out), 0, Xplane, 21, 4); - if (helicopter) - { - Array.Copy(BitConverter.GetBytes((float)1), 0, Xplane, 9, 4); // start data - Array.Copy(BitConverter.GetBytes((float)1), 0, Xplane, 13, 4); - Array.Copy(BitConverter.GetBytes((float)1), 0, Xplane, 17, 4); - Array.Copy(BitConverter.GetBytes((float)1), 0, Xplane, 21, 4); - } - Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 25, 4); Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 29, 4); Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 33, 4); @@ -1211,12 +1228,15 @@ namespace ArdupilotMega.GCSViews Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 69, 4); Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 73, 4); - if (helicopter) + if (heli) { + Array.Copy(BitConverter.GetBytes((float)(0)), 0, Xplane, 53, 4); + + int a = 73 + 4; Array.Copy(BitConverter.GetBytes((int)39), 0, Xplane, a, 4); // packet index a += 4; - Array.Copy(BitConverter.GetBytes((float)(12 * throttle_out - 2)), 0, Xplane, a, 4); // main rotor 0 - 12 + Array.Copy(BitConverter.GetBytes((float)(12 * collective_out)), 0, Xplane, a, 4); // main rotor 0 - 12 a += 4; Array.Copy(BitConverter.GetBytes((float)(12 * rudder_out)), 0, Xplane, a, 4); // tail rotor -12 - 12 a += 4; diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.resx b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.resx index 931ace516c..6bd2f521c2 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.resx @@ -144,7 +144,7 @@ $this - 27 + 28 True @@ -171,7 +171,7 @@ $this - 26 + 27 True @@ -198,7 +198,7 @@ $this - 25 + 26 100 @@ -243,7 +243,7 @@ $this - 24 + 25 26, 13 @@ -294,7 +294,7 @@ $this - 23 + 24 67, 22 @@ -510,7 +510,7 @@ $this - 22 + 23 True @@ -537,7 +537,7 @@ $this - 21 + 22 True @@ -564,7 +564,7 @@ $this - 20 + 21 67, 24 @@ -765,7 +765,7 @@ $this - 19 + 20 7, 100 @@ -927,7 +927,7 @@ $this - 18 + 19 7, 27 @@ -1140,7 +1140,7 @@ $this - 17 + 18 72, 104 @@ -1275,7 +1275,7 @@ $this - 16 + 17 535, 9 @@ -1299,7 +1299,7 @@ $this - 15 + 16 13, 5 @@ -1320,7 +1320,7 @@ $this - 14 + 15 @@ -1345,7 +1345,7 @@ $this - 13 + 14 17, 17 @@ -1657,7 +1657,7 @@ $this - 12 + 13 508, 330 @@ -1681,7 +1681,7 @@ $this - 11 + 12 True @@ -1708,7 +1708,7 @@ $this - 10 + 11 Bottom, Left @@ -1738,7 +1738,7 @@ $this - 9 + 10 Bottom, Left @@ -1768,7 +1768,7 @@ $this - 8 + 9 Bottom, Left @@ -1798,7 +1798,7 @@ $this - 7 + 8 Bottom, Left @@ -1828,7 +1828,7 @@ $this - 6 + 7 566, 368 @@ -1852,13 +1852,13 @@ $this - 5 + 6 True - 567, 324 + 566, 317 59, 17 @@ -1879,7 +1879,7 @@ $this - 4 + 5 True @@ -1888,7 +1888,7 @@ NoControl - 567, 346 + 566, 334 52, 17 @@ -1909,7 +1909,7 @@ $this - 3 + 4 NoControl @@ -1936,7 +1936,7 @@ $this - 2 + 3 NoControl @@ -1963,7 +1963,7 @@ $this - 1 + 2 NoControl @@ -1990,6 +1990,36 @@ $this + 1 + + + True + + + NoControl + + + 566, 350 + + + 44, 17 + + + 47 + + + Heli + + + CHK_heli + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + $this + + 0 diff --git a/Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs b/Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs index 68afd0d354..a780cb4a8a 100644 --- a/Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs +++ b/Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs @@ -159,7 +159,7 @@ namespace ArdupilotMega.HIL //Console.WriteLine("Z {0} halt {1} < gl {2} fh {3}" ,position.Z , home_altitude , ground_level , frame_height); - if (home_latitude == 0) + if (home_latitude == 0 || home_latitude > 90 || home_latitude < -90 || home_longitude == 0) { this.home_latitude = fdm.latitude * rad2deg; this.home_longitude = fdm.longitude * rad2deg; @@ -187,7 +187,7 @@ namespace ArdupilotMega.HIL position = new Vector3d(position.X, position.Y, ground_level + frame_height - home_altitude + 0); // Console.WriteLine("here " + position.Z); - } + } //# update lat/lon/altitude update_position(); diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs index 204a6158ba..cac65747ea 100644 --- a/Tools/ArdupilotMegaPlanner/MainV2.cs +++ b/Tools/ArdupilotMegaPlanner/MainV2.cs @@ -222,12 +222,12 @@ namespace ArdupilotMega private void CMB_serialport_Click(object sender, EventArgs e) { - String old_port = CMB_serialport.Text; + string oldport = CMB_serialport.Text; CMB_serialport.Items.Clear(); CMB_serialport.Items.AddRange(SerialPort.GetPortNames()); CMB_serialport.Items.Add("UDP"); - if (CMB_serialport.Items.Contains(old_port)) - CMB_serialport.Text = old_port; + if (CMB_serialport.Items.Contains(oldport)) + CMB_serialport.Text = oldport; } public static void fixtheme(Control temp) @@ -679,6 +679,7 @@ namespace ArdupilotMega CMB_baudrate.Enabled = false; else CMB_baudrate.Enabled = true; + try { comPort.BaseStream.PortName = CMB_serialport.Text; diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index 639de564f4..a3971a4c46 100644 --- a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs +++ b/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.0.74")] +[assembly: AssemblyFileVersion("1.0.75")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs index 8d52363560..0afc7b9a75 100644 --- a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs +++ b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs @@ -456,7 +456,8 @@ namespace ArdupilotMega.Setup float value = (float)(CB_simple1.Checked ? 1 : 0) + (CB_simple2.Checked ? 1 << 1 : 0) + (CB_simple3.Checked ? 1 <<2 : 0) + (CB_simple4.Checked ? 1 << 3 : 0) + (CB_simple5.Checked ? 1 << 4 : 0) + (CB_simple6.Checked ? 1 << 5 : 0); - MainV2.comPort.setParam("SIMPLE", value); + if (MainV2.comPort.param.ContainsKey("SIMPLE")) + MainV2.comPort.setParam("SIMPLE", value); } } catch { MessageBox.Show("Failed to set Flight modes"); } diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Simulation.resx b/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Simulation.resx index 931ace516c..6bd2f521c2 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Simulation.resx +++ b/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Simulation.resx @@ -144,7 +144,7 @@ $this - 27 + 28 True @@ -171,7 +171,7 @@ $this - 26 + 27 True @@ -198,7 +198,7 @@ $this - 25 + 26 100 @@ -243,7 +243,7 @@ $this - 24 + 25 26, 13 @@ -294,7 +294,7 @@ $this - 23 + 24 67, 22 @@ -510,7 +510,7 @@ $this - 22 + 23 True @@ -537,7 +537,7 @@ $this - 21 + 22 True @@ -564,7 +564,7 @@ $this - 20 + 21 67, 24 @@ -765,7 +765,7 @@ $this - 19 + 20 7, 100 @@ -927,7 +927,7 @@ $this - 18 + 19 7, 27 @@ -1140,7 +1140,7 @@ $this - 17 + 18 72, 104 @@ -1275,7 +1275,7 @@ $this - 16 + 17 535, 9 @@ -1299,7 +1299,7 @@ $this - 15 + 16 13, 5 @@ -1320,7 +1320,7 @@ $this - 14 + 15 @@ -1345,7 +1345,7 @@ $this - 13 + 14 17, 17 @@ -1657,7 +1657,7 @@ $this - 12 + 13 508, 330 @@ -1681,7 +1681,7 @@ $this - 11 + 12 True @@ -1708,7 +1708,7 @@ $this - 10 + 11 Bottom, Left @@ -1738,7 +1738,7 @@ $this - 9 + 10 Bottom, Left @@ -1768,7 +1768,7 @@ $this - 8 + 9 Bottom, Left @@ -1798,7 +1798,7 @@ $this - 7 + 8 Bottom, Left @@ -1828,7 +1828,7 @@ $this - 6 + 7 566, 368 @@ -1852,13 +1852,13 @@ $this - 5 + 6 True - 567, 324 + 566, 317 59, 17 @@ -1879,7 +1879,7 @@ $this - 4 + 5 True @@ -1888,7 +1888,7 @@ NoControl - 567, 346 + 566, 334 52, 17 @@ -1909,7 +1909,7 @@ $this - 3 + 4 NoControl @@ -1936,7 +1936,7 @@ $this - 2 + 3 NoControl @@ -1963,7 +1963,7 @@ $this - 1 + 2 NoControl @@ -1990,6 +1990,36 @@ $this + 1 + + + True + + + NoControl + + + 566, 350 + + + 44, 17 + + + 47 + + + Heli + + + CHK_heli + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + $this + + 0