public static void Process(string filename)
        {
            using (CollectionBuffer file = new CollectionBuffer(File.OpenRead(filename)))
            {
                List <Value> rc = new List <Value>();
                for (int i = 1; i <= 14; i++)
                {
                    rc.Add(new Value());
                }
                var roll  = new Value();
                var pitch = new Value();

                foreach (var DFMSG in file.GetEnumeratorType(new string[] { "ATT", "RCOU" }))
                {
                    if (DFMSG.msgtype == "ATT")
                    {
                        var desroll  = Double.Parse(DFMSG["DesRoll"], CultureInfo.InvariantCulture);
                        var actroll  = Double.Parse(DFMSG["Roll"], CultureInfo.InvariantCulture);
                        var despitch = Double.Parse(DFMSG["DesPitch"], CultureInfo.InvariantCulture);
                        var actpitch = Double.Parse(DFMSG["Pitch"], CultureInfo.InvariantCulture);

                        var rolldelta  = desroll - actroll;
                        var pitchdelta = despitch - actpitch;

                        roll.AddSample(rolldelta);
                        pitch.AddSample(pitchdelta);
                    }
                    else if (DFMSG.msgtype == "RCOU")
                    {
                        for (int i = 1; i <= 14; i++)
                        {
                            foreach (var name in new string[] { "C" + i, "Ch" + i, "Chan" + i })
                            {
                                if (DFMSG.parent.logformat["RCOU"].FieldNames.Contains(name))
                                {
                                    var C = Double.Parse(DFMSG[name], CultureInfo.InvariantCulture);

                                    rc[i - 1].AddSample(C);

                                    break;
                                }
                            }
                        }
                    }
                }

                Console.WriteLine(roll);
                Console.WriteLine(pitch);

                Value avg = new Value();
                for (int i = 1; i <= 14; i++)
                {
                    if (rc[i - 1].min > 800)
                    {
                        avg.AddSample(rc[i - 1].avg);
                    }
                }

                if (avg.maxdelta > 100)
                {
                    Console.Beep();
                    Console.WriteLine("Signs of motor failure");
                }

                for (int i = 1; i <= 14; i++)
                {
                    Console.WriteLine(rc[i - 1]);
                }
            }
        }
Esempio n. 2
0
        private void but_fftimu_Click(object sender, EventArgs e)
        {
            Utilities.FFT2 fft = new FFT2();
            using (
                OpenFileDialog ofd = new OpenFileDialog())
            {
                ofd.Filter = "*.log;*.bin|*.log;*.bin";

                ofd.ShowDialog();

                if (!File.Exists(ofd.FileName))
                    return;

                var file = new CollectionBuffer(File.OpenRead(ofd.FileName));

                int bins = (int)NUM_bins.Value;

                int N = 1 << bins;

                Color[] color = new Color[] { Color.Red, Color.Green, Color.Blue, Color.Black, Color.Violet, Color.Orange };
                ZedGraphControl[] ctls = new ZedGraphControl[]
                {
                    zedGraphControl1, zedGraphControl2, zedGraphControl3, zedGraphControl4, zedGraphControl5,
                    zedGraphControl6
                };

                // 3 imus * 2 sets of measurements(gyr/acc)
                datastate[] alldata = new datastate[3 * 2];
                for (int a = 0; a < alldata.Length; a++)
                    alldata[a] = new datastate();

                Log.DFLog dflog = new Log.DFLog();

                foreach (var item in file.GetEnumeratorType(new string[] {"FMT","IMU","IMU2","IMU3"}))
                {
                    if (item.msgtype == null)
                    {
                        continue;
                    }

                    if (item.msgtype == "FMT")
                    {
                        dflog.FMTLine(file[item.lineno]);
                        continue;
                    }

                    if (item.msgtype.StartsWith("IMU"))
                    {
                        int sensorno = 0;
                        if (item.msgtype == "IMU")
                            sensorno = 0;
                        if (item.msgtype == "IMU2")
                            sensorno = 1;
                        if (item.msgtype == "IMU3")
                            sensorno = 2;

                        alldata[sensorno+3].type = item.msgtype +" A";

                        int offsetAX = dflog.FindMessageOffset(item.msgtype, "AccX");
                        int offsetAY = dflog.FindMessageOffset(item.msgtype, "AccY");
                        int offsetAZ = dflog.FindMessageOffset(item.msgtype, "AccZ");
                        int offsetTime = dflog.FindMessageOffset(item.msgtype, "TimeUS");

                        double time = double.Parse(item.items[offsetTime]) / 1000.0;

                        if (time != alldata[sensorno + 3].lasttime)
                            alldata[sensorno + 3].timedelta = alldata[sensorno + 3].timedelta * 0.99 +
                                                          (time - alldata[sensorno + 3].lasttime) * 0.01;

                        alldata[sensorno + 3].lasttime = time;

                        alldata[sensorno + 3].datax.Add(double.Parse(item.items[offsetAX]));
                        alldata[sensorno + 3].datay.Add(double.Parse(item.items[offsetAY]));
                        alldata[sensorno + 3].dataz.Add(double.Parse(item.items[offsetAZ]));

                        //gyro
                        alldata[sensorno].type = item.msgtype + " G";

                        int offsetGX = dflog.FindMessageOffset(item.msgtype, "GyrX");
                        int offsetGY = dflog.FindMessageOffset(item.msgtype, "GyrY");
                        int offsetGZ = dflog.FindMessageOffset(item.msgtype, "GyrZ");

                        if (time != alldata[sensorno].lasttime)
                            alldata[sensorno].timedelta = alldata[sensorno].timedelta * 0.99 +
                                                          (time - alldata[sensorno].lasttime) * 0.01;

                        alldata[sensorno].lasttime = time;

                        alldata[sensorno].datax.Add(double.Parse(item.items[offsetGX]));
                        alldata[sensorno].datay.Add(double.Parse(item.items[offsetGY]));
                        alldata[sensorno].dataz.Add(double.Parse(item.items[offsetGZ]));
                    }
                }

                int controlindex = 0;

                foreach (var sensordata in alldata)
                {
                    if (sensordata.datax.Count <= N)
                        continue;

                    double samplerate = 0;

                    samplerate = Math.Round(1000 / sensordata.timedelta, 1);

                    double[] freqt = fft.FreqTable(N, (int)samplerate);

                    double[] avgx = new double[N / 2];
                    double[] avgy = new double[N / 2];
                    double[] avgz = new double[N / 2];

                    int totalsamples = sensordata.datax.Count;
                    int count = totalsamples / N;
                    int done = 0;
                    while (count > 1) // skip last part
                    {
                        var fftanswerx = fft.rin(sensordata.datax.Skip(N * done).Take(N).ToArray(), (uint)bins);
                        var fftanswery = fft.rin(sensordata.datay.Skip(N * done).Take(N).ToArray(), (uint)bins);
                        var fftanswerz = fft.rin(sensordata.dataz.Skip(N * done).Take(N).ToArray(), (uint)bins);

                        for (int b = 0; b < N / 2; b++)
                        {
                            if (freqt[b] < (double)NUM_startfreq.Value)
                                continue;

                            avgx[b] += fftanswerx[b] / (N / 2);
                            avgy[b] += fftanswery[b] / (N / 2);
                            avgz[b] += fftanswerz[b] / (N / 2);
                        }

                        count--;
                        done++;
                    }

                    ZedGraph.PointPairList pplx = new ZedGraph.PointPairList(freqt, avgx);
                    ZedGraph.PointPairList pply = new ZedGraph.PointPairList(freqt, avgy);
                    ZedGraph.PointPairList pplz = new ZedGraph.PointPairList(freqt, avgz);

                    var curvex = new LineItem(sensordata.type + " x", pplx, color[0], SymbolType.None);
                    var curvey = new LineItem(sensordata.type + " y", pply, color[1], SymbolType.None);
                    var curvez = new LineItem(sensordata.type + " z", pplz, color[2], SymbolType.None);

                    ctls[controlindex].GraphPane.Legend.IsVisible = true;

                    ctls[controlindex].GraphPane.XAxis.Title.Text = "Freq Hz";
                    ctls[controlindex].GraphPane.YAxis.Title.Text = "Amplitude";
                    ctls[controlindex].GraphPane.Title.Text = "FFT " + sensordata.type + " - " +
                                                              Path.GetFileName(ofd.FileName) + " - " + samplerate +
                                                              "hz input";

                    ctls[controlindex].GraphPane.CurveList.Clear();

                    ctls[controlindex].GraphPane.CurveList.Add(curvex);
                    ctls[controlindex].GraphPane.CurveList.Add(curvey);
                    ctls[controlindex].GraphPane.CurveList.Add(curvez);

                    ctls[controlindex].Invalidate();
                    ctls[controlindex].AxisChange();

                    ctls[controlindex].GraphPane.XAxis.Scale.Max = samplerate / 2;

                    ctls[controlindex].Refresh();

                    controlindex++;
                }
            }
        }
