*/
/**
- * WindVane sensor
+ * WindVane sensor. The compass is a number of sectors, 0...(sectors - 1).
+ * sectors must be a power of 2. 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 128.
*
* @author R. Steve McKown <rsmckown@gmail.com>
*/
#include "WindVane.h"
-module WindVaneP {
- provides interface ReadRef<wind_vane_t>;
+generic module WindVaneP(uint8_t sectors) {
+ provides {
+ interface Get<uint8_t> as Sectors;
+ interface ReadRef<wind_vane_t>;
+ }
uses {
- interface Tick as Second;
interface Read<uint16_t> as Vane;
- interface GeneralIO as WPower;
+ interface Tick as Second;
}
}
implementation {
- enum {
- /* Compass count should ideally be a power of 2, or at least even */
- COMPASS_COUNT = 36,
- ADC_PER_HEADING = (4096 + COMPASS_COUNT - 1) / COMPASS_COUNT,
- ADC_OFFSET = (ADC_PER_HEADING + 1) / 2,
- ADC_BITMASK = 0x0fff,
- };
-
+ uint16_t m_compass[sectors];
+ bool m_overflow;
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.
+ /* 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 */
+ uint16_t degreeToSector(uint16_t degree)
+ {
+ return ((degree * sectors + 180) / 360) % 32;
+ }
+
+ /* 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 + COMPASS_COUNT) - start;
+ return (end > start) ? end - start : (end + sectors) - start;
}
- /* circularly examine compass, starting with pos+1, for the next compass
+ /* 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)
+ uint8_t nextNonZero(uint16_t* compass, uint8_t pos)
{
do {
- pos = (pos + 1) % COMPASS_COUNT;
- } while (tc[pos] == 0);
+ pos = (pos + 1) % sectors;
+ } while (compass[pos] == 0);
return pos;
}
- bool isCompassEmpty(uint16_t* tc)
+ bool isCompassEmpty(uint16_t* compass)
{
int i;
- for (i = 0; i < COMPASS_COUNT; i++) {
- if (tc[i])
+ for (i = 0; i < sectors; i++) {
+ if (compass[i])
break;
}
- return (i == COMPASS_COUNT) ? TRUE : FALSE;
+ return (i == sectors) ? TRUE : FALSE;
}
/* Reduce all compass headings equally until a compass heading has a value
* of zero.
*/
- void minimizeCompass(uint16_t* tc)
+ void minimizeCompass(uint16_t* compass)
{
uint8_t min = 255;
int i;
- for (i = 0; i < COMPASS_COUNT; i++) {
- if (tc[i] < min)
- min = tc[i];
+ for (i = 0; i < sectors; i++) {
+ if (compass[i] < min)
+ min = compass[i];
}
if (min > 0) {
- for (i = 0; i < COMPASS_COUNT; i++)
- tc[i] -= min;
+ for (i = 0; i < sectors; i++)
+ compass[i] -= min;
}
}
- /* Locate the arc in which the compass[] has been constrained, defining
+ /* 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(uint16_t* tc)
+ void findArc(uint16_t* compass)
{
uint8_t begin;
uint8_t save = 255;
uint8_t dist = 0;
- begin = m_data->left = nextNonZero(tc, COMPASS_COUNT - 1);
+ begin = m_data->left = nextNonZero(compass, sectors - 1);
do {
uint8_t d;
- m_data->right = nextNonZero(tc, m_data->left);
+ m_data->right = nextNonZero(compass, m_data->left);
d = distance(m_data->left, m_data->right);
if (d > dist) {
dist = d;
m_data->left = m_data->right;
} while (m_data->left != begin);
m_data->right = save;
- m_data->left = (save + dist) % COMPASS_COUNT;
+ m_data->left = (save + dist) % sectors;
}
- void calcAvg(uint16_t* tc)
+ void calcAvg(uint16_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 + COMPASS_COUNT;
+ m_data->right + sectors;
int i;
for (i = m_data->left; i <= tmp; i++) {
- uint8_t p = i % COMPASS_COUNT;
+ uint8_t p = i % sectors;
- sum += tc[p] * i;
- count += tc[p];
+ sum += compass[p] * i;
+ count += compass[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;
+ m_data->avg = ((sum + count - 1) / count) % sectors;
}
/*** Method implementations ***/
+ command uint8_t Sectors.get()
+ {
+ return sectors;
+ }
+
task void startRead()
{
- call WPower.set();
call Vane.read();
}
event void Vane.readDone(error_t error, uint16_t value)
{
- //call WPower.clr();
if (error == SUCCESS) {
- /* Convert the adc value (0...4095) to a compass heading
- * (0...COMPASS_COUNT - 1).
- */
- value = (value + ADC_OFFSET) / ADC_PER_HEADING;
- while (value >= COMPASS_COUNT)
- value -= COMPASS_COUNT;
- compass[value]++;
+ if (++m_compass[degreeToSector(value)] == 0)
+ m_overflow = TRUE;
}
}
task void readCompass()
{
- uint16_t tc[COMPASS_COUNT];
+ bool overflow;
+ uint16_t compass[sectors];
wind_vane_t* data = m_data;
atomic {
- memcpy(tc, compass, sizeof(compass));
- memset(compass, 0, sizeof(compass));
+ overflow = m_overflow;
+ m_overflow = FALSE;
+ if (!overflow)
+ memcpy(compass, m_compass, sizeof(m_compass));
+ memset(m_compass, 0, sizeof(m_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. The order of calculations is to maximize precision
- * using integer arithmetic. Note that using a 16-bit unsigned field
- * for calculations, the largest value of COMPASS_COUNT is 37.
- */
- m_data->left = 1800U * m_data->left / COMPASS_COUNT / 10;
- m_data->right = 1800U * m_data->right / COMPASS_COUNT / 10;
- }
+ 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);
+ m_data = 0;
+ signal ReadRef.readDone(SUCCESS, data);
+ }
}
}
projects / tinyos-2.x.git / commitdiff