diff --git a/Tools/ArdupilotMegaPlanner/Common.cs b/Tools/ArdupilotMegaPlanner/Common.cs index b15ebb94ad..0267799be1 100644 --- a/Tools/ArdupilotMegaPlanner/Common.cs +++ b/Tools/ArdupilotMegaPlanner/Common.cs @@ -262,8 +262,9 @@ namespace ArdupilotMega LOITER = 5, // Hold a single location RTL = 6, // AUTO control CIRCLE = 7, - POSITION = 8 - } + POSITION = 8, + LAND = 9 // AUTO control + } public static void linearRegression() { diff --git a/Tools/ArdupilotMegaPlanner/CurrentState.cs b/Tools/ArdupilotMegaPlanner/CurrentState.cs index 411f9b828b..9ae1d69503 100644 --- a/Tools/ArdupilotMegaPlanner/CurrentState.cs +++ b/Tools/ArdupilotMegaPlanner/CurrentState.cs @@ -440,6 +440,9 @@ namespace ArdupilotMega case (byte)(100 + Common.ac2modes.CIRCLE): mode = "Circle"; break; + case (byte)(100 + Common.ac2modes.LAND): + mode = "Land"; + break; case (byte)(100 + Common.ac2modes.POSITION): mode = "Position"; break; diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.Designer.cs index 9f9231c654..34572cb0d8 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.Designer.cs @@ -113,6 +113,7 @@ this.RAD_aerosimrc = new System.Windows.Forms.RadioButton(); this.toolTip1 = new System.Windows.Forms.ToolTip(this.components); this.RAD_JSBSim = new System.Windows.Forms.RadioButton(); + this.CHK_xplane10 = new System.Windows.Forms.CheckBox(); ((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).BeginInit(); this.panel1.SuspendLayout(); this.panel2.SuspendLayout(); @@ -701,10 +702,17 @@ this.RAD_JSBSim.UseVisualStyleBackColor = true; this.RAD_JSBSim.CheckedChanged += new System.EventHandler(this.RAD_JSBSim_CheckedChanged); // + // CHK_xplane10 + // + resources.ApplyResources(this.CHK_xplane10, "CHK_xplane10"); + this.CHK_xplane10.Name = "CHK_xplane10"; + this.CHK_xplane10.UseVisualStyleBackColor = true; + // // Simulation // resources.ApplyResources(this, "$this"); this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; + this.Controls.Add(this.CHK_xplane10); this.Controls.Add(this.RAD_JSBSim); this.Controls.Add(this.RAD_aerosimrc); this.Controls.Add(this.CHK_heli); @@ -836,5 +844,6 @@ private System.Windows.Forms.RadioButton RAD_aerosimrc; private System.Windows.Forms.ToolTip toolTip1; private System.Windows.Forms.RadioButton RAD_JSBSim; + private System.Windows.Forms.CheckBox CHK_xplane10; } } diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs index 8843d16763..1b2ec29fb1 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs @@ -793,12 +793,24 @@ namespace ArdupilotMega.GCSViews count += 36; // 8 * float } - 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]); + bool xplane9 = !CHK_xplane10.Checked; + + if (xplane9) + { + 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]); + } else { + att.pitch = (DATA[17][0] * deg2rad); + att.roll = (DATA[17][1] * deg2rad); + att.yaw = (DATA[17][2] * deg2rad); + att.pitchspeed = (DATA[16][0]); + att.rollspeed = (DATA[16][1]); + att.yawspeed = (DATA[16][2]); + } TimeSpan timediff = DateTime.Now - oldtime; @@ -809,10 +821,19 @@ namespace ArdupilotMega.GCSViews //Console.WriteLine("{0:0.00000} {1:0.00000} {2:0.00000} \t {3:0.00000} {4:0.00000} {5:0.00000}", pdiff, rdiff, ydiff, DATA[17][0], DATA[17][1], DATA[17][2]); oldatt = att; + if (xplane9) + { + rdiff = DATA[17][1]; + pdiff = DATA[17][0]; + ydiff = DATA[17][2]; + } + else + { + rdiff = DATA[16][1]; + pdiff = DATA[16][0]; + ydiff = DATA[16][2]; - rdiff = DATA[17][1]; - pdiff = DATA[17][0]; - ydiff = DATA[17][2]; + } Int16 xgyro = Constrain(rdiff * 1000.0, Int16.MinValue, Int16.MaxValue); Int16 ygyro = Constrain(pdiff * 1000.0, Int16.MinValue, Int16.MaxValue); @@ -1434,7 +1455,15 @@ namespace ArdupilotMega.GCSViews { if (RAD_softXplanes.Checked) { - 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); + + bool xplane9 = !CHK_xplane10.Checked; + 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 { + + 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); + } } if (RAD_softFlightGear.Checked || RAD_JSBSim.Checked) @@ -1558,7 +1587,7 @@ namespace ArdupilotMega.GCSViews Array.Copy(BitConverter.GetBytes((float)(rudder_out * REV_rudder)), 0, Xplane, 53, 4); Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 57, 4); - Array.Copy(BitConverter.GetBytes((float)(roll_out * REV_roll * 5)), 0, Xplane, 61, 4); + Array.Copy(BitConverter.GetBytes((float)(roll_out * REV_roll * 0.5)), 0, Xplane, 61, 4); Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 65, 4); Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 69, 4); Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 73, 4); diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.resx b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.resx index 9d99df9038..64d8c8dd98 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.resx @@ -144,7 +144,7 @@ $this - 30 + 31 True @@ -171,7 +171,7 @@ $this - 29 + 30 True @@ -198,7 +198,7 @@ $this - 28 + 29 100 @@ -243,7 +243,7 @@ $this - 27 + 28 26, 13 @@ -294,7 +294,7 @@ $this - 26 + 27 67, 22 @@ -510,11 +510,8 @@ $this - 25 + 26 - - 301, 17 - True @@ -530,6 +527,9 @@ X-plane + + 301, 17 + Can Do Plane/Quad with plugin @@ -543,7 +543,7 @@ $this - 24 + 25 True @@ -573,7 +573,7 @@ $this - 23 + 24 67, 24 @@ -659,75 +659,6 @@ 8 - - label4 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel1 - - - 0 - - - label3 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel1 - - - 1 - - - label2 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel1 - - - 2 - - - label1 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel1 - - - 3 - - - 13, 66 - - - 178, 100 - - - 23 - - - panel1 - - - System.Windows.Forms.Panel, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - $this - - - 22 - 60, 3 @@ -824,98 +755,26 @@ 3 - - label30 + + 13, 66 - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + + 178, 100 - - panel2 + + 23 - - 0 + + panel1 - - TXT_yaw - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel2 - - - 1 - - - label11 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel2 - - - 2 - - - label7 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel2 - - - 3 - - - label6 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel2 - - - 4 - - - label5 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel2 - - - 5 - - - 12, 172 - - - 178, 116 - - - 24 - - - panel2 - - + System.Windows.Forms.Panel, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + $this - - 21 + + 23 7, 100 @@ -1058,6 +917,27 @@ 5 + + 12, 172 + + + 178, 116 + + + 24 + + + panel2 + + + System.Windows.Forms.Panel, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + $this + + + 22 + 7, 27 @@ -1130,87 +1010,6 @@ 5 - - label16 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel3 - - - 0 - - - label15 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel3 - - - 1 - - - label14 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel3 - - - 2 - - - label13 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel3 - - - 3 - - - label12 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel3 - - - 4 - - - 13, 294 - - - 178, 122 - - - 25 - - - panel3 - - - System.Windows.Forms.Panel, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - $this - - - 20 - 50, 8 @@ -1331,86 +1130,26 @@ 4 - - label20 + + 13, 294 - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel4 - - - 0 - - - label19 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel4 - - - 1 - - - TXT_control_mode - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel4 - - - 2 - - - TXT_WP - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel4 - - - 3 - - - label18 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel4 - - - 4 - - - 197, 294 - - + 178, 122 - - 26 + + 25 - - panel4 + + panel3 - + System.Windows.Forms.Panel, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + $this - - 19 + + 21 72, 104 @@ -1526,6 +1265,27 @@ 4 + + 197, 294 + + + 178, 122 + + + 26 + + + panel4 + + + System.Windows.Forms.Panel, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + $this + + + 20 + 535, 9 @@ -1548,7 +1308,7 @@ $this - 18 + 19 13, 5 @@ -1569,7 +1329,7 @@ $this - 17 + 18 @@ -1594,176 +1354,11 @@ $this - 16 + 17 17, 17 - - label28 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel6 - - - 0 - - - label29 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel6 - - - 1 - - - label27 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel6 - - - 2 - - - label25 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel6 - - - 3 - - - TXT_throttlegain - - - System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - panel6 - - - 4 - - - label24 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel6 - - - 5 - - - label23 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel6 - - - 6 - - - label22 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel6 - - - 7 - - - label21 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - panel6 - - - 8 - - - TXT_ruddergain - - - System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - panel6 - - - 9 - - - TXT_pitchgain - - - System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - panel6 - - - 10 - - - TXT_rollgain - - - System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - panel6 - - - 11 - - - 382, 294 - - - 178, 122 - - - 30 - - - panel6 - - - System.Windows.Forms.Panel, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - $this - - - 15 - 126, 76 @@ -2052,6 +1647,27 @@ 11 + + 382, 294 + + + 178, 122 + + + 30 + + + panel6 + + + System.Windows.Forms.Panel, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + $this + + + 16 + 508, 330 @@ -2074,7 +1690,7 @@ $this - 14 + 15 True @@ -2101,7 +1717,7 @@ $this - 13 + 14 Bottom, Left @@ -2131,7 +1747,7 @@ $this - 12 + 13 Bottom, Left @@ -2161,7 +1777,7 @@ $this - 11 + 12 Bottom, Left @@ -2191,7 +1807,7 @@ $this - 10 + 11 Bottom, Left @@ -2221,7 +1837,7 @@ $this - 9 + 10 566, 368 @@ -2245,7 +1861,7 @@ $this - 8 + 9 True @@ -2272,7 +1888,7 @@ $this - 7 + 8 True @@ -2302,7 +1918,7 @@ $this - 6 + 7 NoControl @@ -2329,7 +1945,7 @@ $this - 5 + 6 NoControl @@ -2356,7 +1972,7 @@ $this - 4 + 5 NoControl @@ -2383,7 +1999,7 @@ $this - 3 + 4 True @@ -2413,7 +2029,7 @@ $this - 2 + 3 True @@ -2446,11 +2062,8 @@ $this - 1 + 2 - - 301, 17 - True @@ -2482,6 +2095,36 @@ $this + 1 + + + True + + + NoControl + + + 566, 237 + + + 74, 17 + + + 50 + + + Xplane 10 + + + CHK_xplane10 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + $this + + 0 diff --git a/Tools/ArdupilotMegaPlanner/MAVLink.cs b/Tools/ArdupilotMegaPlanner/MAVLink.cs index 6df63053ae..07581957da 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLink.cs @@ -455,6 +455,7 @@ namespace ArdupilotMega Array.Reverse(datearray); logfile.Write(datearray, 0, datearray.Length); logfile.Write(packet, 0, i); + logfile.Flush(); } } diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs index 254148481a..8c82afdf5e 100644 --- a/Tools/ArdupilotMegaPlanner/MainV2.cs +++ b/Tools/ArdupilotMegaPlanner/MainV2.cs @@ -641,6 +641,14 @@ namespace ArdupilotMega { givecomport = false; + if (comPort.BaseStream.IsOpen && cs.groundspeed > 4) + { + if (DialogResult.No == MessageBox.Show("Your model is still moving are you sure you want to disconnect?", "Disconnect", MessageBoxButtons.YesNo)) + { + return; + } + } + if (comPort.BaseStream.IsOpen) { try diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index 8af3b41239..df18476503 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.1.19")] +[assembly: AssemblyFileVersion("1.1.20")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/libraries/Desktop/support/desktop.h b/libraries/Desktop/support/desktop.h index e373130e78..d3ac289e3d 100644 --- a/libraries/Desktop/support/desktop.h +++ b/libraries/Desktop/support/desktop.h @@ -20,11 +20,11 @@ void sitl_setup(void); int sitl_gps_pipe(void); ssize_t sitl_gps_read(int fd, void *buf, size_t count); void sitl_update_compass(float heading, float roll, float pitch, float yaw); -void sitl_update_gps(float latitude, float longitude, float altitude, - float speedN, float speedE, bool have_lock); +void sitl_update_gps(double latitude, double longitude, float altitude, + double speedN, double speedE, bool have_lock); void sitl_update_adc(float roll, float pitch, float yaw, - float rollRate, float pitchRate, float yawRate, - float xAccel, float yAccel, float zAccel, + double rollRate, double pitchRate, double yawRate, + double xAccel, double yAccel, double zAccel, float airspeed); void sitl_setup_adc(void); void sitl_update_barometer(float altitude); diff --git a/libraries/Desktop/support/sitl_adc.cpp b/libraries/Desktop/support/sitl_adc.cpp index 8ea7de885d..63e44c2111 100644 --- a/libraries/Desktop/support/sitl_adc.cpp +++ b/libraries/Desktop/support/sitl_adc.cpp @@ -40,10 +40,28 @@ static uint16_t airspeed_sensor(float airspeed) average rates to cope with slow update rates. inputs are in degrees + + phi - roll + theta - pitch + psi - true heading + alpha - angle of attack + beta - side slip + phidot - roll rate + thetadot - pitch rate + psidot - yaw rate + v_north - north velocity in local/body frame + v_east - east velocity in local/body frame + v_down - down velocity in local/body frame + A_X_pilot - X accel in body frame + A_Y_pilot - Y accel in body frame + A_Z_pilot - Z accel in body frame + + Note: doubles on high prec. stuff are preserved until the last moment + */ -void sitl_update_adc(float roll, float pitch, float yaw, - float rollRate, float pitchRate, float yawRate, - float xAccel, float yAccel, float zAccel, +void sitl_update_adc(float roll, float pitch, float yaw, // Relative to earth + double rollRate, double pitchRate,double yawRate, // Local to plane + double xAccel, double yAccel, double zAccel, // Local to plane float airspeed) { static const uint8_t sensor_map[6] = { 1, 2, 0, 4, 5, 6 }; @@ -55,9 +73,9 @@ void sitl_update_adc(float roll, float pitch, float yaw, const float _gyro_gain_y = ToRad(0.41); const float _gyro_gain_z = ToRad(0.41); const float _accel_scale = 9.80665 / 423.8; - float adc[7]; - float phi, theta, phiDot, thetaDot, psiDot; - float p, q, r; + double adc[7]; + double phi, theta, phiDot, thetaDot, psiDot; + double p, q, r; /* convert the angular velocities from earth frame to body frame. Thanks to James Goppert for the formula diff --git a/libraries/Desktop/support/sitl_gps.cpp b/libraries/Desktop/support/sitl_gps.cpp index b21dc1b1a9..11a26711f8 100644 --- a/libraries/Desktop/support/sitl_gps.cpp +++ b/libraries/Desktop/support/sitl_gps.cpp @@ -78,8 +78,8 @@ static void gps_send(uint8_t msgid, uint8_t *buf, uint16_t size) /* possibly send a new GPS UBLOX packet */ -void sitl_update_gps(float latitude, float longitude, float altitude, - float speedN, float speedE, bool have_lock) +void sitl_update_gps(double latitude, double longitude, float altitude, + double speedN, double speedE, bool have_lock) { struct ubx_nav_posllh { uint32_t time; // GPS msToW @@ -113,7 +113,7 @@ void sitl_update_gps(float latitude, float longitude, float altitude, const uint8_t MSG_POSLLH = 0x2; const uint8_t MSG_STATUS = 0x3; const uint8_t MSG_VELNED = 0x12; - float lon_scale; + double lon_scale; // 4Hz if (millis() - gps_state.last_update < 250) {