Esempio n. 3
0
        private Dictionary<long, VehicleLocation> readTRIGMsgInLog(string fn)
        {
            Dictionary<long, VehicleLocation> list = new Dictionary<long, VehicleLocation>();


            float currentSAlt = 0;
            using (var sr = new CollectionBuffer(File.OpenRead(fn)))
            {
                foreach (var item in sr.GetEnumeratorType(new string[] { "TRIG", "RFND" }))
                {
                    if (item.msgtype == "TRIG")
                    {
                        int latindex = sr.dflog.FindMessageOffset("TRIG", "Lat");
                        int lngindex = sr.dflog.FindMessageOffset("TRIG", "Lng");
                        int altindex = sr.dflog.FindMessageOffset("TRIG", "Alt");
                        int raltindex = sr.dflog.FindMessageOffset("TRIG", "RelAlt");

                        int rindex = sr.dflog.FindMessageOffset("TRIG", "Roll");
                        int pindex = sr.dflog.FindMessageOffset("TRIG", "Pitch");
                        int yindex = sr.dflog.FindMessageOffset("TRIG", "Yaw");

                        int gtimeindex = sr.dflog.FindMessageOffset("TRIG", "GPSTime");
                        int gweekindex = sr.dflog.FindMessageOffset("TRIG", "GPSWeek");

                        VehicleLocation p = new VehicleLocation();

                        p.Time = GetTimeFromGps(int.Parse(item.items[gweekindex], CultureInfo.InvariantCulture),
                            int.Parse(item.items[gtimeindex], CultureInfo.InvariantCulture));

                        p.Lat = double.Parse(item.items[latindex], CultureInfo.InvariantCulture);
                        p.Lon = double.Parse(item.items[lngindex], CultureInfo.InvariantCulture);
                        p.AltAMSL = double.Parse(item.items[altindex], CultureInfo.InvariantCulture);
                        if (raltindex != -1)
                            p.RelAlt = double.Parse(item.items[raltindex], CultureInfo.InvariantCulture);

                        p.Pitch = float.Parse(item.items[pindex], CultureInfo.InvariantCulture);
                        p.Roll = float.Parse(item.items[rindex], CultureInfo.InvariantCulture);
                        p.Yaw = float.Parse(item.items[yindex], CultureInfo.InvariantCulture);

                        p.SAlt = currentSAlt;

                        list[ToMilliseconds(p.Time)] = p;
                    }
                    else if (item.msgtype == "RFND")
                    {
                        int SAltindex = sr.dflog.FindMessageOffset("RFND", "Dist1");

                        if (SAltindex != -1)
                        {
                            currentSAlt = float.Parse(item.items[SAltindex], CultureInfo.InvariantCulture);
                        }
                    }
                }
            }

            return list;
        }
        public static PointPairList ProcessExpression(ref DFLog dflog, ref CollectionBuffer logdata, string expression)
        {
            PointPairList answer = new PointPairList();

            //earth_accel_df(IMU2,ATT).x
            if (expression.Contains("earth_accel_df"))
            {
                var matchs = Regex.Matches(expression, @"([A-z0-9_]+),([A-z0-9_]+)");

                List<string> msglist = new List<string>();

                foreach (Match match in matchs)
                {
                    foreach (var item in match.Groups)
                    {
                        msglist.Add(item.ToString());
                    }
                }

                foreach (var item in logdata.GetEnumeratorType(msglist.ToArray()))
                {
                    IMU imu = new IMU();
                    ATT att = new ATT();

                    if (item.msgtype == "ATT")
                    {
                        ATT.Roll = double.Parse(item.items[dflog.FindMessageOffset("ATT", "Roll")]);
                        ATT.Pitch = double.Parse(item.items[dflog.FindMessageOffset("ATT", "Pitch")]);
                        ATT.Yaw = double.Parse(item.items[dflog.FindMessageOffset("ATT", "Yaw")]);
                    } 
                    else if (item.msgtype == "IMU")
                    {
                        IMU.AccX = double.Parse(item.items[dflog.FindMessageOffset("IMU", "AccX")]);
                        IMU.AccY = double.Parse(item.items[dflog.FindMessageOffset("IMU", "AccY")]);
                        IMU.AccZ = double.Parse(item.items[dflog.FindMessageOffset("IMU", "AccZ")]);
                    }
                    else if (item.msgtype == "IMU2")
                    {
                        IMU.AccX = double.Parse(item.items[dflog.FindMessageOffset("IMU2", "AccX")]);
                        IMU.AccY = double.Parse(item.items[dflog.FindMessageOffset("IMU2", "AccY")]);
                        IMU.AccZ = double.Parse(item.items[dflog.FindMessageOffset("IMU2", "AccZ")]);
                    }

                    if (expression.Contains(".x"))
                    {
                        answer.Add(item.lineno, earth_accel_df(imu, att).x);
                    }
                    if (expression.Contains(".y"))
                    {
                        answer.Add(item.lineno,earth_accel_df(imu, att).y);
                    }
                    if (expression.Contains(".z"))
                    {
                        answer.Add(item.lineno,earth_accel_df(imu, att).z);
                    }
                }
            } // delta(gps_velocity_df(GPS).x,'x',GPS.TimeUS)
            else if (expression.Contains("delta(gps_velocity_df(GPS)"))
            {
                foreach (var item in logdata.GetEnumeratorType("GPS"))
                {
                    var gps = new GPS();

                    if (item.msgtype == "GPS")
                    {
                        GPS.Spd = double.Parse(item.items[dflog.FindMessageOffset("GPS", "Spd")]);
                        GPS.GCrs = double.Parse(item.items[dflog.FindMessageOffset("GPS", "GCrs")]);
                        GPS.VZ = double.Parse(item.items[dflog.FindMessageOffset("GPS", "VZ")]);
                    }

                    if (expression.Contains(".x"))
                    {
                        answer.Add(item.lineno,delta(gps_velocity_df(gps).x, "x", item.timems * 1000));
                    }
                    else if (expression.Contains(".y"))
                    {
                        answer.Add(item.lineno,delta(gps_velocity_df(gps).y, "y", item.timems * 1000));
                    }
                    else if (expression.Contains(".z"))
                    {
                        answer.Add(item.lineno, delta(gps_velocity_df(gps).z, "z", item.timems * 1000) - 9.8);
                    }
                }
            }
            else if (expression.StartsWith("degrees"))
            {
                var matchs = Regex.Matches(expression, @"([A-z0-9_]+)\.([A-z0-9_]+)");

                if (matchs.Count > 0)
                {
                    var type = matchs[0].Groups[1].Value.ToString();
                    var field = matchs[0].Groups[2].Value.ToString();

                    foreach (var item in logdata.GetEnumeratorType(type))
                    {
                        answer.Add(item.lineno, degrees(double.Parse(item.items[dflog.FindMessageOffset(type, field)])));
                    }
                }
            }
            else if (expression.StartsWith("sqrt"))
            {
                // there are alot of assumptions made in this code
                Dictionary<int,double> work = new Dictionary<int, double>();
                List<KeyValuePair<string, string>> types = new EventList<KeyValuePair<string, string>>();

                var matchs = Regex.Matches(expression, @"(([A-z0-9_]+)\.([A-z0-9_]+)\*\*2)");

                if (matchs.Count > 0)
                {
                    foreach (Match match in matchs)
                    {
                        var type = match.Groups[2].Value.ToString();
                        var field = match.Groups[3].Value.ToString();

                        types.Add(new KeyValuePair<string, string>(type,field));
                    }

                    List<string> keyarray = new List<string>();
                    types.ForEach(g => { keyarray.Add(g.Key); });
                    List<string> valuearray= new List<string>();
                    types.ForEach(g => { valuearray.Add(g.Value); });

                    foreach (var item in logdata.GetEnumeratorType(keyarray.ToArray()))
                    {
                        for (int a = 0; a < types.Count; a++)
                        {
                            var key = keyarray[a];
                            var value = valuearray[a];
                            var offset = dflog.FindMessageOffset(key, value);
                            if (offset == -1)
                                continue;
                            work[a] = double.Parse(item.items[offset]);
                        }

                        double workanswer = 0;
                        foreach (var value in work.Values)
                        {
                            workanswer += Math.Pow(value, 2);
                        }
                        answer.Add(item.lineno, Math.Sqrt(workanswer));
                    }
                }
            }

            return answer;
        }
