From 1487f209aa37c20dc0a7a91593ebbf44410dd030 Mon Sep 17 00:00:00 2001 From: sallai Date: Tue, 10 Jun 2008 22:16:13 +0000 Subject: [PATCH] checking in FTSP to tos/lib --- tos/lib/ftsp/GlobalTime.nc | 65 +++++ tos/lib/ftsp/TimeSyncC.nc | 72 +++++ tos/lib/ftsp/TimeSyncInfo.nc | 66 +++++ tos/lib/ftsp/TimeSyncMode.nc | 54 ++++ tos/lib/ftsp/TimeSyncMsg.h | 52 ++++ tos/lib/ftsp/TimeSyncNotify.nc | 45 ++++ tos/lib/ftsp/TimeSyncP.nc | 478 +++++++++++++++++++++++++++++++++ 7 files changed, 832 insertions(+) create mode 100644 tos/lib/ftsp/GlobalTime.nc create mode 100644 tos/lib/ftsp/TimeSyncC.nc create mode 100644 tos/lib/ftsp/TimeSyncInfo.nc create mode 100644 tos/lib/ftsp/TimeSyncMode.nc create mode 100644 tos/lib/ftsp/TimeSyncMsg.h create mode 100644 tos/lib/ftsp/TimeSyncNotify.nc create mode 100644 tos/lib/ftsp/TimeSyncP.nc diff --git a/tos/lib/ftsp/GlobalTime.nc b/tos/lib/ftsp/GlobalTime.nc new file mode 100644 index 00000000..5cae1fb3 --- /dev/null +++ b/tos/lib/ftsp/GlobalTime.nc @@ -0,0 +1,65 @@ +/* + * 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 +{ + /** + * Returns the current local time of this mote. + */ + async command uint32_t getLocalTime(); + + /** + * Reads the current global time. This method is a combination + * of getLocalTime and local2Global. + * @return SUCCESS if this mote is synchronized, FAIL otherwise. + */ + async command error_t getGlobalTime(uint32_t *time); + + /** + * Converts the local time given in time into the + * corresponding global time and stores this again in + * time. 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 time into the + * correspoding local time and stores this again in + * time. This method performs the inverse of the + * local2Global transformation. + * + * @return SUCCESS if this mote is synchronized, FAIL otherwise. + */ + async command error_t global2Local(uint32_t *time); +} diff --git a/tos/lib/ftsp/TimeSyncC.nc b/tos/lib/ftsp/TimeSyncC.nc new file mode 100644 index 00000000..58af71d2 --- /dev/null +++ b/tos/lib/ftsp/TimeSyncC.nc @@ -0,0 +1,72 @@ +/* + * 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; + + //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; + +} diff --git a/tos/lib/ftsp/TimeSyncInfo.nc b/tos/lib/ftsp/TimeSyncInfo.nc new file mode 100644 index 00000000..58cd40d0 --- /dev/null +++ b/tos/lib/ftsp/TimeSyncInfo.nc @@ -0,0 +1,66 @@ +/* + * 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(); +} diff --git a/tos/lib/ftsp/TimeSyncMode.nc b/tos/lib/ftsp/TimeSyncMode.nc new file mode 100644 index 00000000..baf7dafc --- /dev/null +++ b/tos/lib/ftsp/TimeSyncMode.nc @@ -0,0 +1,54 @@ +/* + * 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(); + + } + + diff --git a/tos/lib/ftsp/TimeSyncMsg.h b/tos/lib/ftsp/TimeSyncMsg.h new file mode 100644 index 00000000..bf5830a7 --- /dev/null +++ b/tos/lib/ftsp/TimeSyncMsg.h @@ -0,0 +1,52 @@ +/* + * 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 diff --git a/tos/lib/ftsp/TimeSyncNotify.nc b/tos/lib/ftsp/TimeSyncNotify.nc new file mode 100644 index 00000000..aac689bd --- /dev/null +++ b/tos/lib/ftsp/TimeSyncNotify.nc @@ -0,0 +1,45 @@ +/* + * 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(); + + } + + diff --git a/tos/lib/ftsp/TimeSyncP.nc b/tos/lib/ftsp/TimeSyncP.nc new file mode 100644 index 00000000..3a5824ae --- /dev/null +++ b/tos/lib/ftsp/TimeSyncP.nc @@ -0,0 +1,478 @@ +/* + * 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; + + //interfaces for extra functionality: need not to be wired + interface TimeSyncInfo; + interface TimeSyncMode; + interface TimeSyncNotify; + } + uses + { + interface Boot; + interface SplitControl as RadioControl; + interface TimeSyncAMSend as Send; + interface Receive; + interface Timer; + interface Leds; + interface TimeSyncPacket; + interface LocalTime 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){} +} -- 2.39.2