Use AverageAngleC in AeroVaneReadC.

[tinyos-2.x.git] / tos / platforms / tmirws / sensors / AeroVaneReadP.nc

diff --git a/tos/platforms/tmirws/sensors/AeroVaneReadP.nc b/tos/platforms/tmirws/sensors/AeroVaneReadP.nc
index c620006e693ccc1dbd0f114e44c6879f8c1c60f9..71d04b08b661655ad28bf04526220f6f99d51cfb 100644 (file)
--- a/tos/platforms/tmirws/sensors/AeroVaneReadP.nc
+++ b/tos/platforms/tmirws/sensors/AeroVaneReadP.nc
@@ -42,7 +42,8 @@ generic module AeroVaneReadP(uint16_t period) {
     interface Timer<TMilli>;
     interface Read<uint16_t>;
     interface StdControl as AnemometerControl;
-    interface AsyncGet<uint16_t> as Count;
+    interface Get<uint16_t> as Revolutions;
+    interface Average<uint16_t>;
     interface State;
   }
 }
@@ -54,15 +55,13 @@ implementation {
     PHYS_PER_READ = 4,         /* # of physical reads per reading */
   };
 
-  uint16_t m_count;
-  int m_vane;
-  int m_sum;
+  uint8_t m_count;
+  uint16_t m_speed;
 
-  void init()
+  inline void init()
   {
     m_count = 0;
-    m_vane = 0;
-    m_sum = 0;
+    call Average.reset();
   }
 
   command error_t Notify.enable()
@@ -87,48 +86,34 @@ implementation {
 
   event void Timer.fired()
   {
-    if (!(call State.isIdle()))
-      call Read.read();
-  }
-
-  int distance(int angle1, int angle2)
-  {
-    int d = angle2 - angle1;
+    if (++m_count == PHYS_PER_READ) {
+      /* Get anemometer revolutions for the period */
+      m_speed = call Revolutions.get();
+    }
 
-    if (d <= -180)
-      d += 360;
-    else if (d > 180)
-      d -= 360;
-    return d;
+    if (!(call State.isIdle())) {
+      /* Avoid notifying if the user has disabled */
+      call Read.read();
+    }
   }
 
   event void Read.readDone(error_t error, uint16_t result)
   {
-    int newVane;
-
-    if (error != SUCCESS)
-      return;
-
-    newVane = m_vane + distance(m_vane, result);
-    m_sum += result;
-    m_vane = newVane;
-    if (++m_count == PHYS_PER_READ) {
-      norace aerovector_t vector;
-
-      /* Get speed average */
-      atomic vector.speed = (call Count.get() * 2.25) / (period * 4 / 1024) +
-       0.5;
-
-      /* Complete direction average */
-      vector.dir = m_sum / PHYS_PER_READ;
-      while (vector.dir < 0)
-       vector.dir += 360;
-      vector.dir %= 360;
-      init();
-
-      /* Inform the consumer of the vector */
-      if (!(call State.isIdle()))
+    if (error == SUCCESS)
+      call Average.submit(result);
+
+    if (m_count == PHYS_PER_READ) {
+      if (call State.isIdle()) {
+       /* Avoid notifying if the user has disabled */
+       init();
+      } else {
+       aerovector_t vector;
+
+       vector.dir = call Average.average();
+       vector.speed = m_speed;
+       init();
        signal Notify.notify(vector);
+      }
     }
   }
 }