Esempio n. 5
0
        /// <summary>
        /// Return a list of all gps messages with there timestamp from the log
        /// </summary>
        /// <param name="fn"></param>
        /// <returns></returns>
        private 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;

                    CurrentState cs = new CurrentState();

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

                        cs.datetime = mine.lastlogread;

                        cs.UpdateCurrentSettings(null, true, mine);

                        // check for valid lock
                        if (!(cs.gpsstatus >=3 || cs.gpsstatus2 >= 3))
                            continue;

                        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;

                        location.SAlt = cs.sonarrange;

                        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
            {
                using (var sr = new CollectionBuffer(File.OpenRead(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;
                    float currentSAlt = 0f;

                    foreach (var item in sr.GetEnumeratorType(new string[] { "GPS", "GPS2", "ATT", "CTUN", "RFND" }))
                    {
                        // 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

                        var gpstouse = UseGpsorGPS2();

                        if (item.msgtype == gpstouse)
                        {
                            if (!sr.dflog.logformat.ContainsKey(gpstouse))
                                continue;

                            int latindex = sr.dflog.FindMessageOffset(gpstouse, "Lat");
                            int lngindex = sr.dflog.FindMessageOffset(gpstouse, "Lng");
                            int altindex = sr.dflog.FindMessageOffset(gpstouse, "Alt");
                            int statusindex = sr.dflog.FindMessageOffset(gpstouse, "Status");
                            int raltindex = sr.dflog.FindMessageOffset(gpstouse, "RAlt");
                            if (raltindex == -1)
                                raltindex = sr.dflog.FindMessageOffset(gpstouse, "RelAlt");

                            VehicleLocation location = new VehicleLocation();

                            try
                            {
                                location.Time = item.time;

                                if (statusindex != -1)
                                {
                                    // check for valid lock
                                    if (double.Parse(item.items[statusindex], CultureInfo.InvariantCulture) < 3)
                                        continue;
                                }
                                if (latindex != -1)
                                    location.Lat = double.Parse(item.items[latindex], CultureInfo.InvariantCulture);
                                if (lngindex != -1)
                                    location.Lon = double.Parse(item.items[lngindex], CultureInfo.InvariantCulture);
                                if (raltindex != -1)
                                    location.RelAlt = double.Parse(item.items[raltindex], CultureInfo.InvariantCulture);
                                if (altindex != -1)
                                    location.AltAMSL = double.Parse(item.items[altindex], CultureInfo.InvariantCulture);

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

                                location.SAlt = currentSAlt;

                                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;
                            }
                            catch
                            {
                                Console.WriteLine("Bad "+gpstouse+" Line");
                            }
                        }
                        else if (item.msgtype == "ATT")
                        {
                            int Rindex = sr.dflog.FindMessageOffset("ATT", "Roll");
                            int Pindex = sr.dflog.FindMessageOffset("ATT", "Pitch");
                            int Yindex = sr.dflog.FindMessageOffset("ATT", "Yaw");

                            if (Rindex != -1)
                                currentRoll = float.Parse(item.items[Rindex], CultureInfo.InvariantCulture);
                            if (Pindex != -1)
                                currentPitch = float.Parse(item.items[Pindex], CultureInfo.InvariantCulture);
                            if (Yindex != -1)
                                currentYaw = float.Parse(item.items[Yindex], CultureInfo.InvariantCulture);
                        }
                        else if (item.msgtype == "CTUN")
                        {
                            int SAltindex = sr.dflog.FindMessageOffset("CTUN", "SAlt");

                            if (SAltindex != -1)
                            {
                                currentSAlt = float.Parse(item.items[SAltindex], CultureInfo.InvariantCulture);
                            }
                        }
                        else if (item.msgtype == "RFND")
                        {
                            int SAltindex = sr.dflog.FindMessageOffset("RFND", "Dist1");

                            if (SAltindex != -1)
                            {
                                currentSAlt = float.Parse(item.items[SAltindex], CultureInfo.InvariantCulture);
                            }
                        }
                    }
                }
            }

            return vehiclePositionList;
        }
Esempio n. 6
0
        /// <summary>
        /// Return a list of all cam messages in a log with timestamp
        /// </summary>
        /// <param name="fn"></param>
        /// <returns></returns>
        private Dictionary<long, VehicleLocation> readCAMMsgInLog(string fn)
        {
            Dictionary<long, VehicleLocation> list = new Dictionary<long, VehicleLocation>();

            // Telemetry Log
            if (fn.ToLower().EndsWith("tlog"))
            {
                TXT_outputlog.AppendText("Warning: tlogs are not fully supported when using CAM Messages\n");

                using (MAVLinkInterface mine = new MAVLinkInterface())
                {
                    mine.logplaybackfile =
                        new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read));
                    mine.logreadmode = true;

                    CurrentState cs = new CurrentState();

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

                        cs.datetime = mine.lastlogread;
                        cs.UpdateCurrentSettings(null, true, mine);

                        if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.CAMERA_FEEDBACK)
                        {
                            var msg = (MAVLink.mavlink_camera_feedback_t)packet.data;

                            VehicleLocation location = new VehicleLocation();
                            location.Time = FromUTCTimeMilliseconds((long)(msg.time_usec / 1000));// cs.datetime;
                            location.Lat = msg.lat;
                            location.Lon = msg.lng;
                            location.RelAlt = msg.alt_rel;
                            location.AltAMSL = msg.alt_msl;

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

                            location.SAlt = cs.sonarrange;

                            list[ToMilliseconds(location.Time)] = location;

                            Console.Write((mine.logplaybackfile.BaseStream.Position*100/
                                           mine.logplaybackfile.BaseStream.Length) + "    \r");
                        }
                    }
                    mine.logplaybackfile.Close();
                }
            }
            // DataFlash Log
            else
            {
                float currentSAlt = 0;
                using (var sr = new CollectionBuffer(File.OpenRead(fn)))
                {
                    foreach (var item in sr.GetEnumeratorType(new string[] { "CAM", "RFND" }))
                    {
                        if (item.msgtype == "CAM")
                        {
                            int latindex = sr.dflog.FindMessageOffset("CAM", "Lat");
                            int lngindex = sr.dflog.FindMessageOffset("CAM", "Lng");
                            int altindex = sr.dflog.FindMessageOffset("CAM", "Alt");
                            int raltindex = sr.dflog.FindMessageOffset("CAM", "RelAlt");

                            int rindex = sr.dflog.FindMessageOffset("CAM", "Roll");
                            int pindex = sr.dflog.FindMessageOffset("CAM", "Pitch");
                            int yindex = sr.dflog.FindMessageOffset("CAM", "Yaw");

                            int gtimeindex = sr.dflog.FindMessageOffset("CAM", "GPSTime");
                            int gweekindex = sr.dflog.FindMessageOffset("CAM", "GPSWeek");

                            VehicleLocation p = new VehicleLocation();

                            p.Time = GetTimeFromGps(int.Parse(item.items[gweekindex], CultureInfo.InvariantCulture),
                                int.Parse(item.items[gtimeindex], CultureInfo.InvariantCulture));

                            p.Lat = double.Parse(item.items[latindex], CultureInfo.InvariantCulture);
                            p.Lon = double.Parse(item.items[lngindex], CultureInfo.InvariantCulture);
                            p.AltAMSL = double.Parse(item.items[altindex], CultureInfo.InvariantCulture);
                            if (raltindex != -1)
                                p.RelAlt = double.Parse(item.items[raltindex], CultureInfo.InvariantCulture);

                            p.Pitch = float.Parse(item.items[pindex], CultureInfo.InvariantCulture);
                            p.Roll = float.Parse(item.items[rindex], CultureInfo.InvariantCulture);
                            p.Yaw = float.Parse(item.items[yindex], CultureInfo.InvariantCulture);

                            p.SAlt = currentSAlt;

                            list[ToMilliseconds(p.Time)] = p;
                        }
                        else if (item.msgtype == "RFND")
                        {
                            int SAltindex = sr.dflog.FindMessageOffset("RFND", "Dist1");

                            if (SAltindex != -1)
                            {
                                currentSAlt = float.Parse(item.items[SAltindex], CultureInfo.InvariantCulture);
                            }
                        }
                    }
                }
            }
            return list;
        }
