- 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();