+/*
+ * Copyright (c) 2008, Titanium Mirror, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * - Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ * - Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * - Neither the name of Titanium Mirror, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+ * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
+ * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+ * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
+ * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ * Average angles representing positions on a circle. 360 degrees represented
+ * as a 10 bit unsigned value.
+ *
+ * @author R. Steve McKown <rsmckown@gmail.com>
+ */
+
+generic module AverageAngleC(uint16_t samples) {
+ provides interface Average<uint16_t>;
+}
+implementation {
+ uint8_t m_count;
+ int16_t m_angle;
+ uint16_t m_lastAngle;
+
+ inline void init()
+ {
+ m_count = 0;
+ m_angle = 0;
+ }
+
+ command void Average.reset()
+ {
+ init();
+ }
+
+ task void signalAverage()
+ {
+ int16_t angle;
+
+ if (m_angle > 0)
+ angle = (m_angle + (m_count / 2)) / m_count;
+ else {
+ angle = (m_angle - (m_count / 2)) / m_count;
+ while (m_angle < 0)
+ angle += 1024;
+ }
+ angle &= 0x03ff; /* %= 1024 */
+ init();
+
+ /* Inform the consumer of the average angle */
+ signal Average.average(angle);
+ }
+
+ int distance(int angle1, int angle2)
+ {
+ int d = angle2 - angle1;
+
+ if (d <= -512)
+ d += 1024;
+ else if (d > 512)
+ d -= 1024;
+ return d;
+ }
+
+ command void Average.submit(uint16_t angle)
+ {
+ m_lastAngle += distance(m_lastAngle, angle);
+ m_angle += m_lastAngle;
+ if (++m_count == samples)
+ post signalAverage();
+ }
+
+ command void Average.null()
+ {
+ if (++m_count == samples)
+ post signalAverage();
+ }
+}