Esempio n. 7
0
        public static double[] getOffsetsLog(string fn)
        {
            // this is for a dxf
            PolylineVertex vertex;
            List<PolylineVertex> vertexes = new List<PolylineVertex>();

            List<Tuple<float, float, float>> data = new List<Tuple<float, float, float>>();

            List<Tuple<float, float, float>> data2 = new List<Tuple<float, float, float>>();

            List<Tuple<float, float, float>> data3 = new List<Tuple<float, float, float>>();

            double[] ofsDoubles = new double[3];
            double[] ofsDoubles2 = new double[3];
            double[] ofsDoubles3 = new double[3];

            CollectionBuffer logdata = new CollectionBuffer(File.OpenRead(fn));

            var dflog = logdata.dflog;
            
            foreach (var line in logdata.GetEnumeratorType(new[]{"MAG","MAG2","MAG3"}))
            {
                if (line.msgtype == "MAG" || line.msgtype == "MAG2" || line.msgtype == "MAG3")
                {
                    int indexmagx = dflog.FindMessageOffset(line.msgtype, "MagX");
                    int indexmagy = dflog.FindMessageOffset(line.msgtype, "MagY");
                    int indexmagz = dflog.FindMessageOffset(line.msgtype, "MagZ");

                    int indexoffsetx = dflog.FindMessageOffset(line.msgtype, "OfsX");
                    int indexoffsety = dflog.FindMessageOffset(line.msgtype, "OfsY");
                    int indexoffsetz = dflog.FindMessageOffset(line.msgtype, "OfsZ");

                    if (indexmagx != -1 && indexoffsetx != -1)
                    {
                        float magx = float.Parse(line.items[indexmagx]);
                        float magy = float.Parse(line.items[indexmagy]);
                        float magz = float.Parse(line.items[indexmagz]);

                        float offsetx = float.Parse(line.items[indexoffsetx]);
                        float offsety = float.Parse(line.items[indexoffsety]);
                        float offsetz = float.Parse(line.items[indexoffsetz]);

                        //offsetx = offsety = offsetz = 0;

                        if (line.msgtype == "MAG")
                        {
                            data.Add(new Tuple<float, float, float>(
                                magx - offsetx,
                                magy - offsety,
                                magz - offsetz));

                            ofsDoubles[0] = offsetx;
                            ofsDoubles[1] = offsety;
                            ofsDoubles[2] = offsetz;

                            // fox dxf
                            vertex = new PolylineVertex(new netDxf.Vector3(magx - offsetx,
                                magy - offsety,
                                magz - offsetz)
                                );
                            vertexes.Add(vertex);
                        }
                        else if (line.msgtype == "MAG2")
                        {
                            data2.Add(new Tuple<float, float, float>(
                                magx - offsetx,
                                magy - offsety,
                                magz - offsetz));

                            ofsDoubles2[0] = offsetx;
                            ofsDoubles2[1] = offsety;
                            ofsDoubles2[2] = offsetz;
                        }
                        else if (line.msgtype == "MAG3")
                        {
                            data3.Add(new Tuple<float, float, float>(
                                magx - offsetx,
                                magy - offsety,
                                magz - offsetz));

                            ofsDoubles3[0] = offsetx;
                            ofsDoubles3[1] = offsety;
                            ofsDoubles3[2] = offsetz;
                        }
                    }
                }
            }

            double[] x = LeastSq(data, false);

            log.InfoFormat("magcal 1 ofs {0},{1},{2} strength {3} old ofs {4},{5},{6}", x[0], x[1], x[2], x[3], ofsDoubles[0], ofsDoubles[1], ofsDoubles[2]);
            
            x = LeastSq(data,true);

            log.InfoFormat("magcalel 1 ofs {0},{1},{2} strength {3} old ofs {4},{5},{6}", x[0], x[1], x[2], x[3], ofsDoubles[0], ofsDoubles[1], ofsDoubles[2]);

            if (data2.Count > 0)
            {
                double[] x2 = LeastSq(data2, false);
                log.InfoFormat("magcal 2 ofs {0},{1},{2} strength {3} old ofs {4},{5},{6}", x2[0], x2[1], x2[2], x2[3], ofsDoubles2[0], ofsDoubles2[1], ofsDoubles2[2]);
                x2 = LeastSq(data2, true);
                log.InfoFormat("magcalel 2 ofs {0},{1},{2} strength {3} old ofs {4},{5},{6}", x2[0], x2[1], x2[2], x2[3], ofsDoubles2[0], ofsDoubles2[1], ofsDoubles2[2]);
            }

            if (data3.Count > 0)
            {
                double[] x3 = LeastSq(data3, false);
                log.InfoFormat("magcal 3 ofs {0},{1},{2} strength {3} old ofs {4},{5},{6}", x3[0], x3[1], x3[2], x3[3], ofsDoubles3[0], ofsDoubles3[1], ofsDoubles3[2]);
                x3 = LeastSq(data3, true);
                log.InfoFormat("magcalel 3 ofs {0},{1},{2} strength {3} old ofs {4},{5},{6}", x3[0], x3[1], x3[2], x3[3], ofsDoubles3[0], ofsDoubles3[1], ofsDoubles3[2]);
            }


            log.Info("Least Sq Done " + DateTime.Now);

            doDXF(vertexes, x);

            Array.Resize<double>(ref x, 3);

            return x;
        }
