--- /dev/null
+/*
+ * Copyright (c) 2002, Vanderbilt University
+ * All rights reserved.
+ *
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation for any purpose, without fee, and without written agreement is
+ * hereby granted, provided that the above copyright notice, the following
+ * two paragraphs and the author appear in all copies of this software.
+ *
+ * IN NO EVENT SHALL THE VANDERBILT UNIVERSITY BE LIABLE TO ANY PARTY FOR
+ * DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
+ * OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE VANDERBILT
+ * UNIVERSITY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * THE VANDERBILT UNIVERSITY SPECIFICALLY DISCLAIMS ANY WARRANTIES,
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
+ * ON AN "AS IS" BASIS, AND THE VANDERBILT UNIVERSITY HAS NO OBLIGATION TO
+ * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
+ *
+ * @author: Miklos Maroti, Brano Kusy (kusy@isis.vanderbilt.edu)
+ * Ported to T2: 3/17/08 by Brano Kusy (branislav.kusy@gmail.com)
+ */
+
+interface GlobalTime<precision_tag>
+{
+ /**
+ * Returns the current local time of this mote.
+ */
+ async command uint32_t getLocalTime();
+
+ /**
+ * Reads the current global time. This method is a combination
+ * of <code>getLocalTime</code> and <code>local2Global</code>.
+ * @return SUCCESS if this mote is synchronized, FAIL otherwise.
+ */
+ async command error_t getGlobalTime(uint32_t *time);
+
+ /**
+ * Converts the local time given in <code>time</code> into the
+ * corresponding global time and stores this again in
+ * <code>time</code>. The following equation is used to compute the
+ * conversion:
+ *
+ * globalTime = localTime + offset + skew * (localTime - syncPoint)
+ *
+ * The skew is normalized to 0.0 (1.0 is subtracted) to increase the
+ * machine precision. The syncPoint value is periodically updated to
+ * increase the machine precision of the floating point arithmetic and
+ * also to allow time wrap.
+ *
+ * @return SUCCESS if this mote is synchronized, FAIL otherwise.
+ */
+ async command error_t local2Global(uint32_t *time);
+
+ /**
+ * Converts the global time given in <code>time</code> into the
+ * correspoding local time and stores this again in
+ * <code>time</code>. This method performs the inverse of the
+ * <code>local2Global</clode> transformation.
+ *
+ * @return SUCCESS if this mote is synchronized, FAIL otherwise.
+ */
+ async command error_t global2Local(uint32_t *time);
+}
--- /dev/null
+/*
+ * Copyright (c) 2002, Vanderbilt University
+ * All rights reserved.
+ *
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation for any purpose, without fee, and without written agreement is
+ * hereby granted, provided that the above copyright notice, the following
+ * two paragraphs and the author appear in all copies of this software.
+ *
+ * IN NO EVENT SHALL THE VANDERBILT UNIVERSITY BE LIABLE TO ANY PARTY FOR
+ * DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
+ * OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE VANDERBILT
+ * UNIVERSITY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * THE VANDERBILT UNIVERSITY SPECIFICALLY DISCLAIMS ANY WARRANTIES,
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
+ * ON AN "AS IS" BASIS, AND THE VANDERBILT UNIVERSITY HAS NO OBLIGATION TO
+ * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
+ *
+ * Author: Miklos Maroti, Brano Kusy, Janos Sallai
+ * Date last modified: 3/17/03
+ * Ported to T2: 3/17/08 by Brano Kusy (branislav.kusy@gmail.com)
+ */
+
+#include "TimeSyncMsg.h"
+
+configuration TimeSyncC
+{
+ uses interface Boot;
+ provides interface Init;
+ provides interface StdControl;
+ provides interface GlobalTime<TMilli>;
+
+ //interfaces for extra fcionality: need not to be wired
+ provides interface TimeSyncInfo;
+ provides interface TimeSyncMode;
+ provides interface TimeSyncNotify;
+}
+
+implementation
+{
+ components new TimeSyncP(TMilli);
+
+ GlobalTime = TimeSyncP;
+ StdControl = TimeSyncP;
+ Init = TimeSyncP;
+ Boot = TimeSyncP;
+ TimeSyncInfo = TimeSyncP;
+ TimeSyncMode = TimeSyncP;
+ TimeSyncNotify = TimeSyncP;
+
+ components TimeSyncMessageC as ActiveMessageC;
+ TimeSyncP.RadioControl -> ActiveMessageC;
+ TimeSyncP.Send -> ActiveMessageC.TimeSyncAMSendMilli[AM_TIMESYNCMSG];
+ TimeSyncP.Receive -> ActiveMessageC.Receive[AM_TIMESYNCMSG];
+ TimeSyncP.TimeSyncPacket -> ActiveMessageC;
+
+ components HilTimerMilliC;
+ TimeSyncP.LocalTime -> HilTimerMilliC;
+
+ components new TimerMilliC() as TimerC;
+ TimeSyncP.Timer -> TimerC;
+
+#if defined(TIMESYNC_LEDS)
+ components LedsC;
+#else
+ components NoLedsC as LedsC;
+#endif
+ TimeSyncP.Leds -> LedsC;
+
+}
--- /dev/null
+/*
+ * Copyright (c) 2002, Vanderbilt University
+ * All rights reserved.
+ *
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation for any purpose, without fee, and without written agreement is
+ * hereby granted, provided that the above copyright notice, the following
+ * two paragraphs and the author appear in all copies of this software.
+ *
+ * IN NO EVENT SHALL THE VANDERBILT UNIVERSITY BE LIABLE TO ANY PARTY FOR
+ * DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
+ * OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE VANDERBILT
+ * UNIVERSITY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * THE VANDERBILT UNIVERSITY SPECIFICALLY DISCLAIMS ANY WARRANTIES,
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
+ * ON AN "AS IS" BASIS, AND THE VANDERBILT UNIVERSITY HAS NO OBLIGATION TO
+ * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
+ *
+ * @author: Miklos Maroti, Brano Kusy (kusy@isis.vanderbilt.edu)
+ * Ported to T2: 3/17/08 by Brano Kusy (branislav.kusy@gmail.com)
+ */
+
+interface TimeSyncInfo
+{
+ /**
+ * Returns current offset of the local time wrt global time.
+ */
+ async command uint32_t getOffset();
+
+ /**
+ * Returns current skew of the local time wrt global time.
+ * This value is normalized to 0.0 (1.0 is subtracted) to get maximum
+ * representation precision.
+ */
+ async command float getSkew();
+
+ /**
+ * Returns the local time of the last synchronization point. This
+ * value is close to the current local time and updated when a new
+ * time synchronization message arrives.
+ */
+ async command uint32_t getSyncPoint();
+
+ /**
+ * Returns the current root to which this node is synchronized.
+ */
+ async command uint16_t getRootID();
+
+ /**
+ * Returns the latest seq number seen from the current root.
+ */
+ async command uint8_t getSeqNum();
+
+ /**
+ * Returns the number of entries stored currently in the
+ * regerssion table.
+ */
+ async command uint8_t getNumEntries();
+
+ /**
+ * Returns the value of heartBeats variable.
+ */
+ async command uint8_t getHeartBeats();
+}
--- /dev/null
+/*
+ * Copyright (c) 2002, Vanderbilt University
+ * All rights reserved.
+ *
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation for any purpose, without fee, and without written agreement is
+ * hereby granted, provided that the above copyright notice, the following
+ * two paragraphs and the author appear in all copies of this software.
+ *
+ * IN NO EVENT SHALL THE VANDERBILT UNIVERSITY BE LIABLE TO ANY PARTY FOR
+ * DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
+ * OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE VANDERBILT
+ * UNIVERSITY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * THE VANDERBILT UNIVERSITY SPECIFICALLY DISCLAIMS ANY WARRANTIES,
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
+ * ON AN "AS IS" BASIS, AND THE VANDERBILT UNIVERSITY HAS NO OBLIGATION TO
+ * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
+ *
+ * @author: Miklos Maroti, Brano Kusy (kusy@isis.vanderbilt.edu)
+ * Ported to T2: 3/17/08 by Brano Kusy (branislav.kusy@gmail.com)
+ */
+
+/**
+ * the time sync module can work in two modes:
+ * - TS_TIMER_MODE (default): TS msgs sent period. from the timer
+ * - TS_USER_MODE: TS msgs sent only when explic. asked by user
+ * via TimeSyncMode.send() command, TimeSync.Timer
+ * is stopped in this mode
+ */
+
+interface TimeSyncMode
+{
+ /**
+ * Sets the current mode of the TimeSync module.
+ * returns FAIL if didn't succeed
+ */
+ command error_t setMode(uint8_t mode);
+
+ /**
+ * Gets the current mode of the TimeSync module.
+ */
+ command uint8_t getMode();
+
+ /**
+ * command to send out time synchronization message.
+ * returns FAIL if TimeSync not in TS_USER_MODE
+ */
+ command error_t send();
+
+ }
+
+
--- /dev/null
+/*
+ * Copyright (c) 2002, Vanderbilt University
+ * All rights reserved.
+ *
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation for any purpose, without fee, and without written agreement is
+ * hereby granted, provided that the above copyright notice, the following
+ * two paragraphs and the author appear in all copies of this software.
+ *
+ * IN NO EVENT SHALL THE VANDERBILT UNIVERSITY BE LIABLE TO ANY PARTY FOR
+ * DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
+ * OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE VANDERBILT
+ * UNIVERSITY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * THE VANDERBILT UNIVERSITY SPECIFICALLY DISCLAIMS ANY WARRANTIES,
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
+ * ON AN "AS IS" BASIS, AND THE VANDERBILT UNIVERSITY HAS NO OBLIGATION TO
+ * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
+ *
+ * @author: Miklos Maroti, Brano Kusy (kusy@isis.vanderbilt.edu)
+ * Ported to T2: 3/17/08 by Brano Kusy (branislav.kusy@gmail.com)
+ */
+
+#if defined(TIMESYNCMSG_H)
+#else
+#define TIMESYNCMSG_H
+
+typedef nx_struct TimeSyncMsg
+{
+ nx_uint16_t rootID; // the node id of the synchronization root
+ nx_uint16_t nodeID; // the node if of the sender
+ nx_uint8_t seqNum; // sequence number for the root
+
+ /* This field is initially set to the offset between global time and local
+ * time. The TimeStamping component will add the current local time when the
+ * message is actually transmitted. Thus the receiver will receive the
+ * global time of the sender when the message is actually sent. */
+ nx_uint32_t globalTime;
+
+ //just for convenience
+ nx_uint32_t localTime;
+} TimeSyncMsg;
+
+enum {
+ AM_TIMESYNCMSG = 0xAA,
+ TIMESYNCMSG_LEN = sizeof(TimeSyncMsg) - sizeof(nx_uint32_t),
+ TS_TIMER_MODE = 0, // see TimeSyncMode interface
+ TS_USER_MODE = 1, // see TimeSyncMode interface
+};
+
+#endif
--- /dev/null
+/*
+ * Copyright (c) 2002, Vanderbilt University
+ * All rights reserved.
+ *
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation for any purpose, without fee, and without written agreement is
+ * hereby granted, provided that the above copyright notice, the following
+ * two paragraphs and the author appear in all copies of this software.
+ *
+ * IN NO EVENT SHALL THE VANDERBILT UNIVERSITY BE LIABLE TO ANY PARTY FOR
+ * DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
+ * OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE VANDERBILT
+ * UNIVERSITY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * THE VANDERBILT UNIVERSITY SPECIFICALLY DISCLAIMS ANY WARRANTIES,
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
+ * ON AN "AS IS" BASIS, AND THE VANDERBILT UNIVERSITY HAS NO OBLIGATION TO
+ * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
+ *
+ * @author: Miklos Maroti, Brano Kusy (kusy@isis.vanderbilt.edu)
+ * Ported to T2: 3/17/08 by Brano Kusy (branislav.kusy@gmail.com)
+ */
+
+/**
+ * time sync module (TimeSyncM) provides notification of arriving
+ * and transmitted time-sync msgs through TimeSyncNotify interface:
+ */
+
+interface TimeSyncNotify
+{
+ /**
+ * fired when time-sync msg is received and accepted
+ */
+ event void msg_received();
+
+ /**
+ * fired when time-sync msg is sent by TimeSyncM or the sending did not
+ * succeed
+ */
+ event void msg_sent();
+
+ }
+
+
--- /dev/null
+/*
+ * Copyright (c) 2002, Vanderbilt University
+ * All rights reserved.
+ *
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation for any purpose, without fee, and without written agreement is
+ * hereby granted, provided that the above copyright notice, the following
+ * two paragraphs and the author appear in all copies of this software.
+ *
+ * IN NO EVENT SHALL THE VANDERBILT UNIVERSITY BE LIABLE TO ANY PARTY FOR
+ * DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
+ * OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE VANDERBILT
+ * UNIVERSITY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * THE VANDERBILT UNIVERSITY SPECIFICALLY DISCLAIMS ANY WARRANTIES,
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
+ * ON AN "AS IS" BASIS, AND THE VANDERBILT UNIVERSITY HAS NO OBLIGATION TO
+ * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
+ *
+ * @author: Miklos Maroti, Brano Kusy (kusy@isis.vanderbilt.edu), Janos Sallai
+ * Ported to T2: 3/17/08 by Brano Kusy (branislav.kusy@gmail.com)
+ */
+#include "TimeSyncMsg.h"
+
+generic module TimeSyncP(typedef precision_tag)
+{
+ provides
+ {
+ interface Init;
+ interface StdControl;
+ interface GlobalTime<precision_tag>;
+
+ //interfaces for extra functionality: need not to be wired
+ interface TimeSyncInfo;
+ interface TimeSyncMode;
+ interface TimeSyncNotify;
+ }
+ uses
+ {
+ interface Boot;
+ interface SplitControl as RadioControl;
+ interface TimeSyncAMSend<precision_tag,uint32_t> as Send;
+ interface Receive;
+ interface Timer<TMilli>;
+ interface Leds;
+ interface TimeSyncPacket<precision_tag,uint32_t>;
+ interface LocalTime<precision_tag> as LocalTime;
+ }
+}
+implementation
+{
+#ifndef TIMESYNC_RATE
+#define TIMESYNC_RATE 10
+#endif
+
+ enum {
+ MAX_ENTRIES = 8, // number of entries in the table
+ BEACON_RATE = TIMESYNC_RATE, // how often send the beacon msg (in seconds)
+ ROOT_TIMEOUT = 5, //time to declare itself the root if no msg was received (in sync periods)
+ IGNORE_ROOT_MSG = 4, // after becoming the root ignore other roots messages (in send period)
+ ENTRY_VALID_LIMIT = 4, // number of entries to become synchronized
+ ENTRY_SEND_LIMIT = 3, // number of entries to send sync messages
+ ENTRY_THROWOUT_LIMIT = 100, // if time sync error is bigger than this clear the table
+ };
+
+ typedef struct TableItem
+ {
+ uint8_t state;
+ uint32_t localTime;
+ int32_t timeOffset; // globalTime - localTime
+ } TableItem;
+
+ enum {
+ ENTRY_EMPTY = 0,
+ ENTRY_FULL = 1,
+ };
+
+ TableItem table[MAX_ENTRIES];
+ uint8_t tableEntries;
+
+ enum {
+ STATE_IDLE = 0x00,
+ STATE_PROCESSING = 0x01,
+ STATE_SENDING = 0x02,
+ STATE_INIT = 0x04,
+ };
+
+ uint8_t state, mode;
+
+/*
+ We do linear regression from localTime to timeOffset (globalTime - localTime).
+ This way we can keep the slope close to zero (ideally) and represent it
+ as a float with high precision.
+
+ timeOffset - offsetAverage = skew * (localTime - localAverage)
+ timeOffset = offsetAverage + skew * (localTime - localAverage)
+ globalTime = localTime + offsetAverage + skew * (localTime - localAverage)
+*/
+
+ float skew;
+ uint32_t localAverage;
+ int32_t offsetAverage;
+ uint8_t numEntries; // the number of full entries in the table
+
+ message_t processedMsgBuffer;
+ message_t* processedMsg;
+
+ message_t outgoingMsgBuffer;
+ TimeSyncMsg* outgoingMsg;
+
+ uint8_t heartBeats; // the number of sucessfully sent messages
+ // since adding a new entry with lower beacon id than ours
+
+ async command uint32_t GlobalTime.getLocalTime()
+ {
+ return call LocalTime.get();
+ }
+
+ async command error_t GlobalTime.getGlobalTime(uint32_t *time)
+ {
+ *time = call GlobalTime.getLocalTime();
+ return call GlobalTime.local2Global(time);
+ }
+
+ error_t is_synced()
+ {
+ if (numEntries>=ENTRY_VALID_LIMIT || outgoingMsg->rootID==TOS_NODE_ID)
+ return SUCCESS;
+ else
+ return FAIL;
+ }
+
+
+ async command error_t GlobalTime.local2Global(uint32_t *time)
+ {
+ *time += offsetAverage + (int32_t)(skew * (int32_t)(*time - localAverage));
+ return is_synced();
+ }
+
+ async command error_t GlobalTime.global2Local(uint32_t *time)
+ {
+ uint32_t approxLocalTime = *time - offsetAverage;
+ *time = approxLocalTime - (int32_t)(skew * (int32_t)(approxLocalTime - localAverage));
+ return is_synced();
+ }
+
+ void calculateConversion()
+ {
+ float newSkew = skew;
+ uint32_t newLocalAverage;
+ int32_t newOffsetAverage;
+
+ int64_t localSum;
+ int64_t offsetSum;
+
+ int8_t i;
+
+ for(i = 0; i < MAX_ENTRIES && table[i].state != ENTRY_FULL; ++i)
+ ;
+
+ if( i >= MAX_ENTRIES ) // table is empty
+ return;
+/*
+ We use a rough approximation first to avoid time overflow errors. The idea
+ is that all times in the table should be relatively close to each other.
+*/
+ newLocalAverage = table[i].localTime;
+ newOffsetAverage = table[i].timeOffset;
+
+ localSum = 0;
+ offsetSum = 0;
+
+ while( ++i < MAX_ENTRIES )
+ if( table[i].state == ENTRY_FULL ) {
+ localSum += (int32_t)(table[i].localTime - newLocalAverage) / tableEntries;
+ offsetSum += (int32_t)(table[i].timeOffset - newOffsetAverage) / tableEntries;
+ }
+
+ newLocalAverage += localSum;
+ newOffsetAverage += offsetSum;
+
+ localSum = offsetSum = 0;
+ for(i = 0; i < MAX_ENTRIES; ++i)
+ if( table[i].state == ENTRY_FULL ) {
+ int32_t a = table[i].localTime - newLocalAverage;
+ int32_t b = table[i].timeOffset - newOffsetAverage;
+
+ localSum += (int64_t)a * a;
+ offsetSum += (int64_t)a * b;
+ }
+
+ if( localSum != 0 )
+ newSkew = (float)offsetSum / (float)localSum;
+
+ atomic
+ {
+ skew = newSkew;
+ offsetAverage = newOffsetAverage;
+ localAverage = newLocalAverage;
+ numEntries = tableEntries;
+ }
+ }
+
+ void clearTable()
+ {
+ int8_t i;
+ for(i = 0; i < MAX_ENTRIES; ++i)
+ table[i].state = ENTRY_EMPTY;
+
+ atomic numEntries = 0;
+ }
+
+ uint8_t numErrors=0;
+ void addNewEntry(TimeSyncMsg *msg)
+ {
+ int8_t i, freeItem = -1, oldestItem = 0;
+ uint32_t age, oldestTime = 0;
+ int32_t timeError;
+
+ tableEntries = 0;
+
+ // clear table if the received entry's been inconsistent for some time
+ timeError = msg->localTime;
+ call GlobalTime.local2Global(&timeError);
+ timeError -= msg->globalTime;
+ if( (is_synced() == SUCCESS) &&
+ (timeError > ENTRY_THROWOUT_LIMIT || timeError < -ENTRY_THROWOUT_LIMIT))
+ {
+ if (++numErrors>3)
+ clearTable();
+ }
+ else
+ numErrors = 0;
+
+
+ for(i = 0; i < MAX_ENTRIES; ++i) {
+ age = msg->localTime - table[i].localTime;
+
+ //logical time error compensation
+ if( age >= 0x7FFFFFFFL )
+ table[i].state = ENTRY_EMPTY;
+
+ if( table[i].state == ENTRY_EMPTY )
+ freeItem = i;
+ else
+ ++tableEntries;
+
+ if( age >= oldestTime ) {
+ oldestTime = age;
+ oldestItem = i;
+ }
+ }
+
+ if( freeItem < 0 )
+ freeItem = oldestItem;
+ else
+ ++tableEntries;
+
+ table[freeItem].state = ENTRY_FULL;
+
+ table[freeItem].localTime = msg->localTime;
+ table[freeItem].timeOffset = msg->globalTime - msg->localTime;
+ }
+
+ void task processMsg()
+ {
+ TimeSyncMsg* msg = (TimeSyncMsg*)(processedMsg->data);
+
+ if( msg->rootID < outgoingMsg->rootID &&
+ // jw: after becoming the root ignore other roots messages (in send period)
+ ~(heartBeats < IGNORE_ROOT_MSG && outgoingMsg->rootID == TOS_NODE_ID) ){
+ outgoingMsg->rootID = msg->rootID;
+ outgoingMsg->seqNum = msg->seqNum;
+ }
+ else if( outgoingMsg->rootID == msg->rootID && (int8_t)(msg->seqNum - outgoingMsg->seqNum) > 0 ) {
+ outgoingMsg->seqNum = msg->seqNum;
+ }
+ else
+ goto exit;
+
+ call Leds.led0Toggle();
+ if( outgoingMsg->rootID < TOS_NODE_ID )
+ heartBeats = 0;
+
+ addNewEntry(msg);
+ calculateConversion();
+ signal TimeSyncNotify.msg_received();
+
+ exit:
+ state &= ~STATE_PROCESSING;
+ }
+
+ event message_t* Receive.receive(message_t* msg, void* payload, uint8_t len)
+ {
+#ifdef TIMESYNC_DEBUG // this code can be used to simulate multiple hopsf
+ uint8_t incomingID = (uint8_t)((TimeSyncMsg*)payload)->nodeID;
+ int8_t diff = (incomingID & 0x0F) - (TOS_NODE_ID & 0x0F);
+ if( diff < -1 || diff > 1 )
+ return msg;
+ diff = (incomingID & 0xF0) - (TOS_NODE_ID & 0xF0);
+ if( diff < -16 || diff > 16 )
+ return msg;
+#endif
+ if( (state & STATE_PROCESSING) == 0 ) {
+ message_t* old = processedMsg;
+
+ processedMsg = msg;
+ ((TimeSyncMsg*)(processedMsg->data))->localTime = call TimeSyncPacket.eventTime(msg);
+
+ state |= STATE_PROCESSING;
+ post processMsg();
+
+ return old;
+ }
+
+ return msg;
+ }
+
+ task void sendMsg()
+ {
+ uint32_t localTime, globalTime;
+
+ globalTime = localTime = call GlobalTime.getLocalTime();
+ call GlobalTime.local2Global(&globalTime);
+
+ // we need to periodically update the reference point for the root
+ // to avoid wrapping the 32-bit (localTime - localAverage) value
+ if( outgoingMsg->rootID == TOS_NODE_ID ) {
+ if( (int32_t)(localTime - localAverage) >= 0x20000000 )
+ {
+ atomic
+ {
+ localAverage = localTime;
+ offsetAverage = globalTime - localTime;
+ }
+ }
+ }
+ else if( heartBeats >= ROOT_TIMEOUT ) {
+ heartBeats = 0; //to allow ROOT_SWITCH_IGNORE to work
+ outgoingMsg->rootID = TOS_NODE_ID;
+ ++(outgoingMsg->seqNum); // maybe set it to zero?
+ }
+
+ outgoingMsg->globalTime = globalTime;
+
+ // we don't send time sync msg, if we don't have enough data
+ if( numEntries < ENTRY_SEND_LIMIT && outgoingMsg->rootID != TOS_NODE_ID ){
+ ++heartBeats;
+ state &= ~STATE_SENDING;
+ }
+ else if( call Send.send(AM_BROADCAST_ADDR, &outgoingMsgBuffer, TIMESYNCMSG_LEN, localTime ) != SUCCESS ){
+ state &= ~STATE_SENDING;
+ signal TimeSyncNotify.msg_sent();
+ }
+ }
+
+ event void Send.sendDone(message_t* ptr, error_t error)
+ {
+ if (ptr != &outgoingMsgBuffer)
+ return;
+
+ if(error == SUCCESS)
+ {
+ ++heartBeats;
+ call Leds.led1Toggle();
+
+ if( outgoingMsg->rootID == TOS_NODE_ID )
+ ++(outgoingMsg->seqNum);
+ }
+
+ state &= ~STATE_SENDING;
+ signal TimeSyncNotify.msg_sent();
+ }
+
+ void timeSyncMsgSend()
+ {
+ if( outgoingMsg->rootID == 0xFFFF && ++heartBeats >= ROOT_TIMEOUT ) {
+ outgoingMsg->seqNum = 0;
+ outgoingMsg->rootID = TOS_NODE_ID;
+ }
+
+ if( outgoingMsg->rootID != 0xFFFF && (state & STATE_SENDING) == 0 ) {
+ state |= STATE_SENDING;
+ post sendMsg();
+ }
+ }
+
+ event void Timer.fired()
+ {
+ if (mode == TS_TIMER_MODE) {
+ timeSyncMsgSend();
+ }
+ else
+ call Timer.stop();
+ }
+
+ command error_t TimeSyncMode.setMode(uint8_t mode_){
+ if (mode == mode_)
+ return SUCCESS;
+
+ if (mode_ == TS_USER_MODE){
+ call Timer.startPeriodic((uint32_t)1000 * BEACON_RATE);
+ }
+ else
+ call Timer.stop();
+
+ mode = mode_;
+ return SUCCESS;
+ }
+
+ command uint8_t TimeSyncMode.getMode(){
+ return mode;
+ }
+
+ command error_t TimeSyncMode.send(){
+ if (mode == TS_USER_MODE){
+ timeSyncMsgSend();
+ return SUCCESS;
+ }
+ return FAIL;
+ }
+
+ command error_t Init.init()
+ {
+ atomic{
+ skew = 0.0;
+ localAverage = 0;
+ offsetAverage = 0;
+ };
+
+ clearTable();
+
+ atomic outgoingMsg = (TimeSyncMsg*)call Send.getPayload(&outgoingMsgBuffer, sizeof(TimeSyncMsg));
+ outgoingMsg->rootID = 0xFFFF;
+
+ processedMsg = &processedMsgBuffer;
+ state = STATE_INIT;
+
+ return SUCCESS;
+ }
+
+ event void Boot.booted()
+ {
+ call RadioControl.start();
+ call StdControl.start();
+ }
+
+ command error_t StdControl.start()
+ {
+ mode = TS_TIMER_MODE;
+ heartBeats = 0;
+ outgoingMsg->nodeID = TOS_NODE_ID;
+ call Timer.startPeriodic((uint32_t)1000 * BEACON_RATE);
+
+ return SUCCESS;
+ }
+
+ command error_t StdControl.stop()
+ {
+ call Timer.stop();
+ return SUCCESS;
+ }
+
+ async command float TimeSyncInfo.getSkew() { return skew; }
+ async command uint32_t TimeSyncInfo.getOffset() { return offsetAverage; }
+ async command uint32_t TimeSyncInfo.getSyncPoint() { return localAverage; }
+ async command uint16_t TimeSyncInfo.getRootID() { return outgoingMsg->rootID; }
+ async command uint8_t TimeSyncInfo.getSeqNum() { return outgoingMsg->seqNum; }
+ async command uint8_t TimeSyncInfo.getNumEntries() { return numEntries; }
+ async command uint8_t TimeSyncInfo.getHeartBeats() { return heartBeats; }
+
+ default event void TimeSyncNotify.msg_received(){}
+ default event void TimeSyncNotify.msg_sent(){}
+
+ event void RadioControl.startDone(error_t error){}
+ event void RadioControl.stopDone(error_t error){}
+}
projects / tinyos-2.x.git / commitdiff