-/*
- * 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. The compass is a number of sectors, 0...(sectors - 1).
- * Sector zero is North, and sectors/2 is South. Like any compass, the heading
- * increases as one traverses the compass clockwise. The number of sectors are
- * contrained by the uint8_t data members of wind_vane_t to no more than 255.
- *
- * @author R. Steve McKown <rsmckown@gmail.com>
- */
-
-#include "WindVane.h"
-
-generic module WindVaneP(uint8_t sectors) {
- provides {
- interface Get<uint8_t> as Sectors;
- interface ReadRef<wind_vane_t>;
- }
- uses {
- interface Read<uint16_t> as Vane;
- interface Tick as Second;
- }
-}
-implementation {
- uint8_t m_compass[sectors];
- bool m_overflow;
- wind_vane_t* m_data;
-
-
- /*** Support functions ***/
-
-
- /* Convert a sector number into a degree heading. We use the degree value
- * that is in the center of the sector to represent that sector. For example,
- * with 32 sectors, each sector is 11.25 degrees. Sector 4 represents all
- * degree headings between 39.375 and 50.624 and is represented on the whole
- * the degree heading of 45 degrees.
- */
- uint16_t sectorToDegree(uint8_t sector)
- {
- return (360.0 / sectors) * sector;
- }
-
- /* Convert a degree heading into a sector */
- uint8_t degreeToSector(uint16_t degree)
- {
- return ((degree * sectors + 180) / 360) % sectors;
- }
-
- /* Return the # of sectors clockwise along the compass from start to end. */
- uint8_t distance(uint8_t start, uint8_t end)
- {
- return (end > start) ? end - start : (end + sectors) - start;
- }
-
- /* circularly examine compass, starting with pos + 1, for the next compass
- * position having a value of zero.
- */
- uint8_t nextNonZero(uint8_t* compass, uint8_t pos)
- {
- do {
- pos = (pos + 1) % sectors;
- } while (compass[pos] == 0);
- return pos;
- }
-
- bool isCompassEmpty(uint8_t* compass)
- {
- int i;
-
- for (i = 0; i < sectors; i++) {
- if (compass[i])
- break;
- }
- return (i == sectors) ? TRUE : FALSE;
- }
-
- /* Reduce all compass headings equally until a compass heading has a value
- * of zero.
- */
- void minimizeCompass(uint8_t* compass)
- {
- uint8_t min = 255;
- int i;
-
- for (i = 0; i < sectors; i++) {
- if (compass[i] < min)
- min = compass[i];
- }
- if (min > 0) {
- for (i = 0; i < sectors; i++)
- compass[i] -= min;
- }
- }
-
- /* Locate the arc in which the m_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(uint8_t* compass)
- {
- uint8_t begin;
- uint8_t save = 255;
- uint8_t dist = 0;
-
- begin = m_data->left = nextNonZero(compass, sectors - 1);
- do {
- uint8_t d;
-
- m_data->right = nextNonZero(compass, 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) % sectors;
- }
-
- void calcAvg(uint8_t* compass)
- {
- /* 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 + sectors;
- int i;
-
- for (i = m_data->left; i <= tmp; i++) {
- uint8_t p = i % sectors;
-
- sum += compass[p] * i;
- count += compass[p];
- }
- m_data->avg = ((sum + count - 1) / count) % sectors;
- }
-
-
- /*** Method implementations ***/
-
- command uint8_t Sectors.get()
- {
- return sectors;
- }
-
- task void startRead()
- {
- call Vane.read();
- }
-
- async event void Second.fired()
- {
- post startRead();
- }
-
- event void Vane.readDone(error_t error, uint16_t value)
- {
- if (error == SUCCESS) {
- if (++m_compass[degreeToSector(value)] == 0)
- m_overflow = TRUE;
- }
- }
-
- 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()
- {
- bool overflow;
- uint8_t compass[sectors];
- wind_vane_t* data = m_data;
-
- atomic {
- overflow = m_overflow;
- m_overflow = FALSE;
- if (!overflow)
- memcpy(compass, m_compass, sizeof(m_compass));
- memset(m_compass, 0, sizeof(m_compass));
- }
-
- if (overflow)
- m_data->left = m_data->avg = m_data->right = WINDVANE_OVERFLOW;
- else {
- minimizeCompass(compass);
- if (isCompassEmpty(compass)) {
- /* 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(compass);
- calcAvg(compass);
- }
-
- m_data = 0;
- signal ReadRef.readDone(SUCCESS, data);
- }
- }
-}