Esempio n. 8
0
        public static void ProcessFile(string logfile)
        {
            if (File.Exists(logfile + ".jpg"))
                return;

            double minx = 99999;
            double maxx = -99999;
            double miny = 99999;
            double maxy = -99999;

            bool sitl = false;

            Dictionary<int, List<PointLatLngAlt>> loc_list = new Dictionary<int, List<PointLatLngAlt>>();

            try
            {
                if (logfile.ToLower().EndsWith(".tlog"))
                {
                    using (MAVLinkInterface mine = new MAVLinkInterface())
                    using (
                        mine.logplaybackfile =
                            new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite))
                        )
                    {
                        mine.logreadmode = true;
                        mine.speechenabled = false;

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

                            if (packet.Length < 5)
                                continue;

                            if (packet.msgid == (byte) MAVLink.MAVLINK_MSG_ID.SIM_STATE ||
                                packet.msgid == (byte) MAVLink.MAVLINK_MSG_ID.SIMSTATE)
                            {
                                sitl = true;
                            }

                            if (packet.msgid == (byte) MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT)
                            {
                                var loc = packet.ToStructure<MAVLink.mavlink_global_position_int_t>();

                                if (loc.lat == 0 || loc.lon == 0)
                                    continue;

                                var id = MAVList.GetID(packet.sysid, packet.compid);

                                if (!loc_list.ContainsKey(id))
                                    loc_list[id] = new List<PointLatLngAlt>();

                                loc_list[id].Add(new PointLatLngAlt(loc.lat/10000000.0f, loc.lon/10000000.0f));

                                minx = Math.Min(minx, loc.lon/10000000.0f);
                                maxx = Math.Max(maxx, loc.lon/10000000.0f);
                                miny = Math.Min(miny, loc.lat/10000000.0f);
                                maxy = Math.Max(maxy, loc.lat/10000000.0f);
                            }
                        }
                    }
                }
                else if (logfile.ToLower().EndsWith(".bin") || logfile.ToLower().EndsWith(".log"))
                {
                    using (
                        CollectionBuffer colbuf =
                            new CollectionBuffer(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite))
                        )
                    {
                        loc_list[0] = new List<PointLatLngAlt>();

                        foreach (var item in colbuf.GetEnumeratorType("GPS"))
                        {
                            if (item.msgtype.StartsWith("GPS"))
                            {
                                if (!colbuf.dflog.logformat.ContainsKey("GPS"))
                                    continue;

                                var status =
                                    double.Parse(item.items[colbuf.dflog.FindMessageOffset(item.msgtype, "Status")]);
                                var lat = double.Parse(item.items[colbuf.dflog.FindMessageOffset(item.msgtype, "Lat")]);
                                var lon = double.Parse(item.items[colbuf.dflog.FindMessageOffset(item.msgtype, "Lng")]);

                                if (lat == 0 || lon == 0 || status < 3)
                                    continue;

                                loc_list[0].Add(new PointLatLngAlt(lat, lon));

                                minx = Math.Min(minx, lon);
                                maxx = Math.Max(maxx, lon);
                                miny = Math.Min(miny, lat);
                                maxy = Math.Max(maxy, lat);
                            }

                        }
                    }
                }

                if (loc_list.Count > 0 && loc_list.First().Value.Count > 10)
                {
                    // add a bit of buffer
                    var area = RectLatLng.FromLTRB(minx - 0.001, maxy + 0.001, maxx + 0.001, miny - 0.001);
                    var map = GetMap(area);

                    var grap = Graphics.FromImage(map);

                    if (sitl)
                    {
                        AddTextToMap(grap, "SITL");
                    }

                    Color[] colours =
                    {
                        Color.Red, Color.Orange, Color.Yellow, Color.Green, Color.Blue, Color.Indigo,
                        Color.Violet, Color.Pink
                    };

                    int a = 0;
                    foreach (var locs in loc_list.Values)
                    {
                        PointF lastpoint = new PointF();
                        var pen = new Pen(colours[a%(colours.Length - 1)]);

                        foreach (var loc in locs)
                        {
                            PointF newpoint = GetPixel(area, loc, map.Size);

                            if (!lastpoint.IsEmpty)
                                grap.DrawLine(pen, lastpoint, newpoint);

                            lastpoint = newpoint;
                        }

                        a++;
                    }

                    map.Save(logfile + ".jpg", System.Drawing.Imaging.ImageFormat.Jpeg);

                    map.Dispose();

                    map = null;
                }
                else
                {
                    DoTextMap(logfile + ".jpg", "No gps data");
                }
            }
            catch (Exception ex)
            {
                if (ex.ToString().Contains("Mavlink 0.9"))
                    DoTextMap(logfile + ".jpg", "Old log\nMavlink 0.9");

                return;
            }

        }
