Ejemplo n.º 1
0
 public static Matrix3 rotation_df(ATT 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;
 }
Ejemplo n.º 2
0
 public static Vector3 earth_accel_df(IMU IMU, ATT 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;
 }
Ejemplo n.º 3
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 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;
        }