diff --git a/Tools/ArdupilotMegaPlanner/CurrentState.cs b/Tools/ArdupilotMegaPlanner/CurrentState.cs index 2ac2a6abe6..ef5b08abc9 100644 --- a/Tools/ArdupilotMegaPlanner/CurrentState.cs +++ b/Tools/ArdupilotMegaPlanner/CurrentState.cs @@ -77,6 +77,8 @@ namespace ArdupilotMega public float my { get; set; } public float mz { get; set; } + public float magfield { get { return (float)Math.Sqrt(Math.Pow(mx, 2) + Math.Pow(my, 2) + Math.Pow(mz, 2)); } } + // calced turn rate public float turnrate { get { if (groundspeed <= 1) return 0; return (roll * 9.8f) / groundspeed; } } diff --git a/Tools/ArdupilotMegaPlanner/MAVLink.cs b/Tools/ArdupilotMegaPlanner/MAVLink.cs index a659e0b912..4eebf9b91b 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLink.cs @@ -327,7 +327,7 @@ namespace ArdupilotMega byte[] buffer = readPacket(); if (buffer.Length > 5) { - log.Info("getHB packet received: " + buffer.Length + " btr " + BaseStream.BytesToRead + " type " + buffer[5] ); + //log.Info("getHB packet received: " + buffer.Length + " btr " + BaseStream.BytesToRead + " type " + buffer[5] ); if (buffer[5] == MAVLINK_MSG_ID_HEARTBEAT) { return buffer; @@ -618,7 +618,7 @@ namespace ArdupilotMega DateTime start = DateTime.Now; DateTime restart = DateTime.Now; - while (got.Count < param_total) + do { if (frmProgressReporter.doWorkArgs.CancelRequested) @@ -629,7 +629,8 @@ namespace ArdupilotMega return param; } - if (!(start.AddMilliseconds(5000) > DateTime.Now)) + // 2 seconds between valid packets + if (!(start.AddMilliseconds(2000) > DateTime.Now)) { if (retrys > 0) { @@ -640,7 +641,7 @@ namespace ArdupilotMega continue; } MainV2.givecomport = false; - throw new Exception("Timeout on read - getParamList " + param_count +" "+ param_total); + throw new Exception("Timeout on read - getParamList " + param_count + " " + param_total); } byte[] buffer = readPacket(); @@ -656,9 +657,9 @@ namespace ArdupilotMega __mavlink_param_value_t par = buffer.ByteArrayToStructure<__mavlink_param_value_t>(6); // set new target - param_total = (par.param_count); + param_total = (par.param_count - 1); + - string paramID = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id); int pos = paramID.IndexOf('\0'); @@ -681,9 +682,13 @@ namespace ArdupilotMega param_count++; got.Add(par.param_index); -// if (Progress != null) -// Progress((param.Count * 100) / param_total, "Got param " + paramID); + // if (Progress != null) + // Progress((param.Count * 100) / param_total, "Got param " + paramID); this.frmProgressReporter.UpdateProgressAndStatus((got.Count * 100) / param_total, "Got param " + paramID); + + // we have them all - lets escape eq total = 176 index = 0-175 + if (par.param_index == (param_total - 1)) + break; } else { @@ -692,7 +697,7 @@ namespace ArdupilotMega //stopwatch.Stop(); //Console.WriteLine("Time elapsed: {0}", stopwatch.Elapsed); } - } + } while (got.Count < param_total); if (got.Count != param_total) { diff --git a/Tools/ArdupilotMegaPlanner/MagCalib.cs b/Tools/ArdupilotMegaPlanner/MagCalib.cs index ca36613cfd..e205c7b7ba 100644 --- a/Tools/ArdupilotMegaPlanner/MagCalib.cs +++ b/Tools/ArdupilotMegaPlanner/MagCalib.cs @@ -38,10 +38,13 @@ namespace ArdupilotMega if (openFileDialog1.ShowDialog() == DialogResult.OK) { + try + { + double[] ans = getOffsets(openFileDialog1.FileName); - double[] ans = getOffsets(openFileDialog1.FileName); - - SaveOffsets(ans); + SaveOffsets(ans); + } + catch (Exception ex) { log.Debug(ex.ToString()); } } } @@ -153,6 +156,12 @@ namespace ArdupilotMega mine.logplaybackfile.Close(); mine.logplaybackfile = null; + if (data.Count < 10) + { + CustomMessageBox.Show("Log does not contain enough data"); + throw new Exception("Not Enough Data"); + } + double[] x = LeastSq(data); System.Console.WriteLine("Old Method {0} {1} {2}", -(maxx + minx) / 2, -(maxy + miny) / 2, -(maxz + minz) / 2); diff --git a/Tools/ArdupilotMegaPlanner/MavlinkLog.cs b/Tools/ArdupilotMegaPlanner/MavlinkLog.cs index 4b01b971ff..a870ddc764 100644 --- a/Tools/ArdupilotMegaPlanner/MavlinkLog.cs +++ b/Tools/ArdupilotMegaPlanner/MavlinkLog.cs @@ -366,7 +366,11 @@ namespace ArdupilotMega { MAVLink mine = new MAVLink(); - mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)); + try + { + mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)); + } + catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; } mine.logreadmode = true; mine.packets.Initialize(); // clear diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index 58eeae5426..33c9b20f74 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.51")] +[assembly: AssemblyFileVersion("1.1.52")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb index 4df910db41..a8ce858821 100644 Binary files a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb and b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb differ