Esempio n. 9
0
        //FMT, 131, 43, IMU, IffffffIIf, TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp
        //FMT, 135, 43, IMU2, IffffffIIf, TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp
        //FMT, 149, 43, IMU3, IffffffIIf, TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp

        //FMT, 172, 23, ACC1, IIfff, TimeMS,TimeUS,AccX,AccY,AccZ
        //FMT, 173, 23, ACC2, IIfff, TimeMS,TimeUS,AccX,AccY,AccZ
        //FMT, 174, 23, ACC3, IIfff, TimeMS,TimeUS,AccX,AccY,AccZ
        //FMT, 175, 23, GYR1, IIfff, TimeMS,TimeUS,GyrX,GyrY,GyrZ
        //FMT, 176, 23, GYR2, IIfff, TimeMS,TimeUS,GyrX,GyrY,GyrZ
        //FMT, 177, 23, GYR3, IIfff, TimeMS,TimeUS,GyrX,GyrY,GyrZ

        private void myButton1_Click(object sender, EventArgs e)
        {
            Utilities.FFT2 fft = new FFT2();

            using (OpenFileDialog ofd = new OpenFileDialog())
            {
                ofd.Filter = "*.log;*.bin|*.log;*.bin";

                ofd.ShowDialog();

                if (!File.Exists(ofd.FileName))
                    return;

                var file =  new CollectionBuffer(File.OpenRead(ofd.FileName));

                int bins = (int) NUM_bins.Value;

                int N = 1 << bins;

                double[] datainGX = new double[N];
                double[] datainGY = new double[N];
                double[] datainGZ = new double[N];
                double[] datainAX = new double[N];
                double[] datainAY = new double[N];
                double[] datainAZ = new double[N];

                List<double[]> avg = new List<double[]>();

                // 6
                avg.Add(new double[N/2]);
                avg.Add(new double[N/2]);
                avg.Add(new double[N/2]);
                avg.Add(new double[N/2]);
                avg.Add(new double[N/2]);
                avg.Add(new double[N/2]);

                object[] datas = new object[] {datainGX, datainGY, datainGZ, datainAX, datainAY, datainAZ};
                string[] datashead = new string[]
                {"GYR1-GyrX", "GYR1-GyrY", "GYR1-GyrZ", "ACC1-AccX", "ACC1-AccY", "ACC1-AccZ"};
                Color[] color = new Color[]
                {Color.Red, Color.Green, Color.Black, Color.Violet, Color.Blue, Color.Orange};
                ZedGraphControl[] ctls = new ZedGraphControl[]
                {
                    zedGraphControl1, zedGraphControl2, zedGraphControl3, zedGraphControl4, zedGraphControl5,
                    zedGraphControl6
                };

                int samplecounta = 0;
                int samplecountg = 0;

                double lasttime = 0;
                double timedelta = 0;
                double[] freqt = null;
                double samplerate = 0;

                foreach (var item in file.GetEnumeratorType(new string[] { "ACC1", "GYR1" }))
                {
                    if (item.msgtype == "ACC1")
                    {
                        int offsetAX = file.dflog.FindMessageOffset("ACC1", "AccX");
                        int offsetAY = file.dflog.FindMessageOffset("ACC1", "AccY");
                        int offsetAZ = file.dflog.FindMessageOffset("ACC1", "AccZ");
                        int offsetTime = file.dflog.FindMessageOffset("ACC1", "TimeUS");

                        double time = double.Parse(item.items[offsetTime])/1000.0;

                        timedelta = timedelta*0.99 + (time - lasttime)*0.01;

                        // we missed gyro data
                        if (samplecounta >= N)
                            continue;

                        datainAX[samplecounta] = double.Parse(item.items[offsetAX]);
                        datainAY[samplecounta] = double.Parse(item.items[offsetAY]);
                        datainAZ[samplecounta] = double.Parse(item.items[offsetAZ]);

                        samplecounta++;

                        lasttime = time;
                    }
                    else if (item.msgtype == "GYR1")
                    {
                        int offsetGX = file.dflog.FindMessageOffset("GYR1", "GyrX");
                        int offsetGY = file.dflog.FindMessageOffset("GYR1", "GyrY");
                        int offsetGZ = file.dflog.FindMessageOffset("GYR1", "GyrZ");
                        int offsetTime = file.dflog.FindMessageOffset("ACC1", "TimeUS");

                        double time = double.Parse(item.items[offsetTime])/1000.0;

                        // we missed accel data
                        if (samplecountg >= N)
                            continue;

                        datainGX[samplecountg] = double.Parse(item.items[offsetGX]);
                        datainGY[samplecountg] = double.Parse(item.items[offsetGY]);
                        datainGZ[samplecountg] = double.Parse(item.items[offsetGZ]);

                        samplecountg++;
                    }

                    if (samplecounta >= N && samplecountg >= N)
                    {
                        int inputdataindex = 0;

                        foreach (var itemlist in datas)
                        {
                            var fftanswer = fft.rin((double[]) itemlist, (uint) bins);

                            for (int b = 0; b < N/2; b++)
                            {
                                avg[inputdataindex][b] += fftanswer[b]*(1.0/(N/2.0));
                            }

                            samplecounta = 0;
                            samplecountg = 0;
                            inputdataindex++;
                        }
                    }
                }

                if (freqt == null)
                {
                    samplerate = Math.Round(1000/timedelta, 1);
                    freqt = fft.FreqTable(N, (int) samplerate);
                }

                // 0 out all data befor cutoff
                for (int inputdataindex = 0; inputdataindex < 6; inputdataindex++)
                {
                    for (int b = 0; b < N/2; b++)
                    {
                        if (freqt[b] < (double) NUM_startfreq.Value)
                        {
                            avg[inputdataindex][b] = 0;
                            continue;
                        }

                        break;
                    }
                }

                int controlindex = 0;
                foreach (var item in avg)
                {
                    ZedGraph.PointPairList ppl = new ZedGraph.PointPairList(freqt, item);

                    //double xMin, xMax, yMin, yMax;

                    var curve = new LineItem(datashead[controlindex], ppl, color[controlindex], SymbolType.None);

                    //curve.GetRange(out xMin, out xMax, out yMin, out  yMax, true, false, ctls[c].GraphPane);

                    ctls[controlindex].GraphPane.Legend.IsVisible = false;

                    ctls[controlindex].GraphPane.XAxis.Title.Text = "Freq Hz";
                    ctls[controlindex].GraphPane.YAxis.Title.Text = "Amplitude";
                    ctls[controlindex].GraphPane.Title.Text = "FFT " + datashead[controlindex] + " - " +
                                                              Path.GetFileName(ofd.FileName) + " - " + samplerate +
                                                              "hz input";

                    ctls[controlindex].GraphPane.CurveList.Clear();

                    ctls[controlindex].GraphPane.CurveList.Add(curve);

                    ctls[controlindex].Invalidate();
                    ctls[controlindex].AxisChange();

                    ctls[controlindex].Refresh();

                    controlindex++;
                }
            }
        }
