Пример #1
0
        public static double mag_heading_df(MAG_t MAG, ATT_t ATT, double declination = 0, Vector3 ofs = null,
            Vector3 diagonals = null, Vector3 offdiagonals = null)
        {
            if (diagonals == null)
                diagonals = Vector3.One;
            if (offdiagonals == null)
                offdiagonals = Vector3.Zero;
            //'''calculate heading from raw magnetometer'''
            //if (declination == 0)
            //declination = degrees(mavutil.mavfile_global.param('COMPASS_DEC', 0));
            var mag = new Vector3(MAG.MagX, MAG.MagY, MAG.MagZ);
            if (ofs != null)
            {
                mag += new Vector3(ofs[0], ofs[1], ofs[2]) - new Vector3(MAG.OfsX, MAG.OfsY, MAG.OfsZ);
                diagonals = new Vector3(diagonals[0], diagonals[1], diagonals[2]);
                offdiagonals = new Vector3(offdiagonals[0], offdiagonals[1], offdiagonals[2]);
                var rot = new Matrix3(new Vector3(diagonals.x, offdiagonals.x, offdiagonals.y),
                    new Vector3(offdiagonals.x, diagonals.y, offdiagonals.z),
                    new Vector3(offdiagonals.y, offdiagonals.z, diagonals.z));
                mag = rot*mag;
            }
//# go via a DCM matrix to match the APM calculation
            var dcm_matrix = rotation_df(ATT);
            var cos_pitch_sq = 1.0 - (dcm_matrix.c.x*dcm_matrix.c.x);
            var headY = mag.y*dcm_matrix.c.z - mag.z*dcm_matrix.c.y;
            var headX = mag.x*cos_pitch_sq - dcm_matrix.c.x*(mag.y*dcm_matrix.c.y + mag.z*dcm_matrix.c.z);

            var heading = degrees(atan2(-headY, headX)) + declination;
            if (heading < 0)
                heading += 360;
            return heading;
        }
Пример #2
0
 public static Matrix3 rotation_df(ATT_t ATT)
 {
     //return the current DCM rotation matrix''' 
     var r = new Matrix3();
     r.from_euler(radians(ATT.Roll), radians(ATT.Pitch), radians(ATT.Yaw));
     return r;
 }
