From 84e71a52b9848426abe8b1b0e3269fc9a9efd7ec Mon Sep 17 00:00:00 2001 From: smckown Date: Mon, 9 Nov 2009 19:30:56 +0000 Subject: [PATCH] Add the averaging components and their interface. --- tos/platforms/tmirws/sensors/Average.nc | 56 +++++++++++ tos/platforms/tmirws/sensors/AverageAngleC.nc | 98 +++++++++++++++++++ tos/platforms/tmirws/sensors/AveragePolarC.nc | 52 ++++++++++ tos/platforms/tmirws/sensors/AveragePolarP.nc | 82 ++++++++++++++++ tos/platforms/tmirws/sensors/AverageU16C.nc | 79 +++++++++++++++ 5 files changed, 367 insertions(+) create mode 100644 tos/platforms/tmirws/sensors/Average.nc create mode 100644 tos/platforms/tmirws/sensors/AverageAngleC.nc create mode 100644 tos/platforms/tmirws/sensors/AveragePolarC.nc create mode 100644 tos/platforms/tmirws/sensors/AveragePolarP.nc create mode 100644 tos/platforms/tmirws/sensors/AverageU16C.nc diff --git a/tos/platforms/tmirws/sensors/Average.nc b/tos/platforms/tmirws/sensors/Average.nc new file mode 100644 index 00000000..cccb75a8 --- /dev/null +++ b/tos/platforms/tmirws/sensors/Average.nc @@ -0,0 +1,56 @@ +/* + * 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 + */ + +interface Average { + /* 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); +} diff --git a/tos/platforms/tmirws/sensors/AverageAngleC.nc b/tos/platforms/tmirws/sensors/AverageAngleC.nc new file mode 100644 index 00000000..52d03ac5 --- /dev/null +++ b/tos/platforms/tmirws/sensors/AverageAngleC.nc @@ -0,0 +1,98 @@ +/* + * 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 + */ + +generic module AverageAngleC(uint16_t samples) { + provides interface Average; +} +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(); + } +} diff --git a/tos/platforms/tmirws/sensors/AveragePolarC.nc b/tos/platforms/tmirws/sensors/AveragePolarC.nc new file mode 100644 index 00000000..982056d0 --- /dev/null +++ b/tos/platforms/tmirws/sensors/AveragePolarC.nc @@ -0,0 +1,52 @@ +/* + * 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 + */ + +#include "AeroVane.h" + +generic configuration AveragePolarC(uint16_t samples) { + provides interface Average; +} +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; +} diff --git a/tos/platforms/tmirws/sensors/AveragePolarP.nc b/tos/platforms/tmirws/sensors/AveragePolarP.nc new file mode 100644 index 00000000..cf49de69 --- /dev/null +++ b/tos/platforms/tmirws/sensors/AveragePolarP.nc @@ -0,0 +1,82 @@ +/* + * 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 + */ + +#include "AeroVane.h" + +generic module AveragePolarC(uint16_t samples) { + provides interface Average; + uses { + interface Average as AverageAngle; + interface Average 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); + } +} diff --git a/tos/platforms/tmirws/sensors/AverageU16C.nc b/tos/platforms/tmirws/sensors/AverageU16C.nc new file mode 100644 index 00000000..85daac5c --- /dev/null +++ b/tos/platforms/tmirws/sensors/AverageU16C.nc @@ -0,0 +1,79 @@ +/* + * 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 + */ + +#include "AeroVane.h" + +generic module AverageU16(uint16_t samples) { + provides interface Average; +} +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(); + } +} -- 2.39.2