public void MoveFilter(FilterBase filter, CMCustomUDPData.DataKey key, int direction)
{
int index = filters[(int)key].IndexOf(filter);
int newIndex = Math.Min(filters[(int)key].Count - 1, Math.Max(0, index + direction));
filters[(int)key].RemoveAt(index);
filters[(int)key].Insert(newIndex, filter);
}
public CMChannelMap(Type _type, CMCustomUDPData.DataKey _dataKey, string _channel, int _offset, float _scale)
{
type = _type;
channel = _channel;
offset = _offset;
scale = _scale;
dataKey = _dataKey;
getBytesMethod = typeof(BitConverter).GetMethod("GetBytes", new[] { type });
fieldInfo = typeof(CMCustomUDPData).GetField(channel);
}
public FilterBase AddFilter(FilterType filterType, CMCustomUDPData.DataKey key, bool updateUI = false)
{
List <FilterBase> filterList = filters[(int)key];
if (filterList == null)
{
filterList = filters[(int)key] = new List <FilterBase>();
}
FilterBase newFilter = null;
switch (filterType)
{
case FilterType.Smooth:
{
NestedSmoothFilter newSmoothFilter = new NestedSmoothFilter();
newSmoothFilter.SetParameters(1, 3, 0.5f);
filterList.Add(newSmoothFilter);
newFilter = newSmoothFilter;
break;
}
case FilterType.Kalman:
{
KalmanNoiseFilter newKalmanFilter = new KalmanNoiseFilter();
newKalmanFilter.SetParameters(1, 1, 0.02f, 1, 0.02f, 0.0f);
filterList.Add(newKalmanFilter);
newFilter = newKalmanFilter;
break;
}
case FilterType.KalmanVelocity:
{
KalmanVelocityNoiseFilter newKalmanFilter = new KalmanVelocityNoiseFilter();
newKalmanFilter.SetParameters(1, 1, 0.02f, 1, 0.02f, 0.0f);
filterList.Add(newKalmanFilter);
newFilter = newKalmanFilter;
break;
}
case FilterType.SavitzkyGolay:
{
break;
}
case FilterType.FIR:
{
break;
}
case FilterType.Median:
{
MedianFilterWrapper newFilterW = new MedianFilterWrapper();
newFilterW.SetParameters(9);
filterList.Add(newFilterW);
newFilter = newFilterW;
break;
}
}
if (updateUI && FilterUI.Instance != null)
{
FilterUI.Instance.InitChartForKey(key);
}
return(newFilter);
}
public void DeleteFilter(FilterBase filter, CMCustomUDPData.DataKey key)
{
filters[(int)key].Remove(filter);
}
public void Filter(CMCustomUDPData dataIn, ref CMCustomUDPData dataOut, int keyMask = int.MaxValue, bool newHistory = true)
{
if (filters == null)
{
return;
}
mutex.WaitOne();
CMCustomUDPData newRawData;
CMCustomUDPData newFiltered;
if (newHistory == true)
{
dataOut.Copy(dataIn);
//copy to raw history
newRawData = new CMCustomUDPData();
newRawData.Copy(dataIn);
rawData.Add(newRawData);
if (rawData.Count > maxHistorySamples)
{
rawData.RemoveAt(0);
}
//add new filtered history
newFiltered = new CMCustomUDPData();
newFiltered.Copy(dataIn);
filteredData.Add(newFiltered);
if (filteredData.Count > maxHistorySamples)
{
filteredData.RemoveAt(0);
}
}
else
{
newFiltered = filteredData[filteredData.Count - 1];
newRawData = rawData[rawData.Count - 1];
}
//do filter lists for each key if it exists
for (int i = 0; i < filters.Length; ++i)
{
CMCustomUDPData.DataKey key = (CMCustomUDPData.DataKey)i;
List <FilterBase> filterList = filters[i];
if (!dataIn.IsValid(key))
{
continue;
}
object value = dataIn.GetValue(key);
if (filterList == null || filterList.Count == 0 || !dataIn.IsFloat(key))
{
newFiltered.SetValue(key, value);
newRawData.SetValue(key, value);
continue;
}
//ignore filter if not in keyMask
if (((1 << i) & keyMask) == 0)
{
continue;
}
//get float val
float floatVal = (float)value;
//do filters for key
foreach (FilterBase filter in filterList)
{
floatVal = filter.Filter(floatVal);
}
//filtered
newFiltered.SetValue(key, floatVal);
dataOut.SetValue(key, floatVal);
}
mutex.ReleaseMutex();
}
public void InitChartForKey(CMCustomUDPData.DataKey dataKey)
{
filterKey = dataKey;
filterChart.Invoke((Action) delegate
{
var chart = filterChart.ChartAreas[0];
chart.AxisX.Minimum = 0;
chart.AxisX.Maximum = 1;
chart.AxisY.Minimum = -chartSize;
chart.AxisY.Maximum = chartSize;
chart.AxisX.Interval = 0;
chart.AxisY.Interval = 0;
});
flowLayoutFilters.Invoke((Action) delegate
{
flowLayoutFilters.Controls.Clear();
List <FilterBase> filters = FilterModuleCustom.Instance.filters[(int)filterKey];
if (filters != null)
{
foreach (FilterBase filter in filters)
{
if (filter is NestedSmoothFilter)
{
SmoothFilterControl newControl = new SmoothFilterControl();
newControl.SetFilter((NestedSmoothFilter)filter);
flowLayoutFilters.Controls.Add(newControl);
}
else
if (filter is KalmanNoiseFilter)
{
KalmanFilterControl newControl = new KalmanFilterControl();
newControl.SetFilter((KalmanNoiseFilter)filter);
flowLayoutFilters.Controls.Add(newControl);
}
else
if (filter is MedianFilterWrapper)
{
MedianFilterControl newControl = new MedianFilterControl();
newControl.SetFilter((MedianFilterWrapper)filter);
flowLayoutFilters.Controls.Add(newControl);
}
else
if (filter is KalmanVelocityNoiseFilter)
{
KalmanVelocityFilterControl newControl = new KalmanVelocityFilterControl();
newControl.SetFilter((KalmanVelocityNoiseFilter)filter);
flowLayoutFilters.Controls.Add(newControl);
}
}
}
Button addButton = new Button();
addButton.Text = @"ADD FILTER";
addButton.Click += new System.EventHandler(this.AddButtonClick);
flowLayoutFilters.Controls.Add(addButton);
});
}
