--- /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.
+ */
+
+/**
+ * WindVane sensor
+ *
+ * @author R. Steve McKown <rsmckown@gmail.com>
+ */
+
+#include "WindVane.h"
+
+module WindVaneP {
+ provides interface ReadRef<wind_vane_t>;
+ uses {
+ interface Tick as Second;
+ interface AsyncGet<uint8_t> as Vane;
+ }
+}
+implementation {
+#define COMPASS_COUNT 32
+
+ wind_vane_t* m_data;
+ /* compass[0] = North, 0 degrees,
+ * compass(COMPASS_COUNT/2) = South, 180 degrees.
+ */
+ uint16_t compass[COMPASS_COUNT];
+
+
+ /*** Support functions ***/
+
+
+ /* return the number of positions clockwise along the compass from start
+ * to end.
+ */
+ uint8_t distance(uint8_t start, uint8_t end)
+ {
+ return (end > start) ? end - start : (end + COMPASS_COUNT) - start;
+ }
+
+ /* circularly examine compass, starting with pos+1, for the next compass
+ * position having a value of zero.
+ */
+ uint8_t nextNonZero(uint16_t* tc, uint8_t pos)
+ {
+ do {
+ pos = (pos + 1) % COMPASS_COUNT;
+ } while (tc[pos] == 0);
+ return pos;
+ }
+
+ bool isCompassEmpty(uint16_t* tc)
+ {
+ int i;
+
+ for (i = 0; i < COMPASS_COUNT; i++) {
+ if (tc[i])
+ break;
+ }
+ return (i == COMPASS_COUNT) ? TRUE : FALSE;
+ }
+
+ /* Reduce all compass headings equally until a compass heading has a value
+ * of zero.
+ */
+ void minimizeCompass(uint16_t* tc)
+ {
+ uint8_t min = 255;
+ int i;
+
+ for (i = 0; i < COMPASS_COUNT; i++) {
+ if (tc[i] < min)
+ min = tc[i];
+ }
+ if (min > 0) {
+ for (i = 0; i < COMPASS_COUNT; i++)
+ tc[i] -= min;
+ }
+ }
+
+ /* Locate the arc in which the compass[] has been constrained, defining
+ * that arc in m_data's left and right fields. The compass arc is always
+ * defined clockwise, from left to right.
+ */
+ void findArc(uint16_t* tc)
+ {
+ uint8_t begin;
+ uint8_t save = 255;
+ uint8_t dist = 0;
+
+ begin = m_data->left = nextNonZero(tc, COMPASS_COUNT - 1);
+ do {
+ uint8_t d;
+
+ m_data->right = nextNonZero(tc, m_data->left);
+ d = distance(m_data->left, m_data->right);
+ if (d > dist) {
+ dist = d;
+ save = m_data->left;
+ }
+ m_data->left = m_data->right;
+ } while (m_data->left != begin);
+ m_data->right = save;
+ m_data->left = (save + dist) % COMPASS_COUNT;
+ }
+
+ void calcAvg(uint16_t* tc)
+ {
+ /* Find the average compass heading */
+ uint32_t sum = 0;
+ uint32_t count = 0;
+ uint32_t tmp = (m_data->right >= m_data->left) ? m_data->right :
+ m_data->right + COMPASS_COUNT;
+ int i;
+
+ for (i = m_data->left; i <= tmp; i++) {
+ uint8_t p = i % COMPASS_COUNT;
+
+ sum += tc[p] * i;
+ count += tc[p];
+ }
+ /* Reduce the sum by whole compass arcs, since only the remaining
+ * partial arc is of relevance.
+ */
+ tmp = COMPASS_COUNT * count;
+ while (sum >= tmp)
+ sum -= tmp;
+ m_data->avg = (sum == 0) ? 0 : (1800 / COMPASS_COUNT) * sum / count / 10;
+ }
+
+
+ /*** Method implementations ***/
+
+
+ async event void Second.fired()
+ {
+ const static uint8_t lookup[] = {
+ 0x01, 0x03, 0x02, 0x06, 0x04, 0x0c, 0x08, 0x18,
+ 0x10, 0x30, 0x20, 0x60, 0x40, 0xc0, 0x80, 0x81
+ };
+ uint8_t wind;
+ uint8_t i = 0;
+
+ wind = call Vane.get();
+ for (i = 0; i < sizeof(lookup); i++) {
+ if (lookup[i] == wind) {
+ compass[i]++;
+ return;
+ }
+ }
+ }
+
+ task void readCompass();
+
+ command error_t ReadRef.read(wind_vane_t* data)
+ {
+ if (!data)
+ return EINVAL;
+ else if (m_data)
+ return EBUSY;
+ else {
+ m_data = data;
+ post readCompass();
+ return SUCCESS;
+ }
+ }
+
+ task void readCompass()
+ {
+ uint16_t tc[COMPASS_COUNT];
+ wind_vane_t* data = m_data;
+
+ atomic {
+ memcpy(tc, compass, sizeof(compass));
+ memset(compass, 0, sizeof(compass));
+ }
+
+ minimizeCompass(tc);
+ if (isCompassEmpty(tc)) {
+ /* If there were no readings in the compass, or each compass heading has
+ * a number of readings equal to all other compass headings, we have an
+ * indeterminate wind direction.
+ */
+ m_data->left = m_data->avg = m_data->right = WINDVANE_NO_HEADING;
+ } else {
+ findArc(tc);
+ calcAvg(tc);
+
+ /* m_data's left, avg and right fields are currently represented in
+ * compass positions. We must now convert those fields into units of
+ * angular degrees.
+ */
+ m_data->left = 1800U / COMPASS_COUNT * m_data->left / 10;
+ m_data->right = 1800U / COMPASS_COUNT * m_data->right / 10;
+ }
+
+ m_data = 0;
+ signal ReadRef.readDone(SUCCESS, data);
+ }
+}
projects / tinyos-2.x.git / blobdiff