Пример #3
0
 public static Vector3 earth_accel_df(IMU_t IMU, ATT_t ATT)
 {
     //return earth frame acceleration vector from df log
     var r = rotation_df(ATT);
     var accel = new Vector3(IMU.AccX, IMU.AccY, IMU.AccZ);
     return r*accel;
 }
        public static Matrix3 rotation_df(ATT_t ATT)
        {
            //return the current DCM rotation matrix'''
            var r = new Matrix3();

            r.from_euler(radians(ATT.Roll), radians(ATT.Pitch), radians(ATT.Yaw));
            return(r);
        }
        public static Vector3 earth_accel_df(IMU_t IMU, ATT_t ATT)
        {
            //return earth frame acceleration vector from df log
            var r     = rotation_df(ATT);
            var accel = new Vector3(IMU.AccX, IMU.AccY, IMU.AccZ);

            return(r * accel);
        }
        public static double mag_heading_df(MAG_t MAG, ATT_t ATT, double declination = 0, Vector3 ofs = null,
                                            Vector3 diagonals = null, Vector3 offdiagonals            = null)
        {
            if (diagonals == null)
            {
                diagonals = Vector3.One;
            }
            if (offdiagonals == null)
            {
                offdiagonals = Vector3.Zero;
            }
            //'''calculate heading from raw magnetometer'''
            //if (declination == 0)
            //declination = degrees(mavutil.mavfile_global.param('COMPASS_DEC', 0));
            var mag = new Vector3(MAG.MagX, MAG.MagY, MAG.MagZ);

            if (ofs != null)
            {
                mag         += new Vector3(ofs[0], ofs[1], ofs[2]) - new Vector3(MAG.OfsX, MAG.OfsY, MAG.OfsZ);
                diagonals    = new Vector3(diagonals[0], diagonals[1], diagonals[2]);
                offdiagonals = new Vector3(offdiagonals[0], offdiagonals[1], offdiagonals[2]);
                var rot = new Matrix3(new Vector3(diagonals.x, offdiagonals.x, offdiagonals.y),
                                      new Vector3(offdiagonals.x, diagonals.y, offdiagonals.z),
                                      new Vector3(offdiagonals.y, offdiagonals.z, diagonals.z));
                mag = rot * mag;
            }
//# go via a DCM matrix to match the APM calculation
            var dcm_matrix   = rotation_df(ATT);
            var cos_pitch_sq = 1.0 - (dcm_matrix.c.x * dcm_matrix.c.x);
            var headY        = mag.y * dcm_matrix.c.z - mag.z * dcm_matrix.c.y;
            var headX        = mag.x * cos_pitch_sq - dcm_matrix.c.x * (mag.y * dcm_matrix.c.y + mag.z * dcm_matrix.c.z);

            var heading = degrees(atan2(-headY, headX)) + declination;

            if (heading < 0)
            {
                heading += 360;
            }
            return(heading);
        }
        public static List <Tuple <DFLog.DFItem, double> > ProcessExpression(DFLog dflog, CollectionBuffer logdata, string expression)
        {
            List <Tuple <DFLog.DFItem, double> > answer = new List <Tuple <DFLog.DFItem, double> >();

            Dictionary <string, List <string> > fieldsUsed = new Dictionary <string, List <string> >();

            var fieldmatchs = Regex.Matches(expression, @"(([A-z0-9_]{2,20})\.([A-z0-9_]+))");

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

                    if (!fieldsUsed.ContainsKey(type))
                    {
                        fieldsUsed[type] = new List <string>();
                    }

                    fieldsUsed[type].Add(field);
                }
            }

            Function f  = new Function("wrap_360(x) = (x+360) # 360");
            Function f1 = new Function("degrees(x) = x*57.295779513");
            Function f2 = new Function("atan2", new atan2());
            Function f3 = new Function("lowpass", new lowpass());
            Function f4 = new Function("delta", new deltaF());

            // fix maths operators
            var filter1 =
                expression.Replace("%", "#").Replace("**", "^").Replace(":2", "");

            //convert paramnames to remove .
            var filter2 = Regex.Replace(filter1, @"(([A-z0-9_]{2,20})\.([A-z0-9_]+))",
                                        match => match.Groups[2].ToString() + match.Groups[3]);

            // convert strings to long
            var filter3 = Regex.Replace(filter2, @"([""']{1}[^""]+[""']{1})",
                                        match => BitConverter.ToUInt64(match.Groups[0].Value.MakeBytesSize(8), 0).ToString());

            Expression e = new Expression(filter3);

            e.addFunctions(f);
            e.addFunctions(f1);
            e.addFunctions(f2);
            e.addFunctions(f3);
            e.addFunctions(f4);

            foreach (var item in fieldsUsed)
            {
                foreach (var value in item.Value)
                {
                    Argument x = new Argument(item.Key + "" + value);
                    e.addArguments(x);
                }
            }

            bool bad = false;

            try
            {
                mXparser.consolePrintTokens(e.getCopyOfInitialTokens());

                if (!e.checkSyntax())
                {
                    bad = true;
                }
            }
            catch
            {
                bad = true;
            }

            if (!bad)
            {
                foreach (var line in logdata.GetEnumeratorType(fieldsUsed.Keys.ToArray()))
                {
                    foreach (var item in fieldsUsed)
                    {
                        foreach (var value in item.Value.Distinct())
                        {
                            e.setArgumentValue(item.Key + "" + value,
                                               double.Parse(line.items[dflog.FindMessageOffset(item.Key, value)]));
                        }
                    }

                    answer.Add(line, e.calculate());
                }
            }

            if (answer.Count > 0)
            {
                return(answer);
            }

            //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());
                    }
                }

                IMU_t IMU = new IMU_t();
                ATT_t ATT = new ATT_t();

                foreach (var item in logdata.GetEnumeratorType(msglist.ToArray()))
                {
                    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, earth_accel_df(IMU, ATT).x);
                    }
                    if (expression.Contains(".y"))
                    {
                        answer.Add(item, earth_accel_df(IMU, ATT).y);
                    }
                    if (expression.Contains(".z"))
                    {
                        answer.Add(item, 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_t();

                    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, delta(gps_velocity_df(GPS).x, "x", item.timems * 1000));
                    }
                    else if (expression.Contains(".y"))
                    {
                        answer.Add(item, delta(gps_velocity_df(GPS).y, "y", item.timems * 1000));
                    }
                    else if (expression.Contains(".z"))
                    {
                        answer.Add(item, delta(gps_velocity_df(GPS).z, "z", item.timems * 1000) - 9.8);
                    }
                }
            }
            else if (expression.Contains("delta(gps_velocity_df(GPS2)"))
            {
                foreach (var item in logdata.GetEnumeratorType("GPS2"))
                {
                    var GPS = new GPS_t();

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

                    if (expression.Contains(".x"))
                    {
                        answer.Add(item, delta(gps_velocity_df(GPS).x, "x", item.timems * 1000));
                    }
                    else if (expression.Contains(".y"))
                    {
                        answer.Add(item, delta(gps_velocity_df(GPS).y, "y", item.timems * 1000));
                    }
                    else if (expression.Contains(".z"))
                    {
                        answer.Add(item, 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, 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 List <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;
                            }
                            var    ans        = logdata.GetUnit(key, value);
                            string unit       = ans.Item1;
                            double multiplier = ans.Item2;
                            work[a] = double.Parse(item.items[offset]) * multiplier;
                        }



                        double workanswer = 0;
                        foreach (var value in work.Values)
                        {
                            workanswer += Math.Pow(value, 2);
                        }
                        answer.Add(item, Math.Sqrt(workanswer));
                    }
                }
            }
            else if (expression.Contains("*")) // ATT.DesRoll*ATT.Roll
            {
                var matchs = Regex.Matches(expression, @"([A-z0-9_]+)\.([A-z0-9_]+)\*([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();

                    var type2  = matchs[0].Groups[3].Value.ToString();
                    var field2 = matchs[0].Groups[4].Value.ToString();

                    foreach (var item in logdata.GetEnumeratorType(new[] { type, type2 }))
                    {
                        if (type == type2)
                        {
                            var idx1 = dflog.FindMessageOffset(type, field);
                            var idx2 = dflog.FindMessageOffset(type2, field2);
                            if (idx1 == -1 || idx2 == -1)
                            {
                                break;
                            }
                            answer.Add(item,
                                       double.Parse(item.items[idx1]) *
                                       double.Parse(item.items[idx2]));
                        }
                    }
                }
            }
            else if (expression.Contains("-")) // ATT.DesRoll-ATT.Roll
            {
                var matchs = Regex.Matches(expression, @"([A-z0-9_]+)\.([A-z0-9_]+)-([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();

                    var type2  = matchs[0].Groups[3].Value.ToString();
                    var field2 = matchs[0].Groups[4].Value.ToString();

                    foreach (var item in logdata.GetEnumeratorType(new[] { type, type2 }))
                    {
                        if (type == type2)
                        {
                            var idx1 = dflog.FindMessageOffset(type, field);
                            var idx2 = dflog.FindMessageOffset(type2, field2);
                            if (idx1 == -1 || idx2 == -1)
                            {
                                break;
                            }
                            answer.Add(item,
                                       double.Parse(item.items[idx1]) -
                                       double.Parse(item.items[idx2]));
                        }
                    }
                }
            }
            else if (expression.Contains("mag_heading_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());
                    }
                }

                var MAG = new MAG_t();
                var ATT = new ATT_t();

                foreach (var item in logdata.GetEnumeratorType(msglist.ToArray()))
                {
                    if (item.msgtype.StartsWith("MAG"))
                    {
                        MAG.MagX = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "MagX")]);
                        MAG.MagY = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "MagY")]);
                        MAG.MagZ = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "MagZ")]);
                        MAG.OfsX = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "OfsX")]);
                        MAG.OfsY = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "OfsY")]);
                        MAG.OfsZ = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "OfsZ")]);
                    }
                    else 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")]);
                    }

                    answer.Add(item, mag_heading_df(MAG, ATT));
                }
            }


            return(answer);
        }
