- 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;
+ 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 */