Esempio n. 10
0
        private void but_ISBH_Click(object sender, EventArgs e)
        {
            Utilities.FFT2 fft = new FFT2();
            using (
                OpenFileDialog ofd = new OpenFileDialog())
            {
                ofd.Filter = "*.log;*.bin|*.log;*.bin;*.BIN;*.LOG";

                ofd.ShowDialog();

                if (!File.Exists(ofd.FileName))
                {
                    return;
                }

                var file = new CollectionBuffer(File.OpenRead(ofd.FileName));

                int bins = (int)NUM_bins.Value;

                int N = 1 << bins;

                Color[] color = new Color[]
                { Color.Red, Color.Green, Color.Blue, Color.Black, Color.Violet, Color.Orange };
                ZedGraphControl[] ctls = new ZedGraphControl[]
                {
                    zedGraphControl1, zedGraphControl2, zedGraphControl3, zedGraphControl4, zedGraphControl5,
                    zedGraphControl6
                };

                // 3 imus * 2 sets of measurements(gyr/acc)
                FFT2.datastate[] alldata = new FFT2.datastate[3 * 2];
                for (int a = 0; a < alldata.Length; a++)
                {
                    alldata[a] = new FFT2.datastate();
                }

                // state cache
                int Ns       = 0;
                int type     = 0;
                int instance = 0;
                int sensorno = 0;

                foreach (var item in file.GetEnumeratorType(new string[] { "ISBH", "ISBD" }))
                {
                    if (item.msgtype == null)
                    {
                        continue;
                    }

                    if (item.msgtype.StartsWith("ISBH"))
                    {
                        Ns = int.Parse(item.items[file.dflog.FindMessageOffset(item.msgtype, "N")],
                                       CultureInfo.InvariantCulture);
                        type = int.Parse(item.items[file.dflog.FindMessageOffset(item.msgtype, "type")],
                                         CultureInfo.InvariantCulture);
                        instance = int.Parse(item.items[file.dflog.FindMessageOffset(item.msgtype, "instance")],
                                             CultureInfo.InvariantCulture);

                        sensorno = type * 3 + instance;

                        alldata[sensorno].sample_rate = double.Parse(item.items[file.dflog.FindMessageOffset(item.msgtype, "smp_rate")],
                                                                     CultureInfo.InvariantCulture);

                        if (type == 0)
                        {
                            alldata[sensorno].type = "ACC" + instance.ToString();
                        }
                        if (type == 1)
                        {
                            alldata[sensorno].type = "GYR" + instance.ToString();
                        }
                    }
                    else if (item.msgtype.StartsWith("ISBD"))
                    {
                        var Nsdata = int.Parse(item.items[file.dflog.FindMessageOffset(item.msgtype, "N")],
                                               CultureInfo.InvariantCulture);

                        if (Ns != Nsdata)
                        {
                            continue;
                        }

                        int offsetX    = file.dflog.FindMessageOffset(item.msgtype, "x");
                        int offsetY    = file.dflog.FindMessageOffset(item.msgtype, "y");
                        int offsetZ    = file.dflog.FindMessageOffset(item.msgtype, "z");
                        int offsetTime = file.dflog.FindMessageOffset(item.msgtype, "TimeUS");

                        double time = double.Parse(item.items[offsetTime],
                                                   CultureInfo.InvariantCulture) / 1000.0;

                        if (time < alldata[sensorno].lasttime)
                        {
                            continue;
                        }

                        if (time != alldata[sensorno].lasttime)
                        {
                            alldata[sensorno].timedelta = alldata[sensorno].timedelta * 0.99 +
                                                          (time - alldata[sensorno].lasttime) * 0.01;
                        }

                        alldata[sensorno].lasttime = time;

                        item.items[offsetX].Split(new[] { ' ', '[', ']' }, StringSplitOptions.RemoveEmptyEntries).ForEach(aa => { alldata[sensorno].datax.Add(double.Parse(aa,
                                                                                                                                                                           CultureInfo.InvariantCulture)); });
                        item.items[offsetY].Split(new[] { ' ', '[', ']' }, StringSplitOptions.RemoveEmptyEntries).ForEach(aa => { alldata[sensorno].datay.Add(double.Parse(aa,
                                                                                                                                                                           CultureInfo.InvariantCulture)); });
                        item.items[offsetZ].Split(new[] { ' ', '[', ']' }, StringSplitOptions.RemoveEmptyEntries).ForEach(aa => { alldata[sensorno].dataz.Add(double.Parse(aa,
                                                                                                                                                                           CultureInfo.InvariantCulture)); });
                    }
                }

                int controlindex = 0;

                foreach (var sensordata in alldata)
                {
                    if (sensordata.datax.Count <= N)
                    {
                        continue;
                    }

                    double samplerate = 0;

                    samplerate = sensordata.sample_rate;// Math.Round(1000 / sensordata.timedelta, 1);

                    double[] freqt = fft.FreqTable(N, (int)samplerate);

                    double[] avgx = new double[N / 2];
                    double[] avgy = new double[N / 2];
                    double[] avgz = new double[N / 2];

                    int totalsamples = sensordata.datax.Count;
                    int count        = totalsamples / N;
                    int done         = 0;
                    while (count > 1) // skip last part
                    {
                        var fftanswerx = fft.rin(sensordata.datax.Skip(N * done).Take(N).ToArray(), (uint)bins);
                        var fftanswery = fft.rin(sensordata.datay.Skip(N * done).Take(N).ToArray(), (uint)bins);
                        var fftanswerz = fft.rin(sensordata.dataz.Skip(N * done).Take(N).ToArray(), (uint)bins);

                        for (int b = 0; b < N / 2; b++)
                        {
                            if (freqt[b] < (double)NUM_startfreq.Value)
                            {
                                continue;
                            }

                            avgx[b] += fftanswerx[b] / (done + count);
                            avgy[b] += fftanswery[b] / (done + count);
                            avgz[b] += fftanswerz[b] / (done + count);
                        }

                        count--;
                        done++;
                    }

                    ZedGraph.PointPairList pplx = new ZedGraph.PointPairList(freqt, avgx);
                    ZedGraph.PointPairList pply = new ZedGraph.PointPairList(freqt, avgy);
                    ZedGraph.PointPairList pplz = new ZedGraph.PointPairList(freqt, avgz);

                    var curvex = new LineItem(sensordata.type + " x", pplx, color[0], SymbolType.None);
                    var curvey = new LineItem(sensordata.type + " y", pply, color[1], SymbolType.None);
                    var curvez = new LineItem(sensordata.type + " z", pplz, color[2], SymbolType.None);

                    ctls[controlindex].GraphPane.Legend.IsVisible = true;

                    ctls[controlindex].GraphPane.XAxis.Title.Text = "Freq Hz";
                    ctls[controlindex].GraphPane.YAxis.Title.Text = "Amplitude";
                    ctls[controlindex].GraphPane.Title.Text       = "FFT " + sensordata.type + " - " +
                                                                    Path.GetFileName(ofd.FileName) + " - " + samplerate +
                                                                    "hz input";

                    ctls[controlindex].GraphPane.CurveList.Clear();

                    ctls[controlindex].GraphPane.CurveList.Add(curvex);
                    ctls[controlindex].GraphPane.CurveList.Add(curvey);
                    ctls[controlindex].GraphPane.CurveList.Add(curvez);

                    ctls[controlindex].Invalidate();
                    ctls[controlindex].AxisChange();

                    ctls[controlindex].GraphPane.XAxis.Scale.Max = samplerate / 2;

                    ctls[controlindex].Refresh();

                    controlindex++;
                }

                SetScale(ctls);
            }
        }