Пример #8
0
        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_t IMU = new IMU_t();
                    ATT_t ATT = new ATT_t();

                    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_t();

                    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.Contains("delta(gps_velocity_df(GPS2)"))
            {
                foreach (var item in logdata.GetEnumeratorType("GPS2"))
                {
                    var GPS = new GPS_t();

                    if (item.msgtype == "GPS2")
                    {
                        GPS.Spd  = double.Parse(item.items[dflog.FindMessageOffset("GPS2", "Spd")]);
                        GPS.GCrs = double.Parse(item.items[dflog.FindMessageOffset("GPS2", "GCrs")]);
                        GPS.VZ   = double.Parse(item.items[dflog.FindMessageOffset("GPS2", "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));
                    }
                }
            }
            else if (expression.Contains("-")) // ATT.DesRoll-ATT.Roll
            {
                var matchs = Regex.Matches(expression, @"([A-z0-9_]+)\.([A-z0-9_]+)-([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();

                    var type2  = matchs[0].Groups[3].Value.ToString();
                    var field2 = matchs[0].Groups[4].Value.ToString();

                    foreach (var item in logdata.GetEnumeratorType(new[] { type, type2 }))
                    {
                        if (type == type2)
                        {
                            answer.Add(item.lineno,
                                       double.Parse(item.items[dflog.FindMessageOffset(type, field)]) -
                                       double.Parse(item.items[dflog.FindMessageOffset(type2, field2)]));
                        }
                    }
                }
            }
            else if (expression.Contains("mag_heading_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());
                    }
                }

                var MAG = new MAG_t();
                var ATT = new ATT_t();

                foreach (var item in logdata.GetEnumeratorType(msglist.ToArray()))
                {
                    if (item.msgtype.StartsWith("MAG"))
                    {
                        MAG.MagX = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "MagX")]);
                        MAG.MagY = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "MagY")]);
                        MAG.MagZ = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "MagZ")]);
                        MAG.OfsX = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "OfsX")]);
                        MAG.OfsY = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "OfsY")]);
                        MAG.OfsZ = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "OfsZ")]);
                    }
                    else 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")]);
                    }

                    answer.Add(item.lineno, mag_heading_df(MAG, ATT));
                }
            }


            return(answer);
        }
