diff --git a/LogViewer/LogViewer/Controls/ChartGroup.cs b/LogViewer/LogViewer/Controls/ChartGroup.cs index 0cc4b1c15b..fa9ba0b88a 100644 --- a/LogViewer/LogViewer/Controls/ChartGroup.cs +++ b/LogViewer/LogViewer/Controls/ChartGroup.cs @@ -14,7 +14,6 @@ namespace LogViewer.Controls public class ChartGroup : Grid { private bool scaleIndependently; - private int scaleIndex = 0; public bool ScaleIndependently { diff --git a/LogViewer/LogViewer/Controls/SimpleLineChart.xaml.cs b/LogViewer/LogViewer/Controls/SimpleLineChart.xaml.cs index db3c537c13..b58876cb2f 100644 --- a/LogViewer/LogViewer/Controls/SimpleLineChart.xaml.cs +++ b/LogViewer/LogViewer/Controls/SimpleLineChart.xaml.cs @@ -527,7 +527,7 @@ private void AddScaledValues(PathFigure figure, int start, int end) double rx = pt.X + offset; - if (pt.X > 0 && pt.X < width) + if (pt.X >= 0 && pt.X < width) { visibleCount++; if (!started) diff --git a/LogViewer/LogViewer/Model/JSonDataLog.cs b/LogViewer/LogViewer/Model/JSonDataLog.cs index db21c7e7f3..2f4ed22d56 100644 --- a/LogViewer/LogViewer/Model/JSonDataLog.cs +++ b/LogViewer/LogViewer/Model/JSonDataLog.cs @@ -56,9 +56,9 @@ public async Task Load(string file, ProgressUtility progress) bool hasStartTime = false; this.startTime = DateTime.MinValue; - DateTime? gpsStartTime = null; - ulong gpsAbsoluteOffset = 0; - ulong logStartTime = 0; + //DateTime? gpsStartTime = null; + //ulong gpsAbsoluteOffset = 0; + //ulong logStartTime = 0; List rows = new List(); diff --git a/LogViewer/LogViewer/Model/Px4ULog.cs b/LogViewer/LogViewer/Model/Px4ULog.cs index 896fe31781..0eb408aa70 100644 --- a/LogViewer/LogViewer/Model/Px4ULog.cs +++ b/LogViewer/LogViewer/Model/Px4ULog.cs @@ -48,6 +48,40 @@ public override string ToString() return typeName + (arraySize > 0 ? "[" + arraySize + "] " : " ") + name; } + public static string GetFieldTypeName(FieldType f) + { + switch (f) + { + case FieldType.Float: + return "float"; + case FieldType.Double: + return "double"; + case FieldType.Int8: + return "int8_t"; + case FieldType.Bool: + return "bool"; + case FieldType.UInt8: + return "uint8_t"; + case FieldType.Int16: + return "int16_t"; + case FieldType.UInt16: + return "uint16_t"; + case FieldType.Int32: + return "int32_t"; + case FieldType.UInt32: + return "uint32_t"; + case FieldType.Int64: + return "int64_t"; + case FieldType.UInt64: + return "uint64_t"; + case FieldType.Char: + return "char"; + case FieldType.Struct: + break; + } + throw new Exception("Unexpected FieldType"); + } + public MessageField(string definition) { string[] parts = definition.Split(' '); @@ -190,6 +224,7 @@ public MessageLogging(byte logLevel, long timestamp, string msg) class MessageData : Message { internal MessageSubscription subscription; + internal MessageFormat format; UInt16 msgId; byte[] value; Dictionary values; @@ -199,6 +234,7 @@ public MessageData(UInt16 msgId, byte[] value, MessageSubscription s) this.msgId = msgId; this.value = value; this.subscription = s; + this.format = s.format; } internal DataValue GetValue(MessageField field) @@ -230,10 +266,14 @@ internal DataValue GetValue(MessageField field) private void ParseValues() { - values = new Dictionary(); BinaryReader reader = new BinaryReader(new MemoryStream(value)); + ReadValues(reader); + } - foreach (var field in subscription.format.fields) + private void ReadValues(BinaryReader reader) + { + values = new Dictionary(); + foreach (var field in this.format.fields) { object value = null; if (field.arraySize > 0) @@ -249,13 +289,14 @@ private void ParseValues() { value = ReadField(reader, field); } + field.value = value; values[field.name] = value; } } private object ReadField(BinaryReader reader, MessageField field) { - object o = null; + object o = null; switch (field.type) { case FieldType.Float: @@ -295,11 +336,126 @@ private object ReadField(BinaryReader reader, MessageField field) o = (char)reader.ReadByte(); break; case FieldType.Struct: - // todo + { + MessageFormat f = field.structType; + if (f == null) + { + throw new Exception(string.Format("Unexpected FieldType.Struct in field {0}", field.name)); + } + MessageData s = new MessageData(this.msgId, null, this.subscription); + s.format = f; + s.ReadValues(reader); + o = s; + } break; } return o; } + + internal MessageData GetNestedData(string fieldName) + { + if (values == null) + { + ParseValues(); + } + + foreach (var field in format.fields) + { + if (field.name == fieldName) + { + if (field.arraySize > 0) + { + // cook up a MessageData just for the array. + MessageData array = new MessageData(this.msgId, null, this.subscription); + StringBuilder fieldTypes = new StringBuilder(); + fieldTypes.Append(fieldName); + fieldTypes.Append(':'); + + var values = new Dictionary(); + bool first = true; + object ts = null; + // we need to copy the timestamp down so the data renders with time sync. + if (this.values.TryGetValue("timestamp", out ts)) + { + fieldTypes.Append("double timestamp"); + values["timestamp"] = ts; + first = false; + } + + Array data = (Array)field.value; + for (int i = 0; i < field.arraySize; i++) + { + if (!first) + { + fieldTypes.Append(';'); + } + fieldTypes.Append(MessageField.GetFieldTypeName(field.type)); + fieldTypes.Append(' '); + fieldTypes.Append(i.ToString()); + values[i.ToString()] = data.GetValue(i); + first = false; + } + array.format = new MessageFormat(fieldTypes.ToString()); + array.values = values; + return array; + } + else if (field.value is MessageData) + { + return (MessageData)field.value; + } + else + { + throw new Exception(string.Format("Field {0} is not a struct", fieldName)); + } + } + } + return null; + } + + internal T GetValue(string fieldName) where T : IComparable + { + if (values == null) + { + ParseValues(); + } + + foreach (var field in format.fields) + { + if (field.name == fieldName) + { + object o = null; + if (values.TryGetValue(field.name, out o)) + { + if (typeof(T) == typeof(UInt64)) + { + object i = Convert.ToUInt64(o); + return (T)i; + } + if (typeof(T) == typeof(float)) + { + object i = Convert.ToSingle(o); + return (T)i; + } + if (typeof(T) == typeof(double)) + { + object i = Convert.ToDouble(o); + return (T)i; + } + if (typeof(T) == typeof(int)) + { + object i = Convert.ToInt32(o); + return (T)i; + } + if (typeof(T) == typeof(byte)) + { + object i = Convert.ToByte(o); + return (T)i; + } + } + } + } + return default(T); + } } class MessageInfo : Message @@ -382,7 +538,7 @@ enum ULogMessageType SYNC = 'S', DROPOUT = 'O', LOGGING = 'L', - }; + }; BinaryReader reader; TimeSpan duration; @@ -428,17 +584,35 @@ public IEnumerable GetDataValues(LogItemSchema schema, DateTime start MessageData data = (MessageData)m; if (data.subscription.id == root.Id) { + MessageFormat f = data.format; // matching root schema, so drill down if necessary. for (int i = 1, n = path.Count; i < n; i++) { + bool found = false; LogItemSchema child = path[i]; - foreach (var field in data.subscription.format.fields) + foreach (var field in f.fields) { if (field.name == child.Name) { - yield return data.GetValue(field); + found = true; + if (i + 1 < n && child.HasChildren) + { + // still need to drill down, so we need a MessageData and MessageFormat for the child item. + data = data.GetNestedData(field.name); + f = data.format; + } + else + { + yield return data.GetValue(field); + found = false; // done drilling down + } + break; } } + if (!found) + { + break; + } } } } @@ -452,7 +626,25 @@ public IEnumerable GetFlights() public IEnumerable GetRows(string typeName, DateTime startTime, TimeSpan duration) { - throw new NotImplementedException(); + if (typeName == "GPS") + { + // convert the ulog "vehicleglobal_position" to a GPS entry. + MessageFormat format = null; + if (formats.TryGetValue("vehicle_global_position", out format)) + { + foreach (var m in msgs) + { + if (m is MessageData) + { + MessageData data = (MessageData)m; + if (data.format == format) + { + yield return new vehicleglobal_position(data); + } + } + } + } + } } public DateTime GetTime(ulong timeMs) @@ -528,13 +720,23 @@ LogItemSchema CreateSchemaItem(MessageSubscription sub, MessageFormat fmt) LogItemSchema element = new LogItemSchema() { Name = fmt.name, Parent = schema, Id = sub.id }; foreach (var f in fmt.fields) { - LogItemSchema column = new LogItemSchema() { Name = f.name, Parent = element, Type = f.typeName + (f.arraySize > 0 ? "[" + f.arraySize + "]" : "") }; + LogItemSchema column = new LogItemSchema() { Name = f.name, Parent = element, Type = f.typeName + (f.arraySize > 0 ? "[" + f.arraySize + "]" : ""), Id = sub.id }; if (f.type == FieldType.Struct) { // nested var child = CreateSchemaItem(sub, f.structType); column.ChildItems = child.ChildItems; } + else if (f.arraySize > 0) + { + List arrayItems = new List(); + // break out the elements of the array as separate items. + for (int i = 0; i < f.arraySize; i++) + { + arrayItems.Add(new LogItemSchema() { Name = i.ToString(), Parent = column, Type = f.typeName, Id = sub.id }); + } + column.ChildItems = arrayItems; + } if (element.ChildItems == null) { element.ChildItems = new List(); @@ -690,6 +892,22 @@ public MessageDropOut ReadDropOutMessage(ushort len) UInt16 duration = reader.ReadUInt16(); return new MessageDropOut(duration); } + + private class vehicleglobal_position : LogEntry + { + public vehicleglobal_position(MessageData m) + { + SetField("GPSTime", m.GetValue("timestamp")); + SetField("EPH", m.GetValue("eph")); + SetField("EPV", m.GetValue("epv")); + SetField("Lat", m.GetValue("lat")); + SetField("Lon", m.GetValue("lon")); + SetField("Alt", m.GetValue("alt")); + SetField("VelN", m.GetValue("vel_n")); + SetField("VelE", m.GetValue("vel_e")); + SetField("VelD", m.GetValue("vel_d")); + } + } } static class BinaryReaderExtensions diff --git a/LogViewer/LogViewer/Utilities/Generated.cs b/LogViewer/LogViewer/Utilities/Generated.cs index b5708a1969..592f4cba75 100644 --- a/LogViewer/LogViewer/Utilities/Generated.cs +++ b/LogViewer/LogViewer/Utilities/Generated.cs @@ -56,6 +56,10 @@ class LogEntryGPS public UInt16 N; public UInt16 J; + public LogEntryGPS() + { + } + public LogEntryGPS(LogEntry entry) { if (entry.HasField("TimeUS")) diff --git a/Unreal/Plugins/AirSim/Source/FlyingPawn.cpp b/Unreal/Plugins/AirSim/Source/FlyingPawn.cpp index ddb99219ec..3b5b21c4d9 100644 --- a/Unreal/Plugins/AirSim/Source/FlyingPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/FlyingPawn.cpp @@ -93,8 +93,12 @@ APIPCamera* AFlyingPawn::getFpvCamera() void AFlyingPawn::setRotorSpeed(int rotor_index, float radsPerSec) { - if (rotor_index >= 0 && rotor_index < rotor_count) - rotating_movements_[rotor_index]->RotationRate.Yaw = radsPerSec * 180.0f / M_PIf * RotatorFactor; + if (rotor_index >= 0 && rotor_index < rotor_count) { + auto comp = rotating_movements_[rotor_index]; + if (comp != nullptr) { + comp->RotationRate.Yaw = radsPerSec * 180.0f / M_PIf * RotatorFactor; + } + } } std::string AFlyingPawn::getVehicleName() diff --git a/Unreal/Plugins/AirSim/Source/MultiRotorConnector.cpp b/Unreal/Plugins/AirSim/Source/MultiRotorConnector.cpp index 479609eac9..d5a3f916f0 100644 --- a/Unreal/Plugins/AirSim/Source/MultiRotorConnector.cpp +++ b/Unreal/Plugins/AirSim/Source/MultiRotorConnector.cpp @@ -167,11 +167,11 @@ void MultiRotorConnector::update(real_T dt) void MultiRotorConnector::reportState(StateReporter& reporter) { // report actual location in unreal coordinates so we can plug that into the UE editor to move the drone. - FVector unrealPosition = vehicle_pawn_->getPosition(); - reporter.writeValue("unreal pos", AVehiclePawnBase::toVector3r(unrealPosition, 1.0f, false)); - - vehicle_.reportState(reporter); - + if (vehicle_pawn_ != nullptr) { + FVector unrealPosition = vehicle_pawn_->getPosition(); + reporter.writeValue("unreal pos", AVehiclePawnBase::toVector3r(unrealPosition, 1.0f, false)); + vehicle_.reportState(reporter); + } } MultiRotorConnector::UpdatableObject* MultiRotorConnector::getPhysicsBody()