--- /dev/null
+/*
+ * 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.
+ */
+
+/**
+ * An interface used by averagers.
+ *
+ * @author R. Steve McKown <rsmckown@gmail.com>
+ */
+
+interface Average<val_t> {
+ /* Reset the averager. Any samples submitted since the last average was
+ * signalled, or all samples submitted if average was not signalled, are
+ * discarded.
+ */
+ command void reset();
+
+ /* Submit a value to the averager, to be averaged into the next average. */
+ command void submit(val_t);
+
+ /* Submit a null value to the averager. Each null submitted counts as one
+ * of the expected values for calculating the average. Each null submitted
+ * reduces the divisor for the subsequent average by one.
+ */
+ command void null();
+
+ /* Signalled after the expected number of samples + nulls have been delivered
+ * and their set have been used to calculate a new average.
+ */
+ event void average(val_t);
+}
--- /dev/null
+/*
+ * 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();
+ }
+}
--- /dev/null
+/*
+ * 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 vectors in polar coordinate space. This is not a true vector
+ * average, but instead creates an resultant polar coordinate that has an
+ * independent average of theta and magnitude (speed).
+ *
+ * @author R. Steve McKown <rsmckown@gmail.com>
+ */
+
+#include "AeroVane.h"
+
+generic configuration AveragePolarC(uint16_t samples) {
+ provides interface Average<aerovector_t>;
+}
+implementation {
+ components new AveragePolarP(samples) as AverageP;
+ Average = AverageP;
+
+ components new AverageAngleC(samples);
+ AverageP.AverageAngle -> AverageAngleC.Average;
+
+ components new AveragePolarC(samples);
+ AverageP.AverageSpeed -> AverageU16C.Average;
+}
--- /dev/null
+/*
+ * 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 vectors in polar coordinate space. This is not a true vector
+ * average, but instead creates an resultant polar coordinate that has an
+ * independent average of theta and magnitude (speed).
+ *
+ * @author R. Steve McKown <rsmckown@gmail.com>
+ */
+
+#include "AeroVane.h"
+
+generic module AveragePolarC(uint16_t samples) {
+ provides interface Average<aerovector_t>;
+ uses {
+ interface Average<int16_t> as AverageAngle;
+ interface Average<uint16_t> as AverageSpeed;
+ }
+}
+implementation {
+ int m_count;
+ aerovector_t vector;
+
+ command void Average.reset()
+ {
+ AverageAngle.init();
+ AverageSpeed.init();
+ }
+
+ event void AverageAngle.average(uint16_t angle)
+ {
+ vector.dir = angle;
+ if (++m_count == 2)
+ signal Average.average(vector);
+ }
+
+ event void AverageSpeed.average(uint16_t speed)
+ {
+ vector.speed = speed;
+ if (++m_count == 2)
+ signal Average.average(vector);
+ }
+
+ command void Average.submit(aerovector_t vector)
+ {
+ call AverageAngle.submit(vector.dir);
+ call AverageSpeed.submit(vector.speed);
+ }
+
+ command void Average.null()
+ {
+ call AverageAngle.submit(vector.dir);
+ call AverageSpeed.submit(vector.speed);
+ }
+}
--- /dev/null
+/*
+ * 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 scalar values.
+ *
+ * @author R. Steve McKown <rsmckown@gmail.com>
+ */
+
+#include "AeroVane.h"
+
+generic module AverageU16(uint16_t samples) {
+ provides interface Average<aerovector_t>;
+}
+implementation {
+ uint8_t m_count;
+ uint16_t m_value;
+
+ inline void init()
+ {
+ m_count = 0;
+ m_value = 0;
+ }
+
+ command void Average.reset()
+ {
+ init();
+ }
+
+ task void signalAverage()
+ {
+ uint16_t value;
+
+ value = (m_value + (m_count / 2)) / m_count;
+ init();
+
+ /* Inform the consumer of the vector */
+ signal Average.average(vector);
+ }
+
+ command Average.submit(uint16_t value)
+ {
+ m_value += value;
+ if (++m_count == samples)
+ post signalAverage();
+ }
+
+ command Average.null()
+ {
+ if (++m_count == samples)
+ post signalAverage();
+ }
+}