Пример #9
0
        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_t IMU = new IMU_t();
                    ATT_t ATT = new ATT_t();

                    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_t();

                    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.Contains("delta(gps_velocity_df(GPS2)"))
            {
                foreach (var item in logdata.GetEnumeratorType("GPS2"))
                {
                    var GPS = new GPS_t();

                    if (item.msgtype == "GPS2")
                    {
                        GPS.Spd = double.Parse(item.items[dflog.FindMessageOffset("GPS2", "Spd")]);
                        GPS.GCrs = double.Parse(item.items[dflog.FindMessageOffset("GPS2", "GCrs")]);
                        GPS.VZ = double.Parse(item.items[dflog.FindMessageOffset("GPS2", "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));
                    }
                }
            }
            else if (expression.Contains("-")) // ATT.DesRoll-ATT.Roll
            {
                var matchs = Regex.Matches(expression, @"([A-z0-9_]+)\.([A-z0-9_]+)-([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();

                    var type2 = matchs[0].Groups[3].Value.ToString();
                    var field2 = matchs[0].Groups[4].Value.ToString();

                    foreach (var item in logdata.GetEnumeratorType(new[] {type, type2}))
                    {
                        if (type == type2)
                            answer.Add(item.lineno,
                                double.Parse(item.items[dflog.FindMessageOffset(type, field)]) -
                                double.Parse(item.items[dflog.FindMessageOffset(type2, field2)]));
                    }
                }
            }
            else if (expression.Contains("mag_heading_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());
                    }
                }

                var MAG = new MAG_t();
                var ATT = new ATT_t();

                foreach (var item in logdata.GetEnumeratorType(msglist.ToArray()))
                {
                    if (item.msgtype.StartsWith("MAG"))
                    {
                        MAG.MagX = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "MagX")]);
                        MAG.MagY = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "MagY")]);
                        MAG.MagZ = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "MagZ")]);
                        MAG.OfsX = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "OfsX")]);
                        MAG.OfsY = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "OfsY")]);
                        MAG.OfsZ = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "OfsZ")]);
                    }
                    else 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")]);
                    }

                    answer.Add(item.lineno, mag_heading_df(MAG, ATT));
                }
            }


            return answer;
        }