diff --git a/ExtLibs/Mavlink/MAVLinkParam.cs b/ExtLibs/Mavlink/MAVLinkParam.cs index abfc13f469..5f1c3962d8 100644 --- a/ExtLibs/Mavlink/MAVLinkParam.cs +++ b/ExtLibs/Mavlink/MAVLinkParam.cs @@ -82,11 +82,12 @@ private MAVLinkParam() /// /// /// - public MAVLinkParam(string name, double value, MAV_PARAM_TYPE wiretype) + public MAVLinkParam(string name, double value, MAV_PARAM_TYPE wiretype, decimal? _default_value = null) { Name = name; Type = wiretype; Value = value; + default_value = _default_value; } /// diff --git a/Log/LogBrowse.cs b/Log/LogBrowse.cs index e37320ceb5..c2049f7c73 100644 --- a/Log/LogBrowse.cs +++ b/Log/LogBrowse.cs @@ -667,6 +667,26 @@ private void ResetTreeView(List seenmessagetypes, string VehicleType) MainV2.comPort.MAV.param.Clear(); MainV2.comPort.MAV.param.AddRange(parmdata); + // If we are not currently connected to a vehicle, then use VehicleType to set CurrentState firmware + // (we probably aren't connected to a vehicle when reviewing a log, but if we are, we don't want to override this) + if (!MainV2.comPort.BaseStream.IsOpen) + { + var firmware_lookup = new Dictionary() + { + {"ArduPlane", Firmwares.ArduPlane}, + {"ArduCopter", Firmwares.ArduCopter2}, + {"Blimp", Firmwares.ArduCopter2}, + {"ArduRover",Firmwares.ArduRover}, + {"ArduSub",Firmwares.ArduSub}, + {"AntennaTracker", Firmwares.ArduTracker} + }; + var match = firmware_lookup.Where(d => VehicleType.StartsWith(d.Key)).ToArray(); + if (match.Count() == 1) + { + MainV2.comPort.MAV.cs.firmware = match[0].Value; + } + } + var sorted = new SortedList(dflog.logformat); // go through all fmt's foreach (DFLog.Label item in sorted.Values) @@ -3738,32 +3758,33 @@ private void chk_params_CheckedChanged(object sender, EventArgs e) chk_params.Checked = false; - var parmdata = logdata.GetEnumeratorType("PARM").Select(a => - new MAVLink.MAVLinkParam(a["Name"], double.Parse(a["Value"], CultureInfo.InvariantCulture), - MAVLink.MAV_PARAM_TYPE.REAL32)); - - //Check the list for duplicates and use the latest occurence for value - MAVLink.MAVLinkParamList newparamdata = new MAVLink.MAVLinkParamList(); - foreach (MAVLink.MAVLinkParam sourceItem in parmdata) + MAVLink.MAVLinkParamList paramdata = new MAVLink.MAVLinkParamList(); + bool has_defaults = logdata.dflog.logformat["PARM"].FieldNames.Contains("Default"); + foreach (var msg in logdata.GetEnumeratorType("PARM")) { - //Lookup the next item in the target list - for (var idx_target = 0; idx_target < newparamdata.Count(); idx_target++) + double value = double.Parse(msg["Value"], CultureInfo.InvariantCulture); + decimal tmp; + decimal? default_value = has_defaults && decimal.TryParse(msg["Default"], out tmp) ? (decimal?)tmp : null; + MAVLink.MAVLinkParam sourceItem = new MAVLink.MAVLinkParam(msg["Name"], value, MAVLink.MAV_PARAM_TYPE.REAL32, default_value); + + // Lookup the next item in the target list + for (var idx_target = 0; idx_target < paramdata.Count(); idx_target++) { - if (sourceItem.Name == newparamdata[idx_target].Name) + if (sourceItem.Name == paramdata[idx_target].Name) { - //This item is already in the parameters, since source is in time order, it means the value changed - //We can replace the item in the output with this new value - Console.WriteLine("Duplicated item Name:{0} Prev:{1} New:{2}", sourceItem.Name, newparamdata[idx_target].Value, sourceItem.Value); - newparamdata[idx_target] = sourceItem; + // This item is already in the parameters, since source is in time order, it means the value changed. + // We can replace the item in the output with this new value + Console.WriteLine("Duplicated item Name:{0} Prev:{1} New:{2}", sourceItem.Name, paramdata[idx_target].Value, sourceItem.Value); + paramdata[idx_target] = sourceItem; break; } } - //item is not in target list, we can add it - newparamdata.Add(sourceItem); + // The item is not in target list, we can add it + paramdata.Add(sourceItem); } MainV2.comPort.MAV.param.Clear(); - MainV2.comPort.MAV.param.AddRange(newparamdata); + MainV2.comPort.MAV.param.AddRange(paramdata); var frm = new ConfigRawParams().ShowUserControl(); }