Diff: filters.cpp

Revision:

0:0a76ae27065b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/filters.cpp	Wed May 05 12:10:15 2010 +0000
@@ -0,0 +1,84 @@
+/*
+mbed touchpad & accelerometer experiments.
+
+CPV, 14/09/2009
+*/
+
+
+#include <string.h>
+#include "filters.h"
+
+
+void FilterBoxF::initialise(unsigned int size)
+{
+  m_size = size;
+  if (m_size>FILTER_MAX_SIZE) m_size = FILTER_MAX_SIZE;
+  memset(m_history,0,sizeof(float)*FILTER_MAX_SIZE);
+  m_index = 0;
+  m_out = 0;
+}
+
+
+float FilterBoxF::tick(float in)
+{
+  m_history[m_index] = in;
+  m_index += 1;
+  if (m_index>=m_size) m_index = 0;
+
+  float sum = 0;
+  for (int i=0; i<m_size; i++)
+  {
+    sum += m_history[i];
+  }
+  m_out = sum/m_size;
+  return m_out;
+}
+
+//=================================
+
+void FilterBoxI::initialise(unsigned int size)
+{
+  m_size = size;
+  if (m_size>FILTER_MAX_SIZE) m_size = FILTER_MAX_SIZE;
+  memset(m_history,0,sizeof(int)*FILTER_MAX_SIZE);
+  m_index = 0;
+  m_out = 0;
+}
+
+
+int FilterBoxI::tick(int in)
+{
+  m_history[m_index] = in;
+  m_index += 1;
+  if (m_index>=m_size) m_index = 0;
+
+  int sum = 0;
+  for (int i=0; i<m_size; i++)
+  {
+    sum += m_history[i];
+  }
+  m_out = sum/m_size;
+  return m_out;
+}
+
+//=================================
+
+void FilterIirI::initialise(int tau, int limit)
+{
+  m_tau = tau;
+  m_limit = limit;
+  m_remainder = 0;
+  m_out = 0;
+}
+
+
+int FilterIirI::tick(int in)
+{
+  int temp = in - m_out + m_remainder;
+  m_remainder = temp % m_tau;
+  m_out += temp / m_tau;
+  if (m_out>m_limit) m_out = m_limit;
+  if (m_out<-m_limit) m_out = -m_limit;
+  return m_out;
+}
+