interface Read<uint16_t>;
interface StdControl as AnemometerControl;
interface Get<uint16_t> as Revolutions;
+ interface Average<uint16_t>;
interface State;
}
}
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()
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);
+ }
}
}
}