From 69fc4c14eb331474d8c6654eebcf4147b7780ec8 Mon Sep 17 00:00:00 2001
From: smckown <smckown@4bc1554a-c7f2-4f65-a403-e0be01f0239c>
Date: Mon, 9 Nov 2009 19:31:01 +0000
Subject: [PATCH] Use AverageAngleC in AeroVaneReadC.

---
 tos/platforms/tmirws/sensors/AeroVaneReadC.nc |  3 ++
 tos/platforms/tmirws/sensors/AeroVaneReadP.nc | 54 +++++++------------
 2 files changed, 21 insertions(+), 36 deletions(-)

diff --git a/tos/platforms/tmirws/sensors/AeroVaneReadC.nc b/tos/platforms/tmirws/sensors/AeroVaneReadC.nc
index 5ba23a9f..8475bdac 100644
--- a/tos/platforms/tmirws/sensors/AeroVaneReadC.nc
+++ b/tos/platforms/tmirws/sensors/AeroVaneReadC.nc
@@ -53,6 +53,9 @@ implementation {
   AeroVaneReadP.AnemometerControl -> AnemometerReadC.StdControl;
   AeroVaneReadP.Revolutions -> AnemometerReadC;
 
+  components new AverageAngleC();
+  AeroVaneReadP.Average -> AverageAngleC;
+
   components new StateC();
   AeroVaneReadP.State -> StateC;
 }
diff --git a/tos/platforms/tmirws/sensors/AeroVaneReadP.nc b/tos/platforms/tmirws/sensors/AeroVaneReadP.nc
index 68be7c67..71d04b08 100644
--- a/tos/platforms/tmirws/sensors/AeroVaneReadP.nc
+++ b/tos/platforms/tmirws/sensors/AeroVaneReadP.nc
@@ -43,6 +43,7 @@ generic module AeroVaneReadP(uint16_t period) {
     interface Read<uint16_t>;
     interface StdControl as AnemometerControl;
     interface Get<uint16_t> as Revolutions;
+    interface Average<uint16_t>;
     interface State;
   }
 }
@@ -56,14 +57,11 @@ implementation {
 
   uint8_t m_count;
   uint16_t m_speed;
-  int m_vane;
-  int m_sum;
 
-  void init()
+  inline void init()
   {
     m_count = 0;
-    m_vane = 0;
-    m_sum = 0;
+    call Average.reset();
   }
 
   command error_t Notify.enable()
@@ -93,45 +91,29 @@ implementation {
       m_speed = call Revolutions.get();
     }
 
-    if (!(call State.isIdle()))
+    if (!(call State.isIdle())) {
+      /* Avoid notifying if the user has disabled */
       call Read.read();
-  }
-
-  int distance(int angle1, int angle2)
-  {
-    int d = angle2 - angle1;
-
-    if (d <= -512)
-      d += 1024;
-    else if (d > 512)
-      d -= 1024;
-    return d;
+    }
   }
 
   event void Read.readDone(error_t error, uint16_t result)
   {
-    int newVane;
-
-    if (error != SUCCESS)
-      return;
+    if (error == SUCCESS)
+      call Average.submit(result);
 
-    newVane = m_vane + distance(m_vane, result);
-    m_sum += m_vane;
-    m_vane = newVane;
     if (m_count == PHYS_PER_READ) {
-      aerovector_t vector;
-
-      vector.speed = m_speed;
-      if (m_sum > 0)
-	vector.dir = (m_sum + (PHYS_PER_READ / 2)) / PHYS_PER_READ;
-      else
-	vector.dir = (m_sum - (PHYS_PER_READ / 2)) / PHYS_PER_READ + 1024;
-      vector.dir &= 0x03ff; /* %= 1024 */
-      init();
-
-      /* Inform the consumer of the vector */
-      if (!(call State.isIdle()))
+      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);
+      }
     }
   }
 }
-- 
2.39.2