Esempio n. 11
0
        //FMT, 131, 43, IMU, IffffffIIf, TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp
        //FMT, 135, 43, IMU2, IffffffIIf, TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp
        //FMT, 149, 43, IMU3, IffffffIIf, TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp

        //FMT, 172, 23, ACC1, IIfff, TimeMS,TimeUS,AccX,AccY,AccZ
        //FMT, 173, 23, ACC2, IIfff, TimeMS,TimeUS,AccX,AccY,AccZ
        //FMT, 174, 23, ACC3, IIfff, TimeMS,TimeUS,AccX,AccY,AccZ
        //FMT, 175, 23, GYR1, IIfff, TimeMS,TimeUS,GyrX,GyrY,GyrZ
        //FMT, 176, 23, GYR2, IIfff, TimeMS,TimeUS,GyrX,GyrY,GyrZ
        //FMT, 177, 23, GYR3, IIfff, TimeMS,TimeUS,GyrX,GyrY,GyrZ

        private void myButton1_Click(object sender, EventArgs e)
        {
            Utilities.FFT2 fft = new FFT2();

            using (OpenFileDialog ofd = new OpenFileDialog())
            {
                ofd.Filter = "*.log;*.bin|*.log;*.bin;*.BIN;*.LOG";

                ofd.ShowDialog();

                if (!File.Exists(ofd.FileName))
                {
                    return;
                }

                var file = new CollectionBuffer(File.OpenRead(ofd.FileName));

                int bins = (int)NUM_bins.Value;

                int N = 1 << bins;

                double[] datainGX = new double[N];
                double[] datainGY = new double[N];
                double[] datainGZ = new double[N];
                double[] datainAX = new double[N];
                double[] datainAY = new double[N];
                double[] datainAZ = new double[N];

                List <double[]> avg = new List <double[]>
                {
                    // 6
                    new double[N / 2],
                    new double[N / 2],
                    new double[N / 2],
                    new double[N / 2],
                    new double[N / 2],
                    new double[N / 2]
                };

                object[] datas     = new object[] { datainGX, datainGY, datainGZ, datainAX, datainAY, datainAZ };
                string[] datashead = new string[]
                { "GYR1-GyrX", "GYR1-GyrY", "GYR1-GyrZ", "ACC1-AccX", "ACC1-AccY", "ACC1-AccZ" };
                Color[] color = new Color[]
                { Color.Red, Color.Green, Color.Black, Color.Violet, Color.Blue, Color.Orange };
                ZedGraphControl[] ctls = new ZedGraphControl[]
                {
                    zedGraphControl1, zedGraphControl2, zedGraphControl3, zedGraphControl4, zedGraphControl5,
                    zedGraphControl6
                };

                int samplecounta = 0;
                int samplecountg = 0;

                double   lasttime   = 0;
                double   timedelta  = 0;
                double[] freqt      = null;
                double   samplerate = 0;

                foreach (var item in file.GetEnumeratorType(new string[] { "ACC1", "GYR1" }))
                {
                    if (item.msgtype == "ACC1")
                    {
                        int offsetAX   = file.dflog.FindMessageOffset("ACC1", "AccX");
                        int offsetAY   = file.dflog.FindMessageOffset("ACC1", "AccY");
                        int offsetAZ   = file.dflog.FindMessageOffset("ACC1", "AccZ");
                        int offsetTime = file.dflog.FindMessageOffset("ACC1", "TimeUS");

                        double time = double.Parse(item.items[offsetTime],
                                                   CultureInfo.InvariantCulture) / 1000.0;

                        timedelta = timedelta * 0.99 + (time - lasttime) * 0.01;

                        // we missed gyro data
                        if (samplecounta >= N)
                        {
                            continue;
                        }

                        datainAX[samplecounta] = double.Parse(item.items[offsetAX],
                                                              CultureInfo.InvariantCulture);
                        datainAY[samplecounta] = double.Parse(item.items[offsetAY],
                                                              CultureInfo.InvariantCulture);
                        datainAZ[samplecounta] = double.Parse(item.items[offsetAZ],
                                                              CultureInfo.InvariantCulture);

                        samplecounta++;

                        lasttime = time;
                    }
                    else if (item.msgtype == "GYR1")
                    {
                        int offsetGX   = file.dflog.FindMessageOffset("GYR1", "GyrX");
                        int offsetGY   = file.dflog.FindMessageOffset("GYR1", "GyrY");
                        int offsetGZ   = file.dflog.FindMessageOffset("GYR1", "GyrZ");
                        int offsetTime = file.dflog.FindMessageOffset("ACC1", "TimeUS");

                        double time = double.Parse(item.items[offsetTime],
                                                   CultureInfo.InvariantCulture) / 1000.0;

                        // we missed accel data
                        if (samplecountg >= N)
                        {
                            continue;
                        }

                        datainGX[samplecountg] = double.Parse(item.items[offsetGX],
                                                              CultureInfo.InvariantCulture);
                        datainGY[samplecountg] = double.Parse(item.items[offsetGY],
                                                              CultureInfo.InvariantCulture);
                        datainGZ[samplecountg] = double.Parse(item.items[offsetGZ],
                                                              CultureInfo.InvariantCulture);

                        samplecountg++;
                    }

                    if (samplecounta >= N && samplecountg >= N)
                    {
                        int inputdataindex = 0;

                        foreach (var itemlist in datas)
                        {
                            var fftanswer = fft.rin((double[])itemlist, (uint)bins);

                            for (int b = 0; b < N / 2; b++)
                            {
                                avg[inputdataindex][b] += fftanswer[b] * (1.0 / (N / 2.0));
                            }

                            samplecounta = 0;
                            samplecountg = 0;
                            inputdataindex++;
                        }
                    }
                }

                if (freqt == null)
                {
                    samplerate = Math.Round(1000 / timedelta, 1);
                    freqt      = fft.FreqTable(N, (int)samplerate);
                }

                // 0 out all data befor cutoff
                for (int inputdataindex = 0; inputdataindex < 6; inputdataindex++)
                {
                    for (int b = 0; b < N / 2; b++)
                    {
                        if (freqt[b] < (double)NUM_startfreq.Value)
                        {
                            avg[inputdataindex][b] = 0;
                            continue;
                        }

                        break;
                    }
                }

                int controlindex = 0;
                foreach (var item in avg)
                {
                    ZedGraph.PointPairList ppl = new ZedGraph.PointPairList(freqt, item);

                    //double xMin, xMax, yMin, yMax;

                    var curve = new LineItem(datashead[controlindex], ppl, color[controlindex], SymbolType.None);

                    //curve.GetRange(out xMin, out xMax, out yMin, out  yMax, true, false, ctls[c].GraphPane);

                    ctls[controlindex].GraphPane.Legend.IsVisible = false;

                    ctls[controlindex].GraphPane.XAxis.Title.Text = "Freq Hz";
                    ctls[controlindex].GraphPane.YAxis.Title.Text = "Amplitude";
                    ctls[controlindex].GraphPane.Title.Text       = "FFT " + datashead[controlindex] + " - " +
                                                                    Path.GetFileName(ofd.FileName) + " - " + samplerate +
                                                                    "hz input";

                    ctls[controlindex].GraphPane.CurveList.Clear();

                    ctls[controlindex].GraphPane.CurveList.Add(curve);

                    ctls[controlindex].Invalidate();
                    ctls[controlindex].AxisChange();

                    ctls[controlindex].Refresh();

                    controlindex++;
                }

                SetScale(ctls);
            }
        }