C# MAVLinkInterface.readPacket方法代码示例

        public static string tlogToCSV(string filepath)
        {
            CurrentState.SpeedUnit = "m/s";
            CurrentState.DistanceUnit = "m";
            MAVLinkInterface proto = new MAVLinkInterface();

               OpenFileDialog openFileDialog1 = new OpenFileDialog();

               string LogFilePath;
              openFileDialog1.FileName = filepath;

                    foreach (string logfile in openFileDialog1.FileNames)
                    {

                        using (MAVLinkInterface mine = new MAVLinkInterface())
                        {
                            try
                            {
                                mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
                            }
                            catch (Exception ex) { log.Debug(ex.ToString()); }
                            mine.logreadmode = true;

                            mine.MAV.packets.Initialize(); // clear

                            StreamWriter sw = new StreamWriter(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + Path.GetFileNameWithoutExtension(logfile) + ".csv");

                            while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
                            {

                                byte[] packet = mine.readPacket();
                                string text = "";
                                mine.DebugPacket(packet, ref text, true, ",");

                                sw.Write(mine.lastlogread.ToString("yyyy-MM-ddTHH:mm:ss.fff") + "," + text);
                            }

                            sw.Close();

                            mine.logreadmode = false;
                            mine.logplaybackfile.Close();
                            mine.logplaybackfile = null;
                            LogFilePath = (Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + (Path.GetFileNameWithoutExtension(logfile) + ".csv"));

                            return LogFilePath;
                        }
                    }

                    return null;
        }

        // Return List with all GPS Messages splitted in string arrays
        Dictionary<long, VehicleLocation> readGPSMsgInLog(string fn)
        {
            Dictionary<long, VehicleLocation> vehiclePositionList = new Dictionary<long,VehicleLocation>();

            // Telemetry Log
            if (fn.ToLower().EndsWith("tlog"))
            {
                MAVLinkInterface mine = new MAVLinkInterface();
                mine.logplaybackfile = new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read));
                mine.logreadmode = true;

                mine.MAV.packets.Initialize(); // clear

                CurrentState cs = new CurrentState();

                while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
                {
                    byte[] packet = mine.readPacket();

                    cs.datetime = mine.lastlogread;

                    cs.UpdateCurrentSettings(null, true, mine);

                    VehicleLocation location = new VehicleLocation();
                    location.Time = cs.datetime;
                    location.Lat = cs.lat;
                    location.Lon = cs.lng;
                    location.RelAlt = cs.alt;
                    location.AltAMSL = cs.altasl;

                    location.Roll = cs.roll;
                    location.Pitch = cs.pitch;
                    location.Yaw = cs.yaw;

                    vehiclePositionList[ToMilliseconds(location.Time)] = location;
                    // 4 5 7
                    Console.Write((mine.logplaybackfile.BaseStream.Position * 100 / mine.logplaybackfile.BaseStream.Length) + "    \r");
                }
                mine.logplaybackfile.Close();
            }
            // DataFlash Log
            else
            {
                StreamReader sr = new StreamReader(fn);

                // Will hold the last seen Attitude information in order to incorporate them into the GPS Info
                float currentYaw = 0f;
                float currentRoll = 0f;
                float currentPitch = 0f;

                while (!sr.EndOfStream)
                {
                    string line = sr.ReadLine();

                    // Look for GPS Messages. However GPS Messages do not have Roll, Pitch and Yaw
                    // So we have to look for one ATT message after having read a GPS one
                    if (line.ToLower().StartsWith("gps"))
                    {
                        VehicleLocation location = new VehicleLocation();

                        string[] gpsLineValues = line.Split(new char[] { ',', ':' });

                        try
                        {

                            location.Time = GetTimeFromGps(int.Parse(getValueFromStringArray(gpsLineValues, gpsweekpos), CultureInfo.InvariantCulture), int.Parse(getValueFromStringArray(gpsLineValues, timepos), CultureInfo.InvariantCulture));
                            location.Lat = double.Parse(getValueFromStringArray(gpsLineValues, latpos), CultureInfo.InvariantCulture);
                            location.Lon = double.Parse(getValueFromStringArray(gpsLineValues, lngpos), CultureInfo.InvariantCulture);
                            location.RelAlt = double.Parse(getValueFromStringArray(gpsLineValues, altpos), CultureInfo.InvariantCulture);
                            location.AltAMSL = double.Parse(getValueFromStringArray(gpsLineValues, altAMSLpos), CultureInfo.InvariantCulture);

                            location.Roll = currentRoll;
                            location.Pitch = currentPitch;
                            location.Yaw = currentYaw;



                            long millis = ToMilliseconds(location.Time);

                            //System.Diagnostics.Debug.WriteLine("GPS MSG - UTCMillis = " + millis  + "  GPS Week = " + getValueFromStringArray(gpsLineValues, gpsweekpos) + "  TimeMS = " + getValueFromStringArray(gpsLineValues, timepos));

                            if (!vehiclePositionList.ContainsKey(millis))
                                vehiclePositionList[millis] = location;
                        }
                        catch { Console.WriteLine("Bad GPS Line"); }
                    }
                    else if (line.ToLower().StartsWith("att"))
                    {
                        string[] attLineValues = line.Split(new char[] { ',', ':' });

                        currentRoll = float.Parse(getValueFromStringArray(attLineValues, rollATT), CultureInfo.InvariantCulture);
                        currentPitch = float.Parse(getValueFromStringArray(attLineValues, pitchATT), CultureInfo.InvariantCulture);
                        currentYaw = float.Parse(getValueFromStringArray(attLineValues, yawATT), CultureInfo.InvariantCulture);

                    }


                }

//.........这里部分代码省略.........

        void dolog()
        {
            flightdata.Clear();

            using (MAVLinkInterface mine = new MAVLinkInterface())
            {
                try
                {
                    mine.logplaybackfile =
                        new BinaryReader(File.Open(txt_tlog.Text, FileMode.Open, FileAccess.Read, FileShare.Read));
                }
                catch
                {
                    CustomMessageBox.Show("Log Can not be opened. Are you still connected?");
                    return;
                }
                mine.logreadmode = true;

                mine.MAV.packets.Initialize(); // clear

                mine.readPacket();

                startlogtime = mine.lastlogread;

                double oldlatlngsum = 0;

                int appui = 0;

                while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
                {
                    byte[] packet = mine.readPacket();

                    cs.datetime = mine.lastlogread;

                    cs.UpdateCurrentSettings(null, true, mine);

                    if (appui != DateTime.Now.Second)
                    {
                        // cant do entire app as mixes with flightdata timer
                        this.Refresh();
                        appui = DateTime.Now.Second;
                    }

                    try
                    {
                        if (MainV2.speechEngine != null)
                            MainV2.speechEngine.SpeakAsyncCancelAll();
                    }
                    catch
                    {
                    }
                        // ignore because of this Exception System.PlatformNotSupportedException: No voice installed on the system or none available with the current security setting.

                    // if ((float)(cs.lat + cs.lng + cs.alt) != oldlatlngsum
                    //     && cs.lat != 0 && cs.lng != 0)

                    DateTime nexttime = mine.lastlogread.AddMilliseconds(-(mine.lastlogread.Millisecond%100));

                    if (!flightdata.ContainsKey(nexttime))
                    {
                        Console.WriteLine(cs.lat + " " + cs.lng + " " + cs.alt + "   lah " +
                                          (float) (cs.lat + cs.lng + cs.alt) + "!=" + oldlatlngsum);
                        CurrentState cs2 = (CurrentState) cs.Clone();

                        try
                        {
                            flightdata.Add(nexttime, cs2);
                        }
                        catch
                        {
                        }

                        oldlatlngsum = (cs.lat + cs.lng + cs.alt);
                    }
                }

                mine.logreadmode = false;
                mine.logplaybackfile.Close();
                mine.logplaybackfile = null;
            }
        }

        private void start_Terminal(bool px4)
        {
            try
            {
                if (MainV2.comPort != null && MainV2.comPort.BaseStream != null && MainV2.comPort.BaseStream.IsOpen)
                    MainV2.comPort.BaseStream.Close();

                setcomport();

                if (px4)
                {
                    TXT_terminal.AppendText("Rebooting " + MainV2.comPortName + " at " + comPort.BaudRate + "\n");
                    // keep it local
                    using (MAVLinkInterface mine = new MAVLinkInterface())
                    {

                        mine.BaseStream.PortName = MainV2.comPortName;
                        mine.BaseStream.BaudRate = comPort.BaudRate;

                        mine.giveComport = true;
                        mine.BaseStream.Open();

                        // check if we are a mavlink stream
                        byte[] buffer = mine.readPacket();

                        if (buffer.Length > 0)
                        {
                            log.Info("got packet - sending reboot via mavlink");
                            TXT_terminal.AppendText("Via Mavlink\n");
                            mine.doReboot(false);
                            try
                            {
                                mine.BaseStream.Close();
                            }
                            catch { }

                        }
                        else
                        {
                            log.Info("no packet - sending reboot via console");
                            TXT_terminal.AppendText("Via Console\n");
                            try
                            {
                                mine.BaseStream.Write("reboot\r");
                                mine.BaseStream.Write("exit\rreboot\r");
                            }
                            catch { }
                            try
                            {
                                mine.BaseStream.Close();
                            }
                            catch { }
                        }
                    }

                    TXT_terminal.AppendText("Waiting for reboot\n");

                    // wait 7 seconds for px4 reboot
                    log.Info("waiting for reboot");
                    DateTime deadline = DateTime.Now.AddSeconds(9);
                    while (DateTime.Now < deadline)
                    {
                        System.Threading.Thread.Sleep(500);
                        Application.DoEvents();
                    }

                    int a = 0;
                    while (a < 5)
                    {
                        try
                        {
                            if (!comPort.IsOpen)
                                comPort.Open();
                        }
                        catch { }
                        System.Threading.Thread.Sleep(200);
                        a++;
                    }

                }
                else
                {
                    log.Info("About to open " + comPort.PortName);

                    comPort.Open();

                    log.Info("Toggle dtr");

                    comPort.toggleDTR();
                }

                try
                {
                    comPort.DiscardInBuffer();
                }
                catch { }

                Console.WriteLine("Terminal_Load run " + threadrun + " " + comPort.IsOpen);

                t11 = new System.Threading.Thread(delegate()
//.........这里部分代码省略.........

        private void start_Terminal(bool px4)
        {
            setcomport();

            try
            {
                if (MainV2.comPort != null && MainV2.comPort.BaseStream != null && MainV2.comPort.BaseStream.IsOpen)
                    MainV2.comPort.BaseStream.Close();

                if (comPort.IsOpen)
                {
                    Console.WriteLine("Terminal Start - Close Port");
                    threadrun = false;
                  //  if (DialogResult.Cancel == CustomMessageBox.Show("The port is open\n Continue?", "Continue", MessageBoxButtons.YesNo))
                    {
                      //  return;
                    }

                    comPort.Close();

                    // allow things to cleanup
                    System.Threading.Thread.Sleep(400);
                }

                comPort.ReadBufferSize = 1024 * 1024 * 4;

                comPort.PortName = MainV2.comPortName;

                // test moving baud rate line

                comPort.BaudRate = int.Parse(MainV2._connectionControl.CMB_baudrate.Text);

                if (px4)
                {
                    TXT_terminal.AppendText("Rebooting\n");
                    // keep it local
                    MAVLinkInterface mine = new MAVLinkInterface();

                    mine.BaseStream.PortName = MainV2.comPortName;
                    mine.BaseStream.BaudRate = comPort.BaudRate;

                    mine.giveComport = true;
                    mine.BaseStream.Open();

                    // check if we are a mavlink stream
                    byte[] buffer = mine.readPacket();

                    if (buffer.Length > 0)
                    {
                        log.Info("got packet - sending reboot via mavlink");
                        TXT_terminal.AppendText("Via Mavlink\n");
                        mine.doReboot(false);
                        try
                        {
                            mine.BaseStream.Close();
                        }
                        catch { }

                    }
                    else
                    {
                        log.Info("no packet - sending reboot via console");
                        TXT_terminal.AppendText("Via Console\n");
                        MainV2.comPort.BaseStream.Write("exit\rreboot\r");
                        try
                        {
                            MainV2.comPort.BaseStream.Close();
                        }
                        catch { }

                    }

                    TXT_terminal.AppendText("Waiting for reboot\n");

                    // wait 7 seconds for px4 reboot
                    log.Info("waiting for reboot");
                    DateTime deadline = DateTime.Now.AddSeconds(8);
                    while (DateTime.Now < deadline)
                    {
                        System.Threading.Thread.Sleep(500);
                        Application.DoEvents();
                    }

                    int a = 0;
                    while (a < 5)
                    {
                        try
                        {
                            if (!comPort.IsOpen)
                                comPort.Open();
                        }
                        catch { }
                        System.Threading.Thread.Sleep(200);
                        a++;
                    }

                }
                else
                {

//.........这里部分代码省略.........

        /// <summary>
        /// Processes a tlog to get the offsets - creates dxf of data
        /// </summary>
        /// <param name="fn">Filename</param>
        /// <returns>Offsets</returns>
        public static double[] getOffsets(string fn, int throttleThreshold = 0)
        {
            // based off tridge's work
            string logfile = fn;

            // old method
            float minx = 0;
            float maxx = 0;
            float miny = 0;
            float maxy = 0;
            float minz = 0;
            float maxz = 0;

            // this is for a dxf
            Polyline3dVertex vertex;
            List<Polyline3dVertex> vertexes = new List<Polyline3dVertex>();

            // data storage
            Tuple<float, float, float> offset = new Tuple<float, float, float>(0, 0, 0);
            List<Tuple<float, float, float>> data = new List<Tuple<float, float, float>>();

            Hashtable filter = new Hashtable();

            // track data to use
            bool useData = false;

            if (throttleThreshold <= 0)
                useData = true;

            log.Info("Start log: " + DateTime.Now);

            using (MAVLinkInterface mine = new MAVLinkInterface())
            {
                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 new double[] {0};
                }

                mine.logreadmode = true;

                mine.MAV.packets.Initialize(); // clear

                // gather data
                while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
                {
                    byte[] packetraw = mine.readPacket();

                    var packet = mine.DebugPacket(packetraw, false);

                    // this is for packets we dont know about
                    if (packet == null)
                        continue;

                    if (packet.GetType() == typeof (MAVLink.mavlink_vfr_hud_t))
                    {
                        if (((MAVLink.mavlink_vfr_hud_t) packet).throttle >= throttleThreshold)
                        {
                            useData = true;
                        }
                        else
                        {
                            useData = false;
                        }
                    }

                    if (packet.GetType() == typeof (MAVLink.mavlink_sensor_offsets_t))
                    {
                        offset = new Tuple<float, float, float>(
                            ((MAVLink.mavlink_sensor_offsets_t) packet).mag_ofs_x,
                            ((MAVLink.mavlink_sensor_offsets_t) packet).mag_ofs_y,
                            ((MAVLink.mavlink_sensor_offsets_t) packet).mag_ofs_z);
                    }
                    else if (packet.GetType() == typeof (MAVLink.mavlink_raw_imu_t) && useData)
                    {
                        int div = 20;

                        // fox dxf
                        vertex = new Polyline3dVertex(new Vector3f(
                            ((MAVLink.mavlink_raw_imu_t) packet).xmag - offset.Item1,
                            ((MAVLink.mavlink_raw_imu_t) packet).ymag - offset.Item2,
                            ((MAVLink.mavlink_raw_imu_t) packet).zmag - offset.Item3)
                            );
                        vertexes.Add(vertex);


                        // for old method
                        setMinorMax(((MAVLink.mavlink_raw_imu_t) packet).xmag - offset.Item1, ref minx, ref maxx);
                        setMinorMax(((MAVLink.mavlink_raw_imu_t) packet).ymag - offset.Item2, ref miny, ref maxy);
//.........这里部分代码省略.........

        public static void tlog(string logfile)
        {
            List<MLArray> mlList = new List<MLArray>();
            Hashtable datappl = new Hashtable();

            using (MAVLinkInterface mine = new MAVLinkInterface())
            {
                try
                {
                    mine.logplaybackfile =
                        new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
                }
                catch
                {
                    CustomMessageBox.Show("Log Can not be opened. Are you still connected?");
                    return;
                }
                mine.logreadmode = true;
                mine.speechenabled = false;

                while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
                {
                    MAVLink.MAVLinkMessage packet = mine.readPacket();
                    object data = packet.data;

                    if (data == null)
                        continue;

                    if (data is MAVLink.mavlink_heartbeat_t)
                    {
                        if (((MAVLink.mavlink_heartbeat_t) data).type == (byte) MAVLink.MAV_TYPE.GCS)
                            continue;
                    }

                    Type test = data.GetType();

                    DateTime time = mine.lastlogread;

                    double matlabtime = GetMatLabSerialDate(time);

                    try
                    {
                        foreach (var field in test.GetFields())
                        {
                            // field.Name has the field's name.

                            object fieldValue = field.GetValue(data); // Get value
                            if (field.FieldType.IsArray)
                            {
                            }
                            else
                            {
                                if (!datappl.ContainsKey(field.Name + "_" + field.DeclaringType.Name))
                                {
                                    datappl[field.Name + "_" + field.DeclaringType.Name] = new List<double[]>();
                                }

                                List<double[]> list =
                                    ((List<double[]>) datappl[field.Name + "_" + field.DeclaringType.Name]);

                                object value = fieldValue;

                                if (value.GetType() == typeof (Single))
                                {
                                    list.Add(new double[] {matlabtime, (double) (Single) field.GetValue(data)});
                                }
                                else if (value.GetType() == typeof (short))
                                {
                                    list.Add(new double[] {matlabtime, (double) (short) field.GetValue(data)});
                                }
                                else if (value.GetType() == typeof (ushort))
                                {
                                    list.Add(new double[] {matlabtime, (double) (ushort) field.GetValue(data)});
                                }
                                else if (value.GetType() == typeof (byte))
                                {
                                    list.Add(new double[] {matlabtime, (double) (byte) field.GetValue(data)});
                                }
                                else if (value.GetType() == typeof (sbyte))
                                {
                                    list.Add(new double[] {matlabtime, (double) (sbyte) field.GetValue(data)});
                                }
                                else if (value.GetType() == typeof (Int32))
                                {
                                    list.Add(new double[] {matlabtime, (double) (Int32) field.GetValue(data)});
                                }
                                else if (value.GetType() == typeof (UInt32))
                                {
                                    list.Add(new double[] {matlabtime, (double) (UInt32) field.GetValue(data)});
                                }
                                else if (value.GetType() == typeof (ulong))
                                {
                                    list.Add(new double[] {matlabtime, (double) (ulong) field.GetValue(data)});
                                }
                                else if (value.GetType() == typeof (long))
                                {
                                    list.Add(new double[] {matlabtime, (double) (long) field.GetValue(data)});
                                }
                                else if (value.GetType() == typeof (double))
                                {
//.........这里部分代码省略.........

        List<string[]> readLog(string fn)
        {
            if (logcache.Count > 0)
                return logcache;

            List<string[]> list = new List<string[]>();

            if (fn.ToLower().EndsWith("tlog"))
            {
                MAVLinkInterface mine = new MAVLinkInterface();
                mine.logplaybackfile = new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read));
                mine.logreadmode = true;

                mine.MAV.packets.Initialize(); // clear

                CurrentState cs = new CurrentState();

                string[] oldvalues = {""};

                while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
                {

                    byte[] packet = mine.readPacket();

                    cs.datetime = mine.lastlogread;

                    cs.UpdateCurrentSettings(null, true, mine);

                    // old
                    //		line	"GPS: 82686250, 1, 8, -34.1406480, 118.5441900, 0.0000, 309.1900, 315.9500, 0.0000, 279.1200"	string

                    //Status,Time,NSats,HDop,Lat,Lng,RelAlt,Alt,Spd,GCrs
                    //GPS, 3, 122732, 10, 0.00, -35.3628880, 149.1621961, 808.90, 810.30, 23.30, 94.04

                    string[] vals = new string[] { "GPS", "3",  (cs.datetime.ToUniversalTime() - new DateTime(cs.datetime.Year,cs.datetime.Month,cs.datetime.Day,0,0,0,DateTimeKind.Utc)).TotalMilliseconds.ToString(),
                    cs.satcount.ToString(),cs.gpshdop.ToString(),cs.lat.ToString(),cs.lng.ToString(),cs.altasl.ToString(),cs.altasl.ToString(),cs.groundspeed.ToString(),cs.yaw.ToString()};

                    if (oldvalues.Length > 2 && oldvalues[latpos] == vals[latpos]
                        && oldvalues[lngpos] == vals[lngpos]
                        && oldvalues[altpos] == vals[altpos])
                        continue;

                    oldvalues = vals;

                    list.Add(vals);
                    // 4 5 7
                    Console.Write((mine.logplaybackfile.BaseStream.Position * 100 / mine.logplaybackfile.BaseStream.Length) + "    \r");

                }

                mine.logplaybackfile.Close();

                logcache = list;

                return list;
            }

            StreamReader sr = new StreamReader(fn);

            string lasttime = "0";

            while (!sr.EndOfStream)
            {
                string line = sr.ReadLine();

                if (line.ToLower().StartsWith("gps"))
                {
                    if (!sr.EndOfStream)
                    {
                        string line2 = sr.ReadLine();
                        if (line2.ToLower().StartsWith("att"))
                        {
                            line = string.Concat(line, ",", line2);
                        }
                    }
                    string[] vals = line.Split(new char[] {',',':'});

                    if (lasttime == vals[timepos])
                        continue;

                    lasttime = vals[timepos];

                    list.Add(vals);
                }

            }

            sr.Close();

            logcache = list;

            return list;
        }

        static string process(MAVLinkInterface mavint)
        {
            DateTime Deadline = DateTime.Now.AddSeconds(60);

            Vector3 last_mag = null;
            double last_usec = 0;
            double count = 0;
            double[] total_error = new double[rotations.Length];

            float AHRS_ORIENTATION = 0;
            float COMPASS_ORIENT = 0;
            float COMPASS_EXTERNAL = 0;

            //# now gather all the data
            while (DateTime.Now < Deadline || mavint.BaseStream.BytesToRead > 0)
            {
                MAVLink.MAVLinkMessage packetbytes = mavint.readPacket();
                if (packetbytes.Length < 5)
                    continue;
                object packet = packetbytes.data;
                if (packet == null)
                    continue;
                if (packet is MAVLink.mavlink_param_value_t)
                {
                    MAVLink.mavlink_param_value_t m = (MAVLink.mavlink_param_value_t) packet;
                    if (str(m.param_id) == "AHRS_ORIENTATION")
                        AHRS_ORIENTATION = (int) (m.param_value);
                    if (str(m.param_id) == "COMPASS_ORIENT")
                        COMPASS_ORIENT = (int) (m.param_value);
                    if (str(m.param_id) == "COMPASS_EXTERNAL")
                        COMPASS_EXTERNAL = (int) (m.param_value);
                }

                if (packet is MAVLink.mavlink_raw_imu_t)
                {
                    MAVLink.mavlink_raw_imu_t m = (MAVLink.mavlink_raw_imu_t) packet;
                    Vector3 mag = new Vector3(m.xmag, m.ymag, m.zmag);
                    mag = mag_fixup(mag, AHRS_ORIENTATION, COMPASS_ORIENT, COMPASS_EXTERNAL);
                    Vector3 gyr = new Vector3(m.xgyro, m.ygyro, m.zgyro)*0.001;
                    double usec = m.time_usec;
                    if (last_mag != null && gyr.length() > radians(5.0))
                    {
                        add_errors(mag, gyr, last_mag, (usec - last_usec)*1.0e-6, total_error, rotations);
                        count += 1;
                    }
                    last_mag = mag;
                    last_usec = usec;
                }
            }

            int best_i = 0;
            double best_err = total_error[0];
            foreach (var i in range(len(rotations)))
            {
                Rotation r = rotations[i];
                //  if (!r.is_90_degrees())
                //      continue;

                //if ( opts.verbose) {
                //  print("%s err=%.2f" % (r, total_error[i]/count));}
                if (total_error[i] < best_err)
                {
                    best_i = i;
                    best_err = total_error[i];
                }
            }
            Rotation rans = rotations[best_i];
            Console.WriteLine("Best rotation is {0} err={1} from {2} points", rans, best_err/count, count);
            //print("Best rotation is %s err=%.2f from %u points" % (r, best_err/count, count));
            return rans.name;
        }

        private void but_droneapi_Click(object sender, EventArgs e)
        {
            string droneshareusername = MainV2.getConfig("droneshareusername");

            InputBox.Show("Username", "Username", ref droneshareusername);

            MainV2.config["droneshareusername"] = droneshareusername;

            string dronesharepassword = MainV2.getConfig("dronesharepassword");

            if (dronesharepassword != "")
            {
                try
                {
                    // fail on bad entry
                    var crypto = new Crypto();
                    dronesharepassword = crypto.DecryptString(dronesharepassword);
                }
                catch
                {
                }
            }

            InputBox.Show("Password", "Password", ref dronesharepassword, true);

            var crypto2 = new Crypto();

            string encryptedpw = crypto2.EncryptString(dronesharepassword);

            MainV2.config["dronesharepassword"] = encryptedpw;

            DroneProto dp = new DroneProto();

            if (dp.connect())
            {
                if (dp.loginUser(droneshareusername, dronesharepassword))
                {
                    MAVLinkInterface mine = new MAVLinkInterface();
                    var comfile = new CommsFile();
                    mine.BaseStream = comfile;
                    mine.BaseStream.PortName = @"C:\Users\hog\Documents\apm logs\iris 6-4-14\2014-04-06 09-07-32.tlog";
                    mine.BaseStream.Open();

                    comfile.bps = 4000;

                    mine.getHeartBeat();

                    dp.setVechileId(mine.MAV.Guid, 0, mine.MAV.sysid);

                    dp.startMission();

                    while (true)
                    {
                        byte[] packet = mine.readPacket();

                        dp.SendMavlink(packet, 0);
                    }

                    // dp.close();

                    // mine.Close();
                }
            }
        }

        // Return List with all GPS Messages splitted in string arrays
        Dictionary<long, VehicleLocation> readGPSMsgInLog(string fn)
        {
            Dictionary<long, VehicleLocation> vehiclePositionList = new Dictionary<long,VehicleLocation>();

            // Telemetry Log
            if (fn.ToLower().EndsWith("tlog"))
            {
                using (MAVLinkInterface mine = new MAVLinkInterface())
                {
                    mine.logplaybackfile = new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read));
                    mine.logreadmode = true;

                    mine.MAV.packets.Initialize(); // clear

                    CurrentState cs = new CurrentState();

                    while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
                    {
                        byte[] packet = mine.readPacket();

                        cs.datetime = mine.lastlogread;

                        cs.UpdateCurrentSettings(null, true, mine);

                        VehicleLocation location = new VehicleLocation();
                        location.Time = cs.datetime;
                        location.Lat = cs.lat;
                        location.Lon = cs.lng;
                        location.RelAlt = cs.alt;
                        location.AltAMSL = cs.altasl;

                        location.Roll = cs.roll;
                        location.Pitch = cs.pitch;
                        location.Yaw = cs.yaw;

                        vehiclePositionList[ToMilliseconds(location.Time)] = location;
                        // 4 5 7
                        Console.Write((mine.logplaybackfile.BaseStream.Position * 100 / mine.logplaybackfile.BaseStream.Length) + "    \r");
                    }
                    mine.logplaybackfile.Close();
                }
            }
            // DataFlash Log
            else
            {
                // convert bin to log
                if (fn.ToLower().EndsWith("bin"))
                {
                    string tempfile = Path.GetTempFileName();
                    Log.BinaryLog.ConvertBin(fn, tempfile);
                    fn = tempfile;
                }

                StreamReader sr = new StreamReader(fn);

                // Will hold the last seen Attitude information in order to incorporate them into the GPS Info
                float currentYaw = 0f;
                float currentRoll = 0f;
                float currentPitch = 0f;
                int a = 0;
                while (!sr.EndOfStream)
                {
                    a++;
                    string line = sr.ReadLine();

                    var item = DFLog.GetDFItemFromLine(line, a);

                    if (item.msgtype == "GPS")
                    {
                        if (!DFLog.logformat.ContainsKey("GPS"))
                            continue;

                        int latindex = DFLog.FindMessageOffset("GPS", "Lat");
                        int lngindex = DFLog.FindMessageOffset("GPS", "Lng");
                        int altindex = DFLog.FindMessageOffset("GPS", "Alt");
                        int raltindex = DFLog.FindMessageOffset("GPS", "RAlt");
                        if (raltindex == -1)
                            raltindex = DFLog.FindMessageOffset("GPS", "RelAlt");

                        VehicleLocation location = new VehicleLocation();

                        try
                        {
                            location.Time = DFLog.GetTimeGPS(line);
                            location.Lat = double.Parse(item.items[latindex], CultureInfo.InvariantCulture);
                            location.Lon = double.Parse(item.items[lngindex], CultureInfo.InvariantCulture);
                            location.RelAlt = double.Parse(item.items[raltindex], CultureInfo.InvariantCulture);
                            location.AltAMSL = double.Parse(item.items[altindex], CultureInfo.InvariantCulture);

                            location.Roll = currentRoll;
                            location.Pitch = currentPitch;
                            location.Yaw = currentYaw;

                            long millis = ToMilliseconds(location.Time);

                            //System.Diagnostics.Debug.WriteLine("GPS MSG - UTCMillis = " + millis  + "  GPS Week = " + getValueFromStringArray(gpsLineValues, gpsweekpos) + "  TimeMS = " + getValueFromStringArray(gpsLineValues, timepos));

                            if (!vehiclePositionList.ContainsKey(millis) && (location.Time != DateTime.MinValue))
                                vehiclePositionList[millis] = location;
//.........这里部分代码省略.........

        private void but_gpsinj_Click(object sender, EventArgs e)
        {
            OpenFileDialog ofd = new OpenFileDialog();
            ofd.Filter = "tlog|*.tlog";
            ofd.ShowDialog();

            SaveFileDialog sfd = new SaveFileDialog();
            sfd.FileName = "output.dat";
            sfd.ShowDialog();

            if (ofd.CheckFileExists)
            {
                using (var st = sfd.OpenFile())
                {
                    using (MAVLinkInterface mine = new MAVLinkInterface(ofd.OpenFile()))
                    {
                        mine.logreadmode = true;

                        while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
                        {
                            MAVLink.MAVLinkMessage packet = mine.readPacket();

                            if (packet.msgid == (uint) MAVLink.MAVLINK_MSG_ID.GPS_INJECT_DATA)
                            {
                                var item = packet.ToStructure<MAVLink.mavlink_gps_inject_data_t>();
                                st.Write(item.data, 0, item.len);
                            }
                        }
                    }
                }
            }
        }