From 97da226255c864f2e15d19c754e6d938b0f867c5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20L=C3=B6scher?= <andreas.loscher@it.uu.se>
Date: Thu, 26 Feb 2015 16:40:48 +0100
Subject: [PATCH] time deviation for MSP based nodes

---
 .../org/contikios/cooja/mspmote/MspMote.java  | 23 ++++++++++++++++++-
 .../cooja/mspmote/interfaces/MspClock.java    | 14 +++++++++--
 2 files changed, 34 insertions(+), 3 deletions(-)

diff --git a/tools/cooja/apps/mspsim/src/org/contikios/cooja/mspmote/MspMote.java b/tools/cooja/apps/mspsim/src/org/contikios/cooja/mspmote/MspMote.java
index 403376583..2abbd7f47 100644
--- a/tools/cooja/apps/mspsim/src/org/contikios/cooja/mspmote/MspMote.java
+++ b/tools/cooja/apps/mspsim/src/org/contikios/cooja/mspmote/MspMote.java
@@ -78,6 +78,8 @@ import se.sics.mspsim.util.MapEntry;
 import se.sics.mspsim.util.MapTable;
 import se.sics.mspsim.profiler.SimpleProfiler;
 
+import org.contikios.cooja.mspmote.interfaces.MspClock;
+
 /**
  * @author Fredrik Osterlind
  */
@@ -313,9 +315,24 @@ public abstract class MspMote extends AbstractEmulatedMote implements Mote, Watc
       throw new RuntimeException("Bad event ordering: " + lastExecute + " < " + t);
     }
 
+    /* time deviation skip if ahead*/
+    double rtime = ((MspClock) (myMoteInterfaceHandler.getClock()))
+        .getReferenceTime();
+    double deviation = ((MspClock) myMoteInterfaceHandler.getClock())
+        .getDeviation();
+    long drift = myMoteInterfaceHandler.getClock().getDrift();
+    if (Math.round(rtime) < (t + drift)) {
+      lastExecute = t;
+      nextExecute = t + duration;
+      ((MspClock) (myMoteInterfaceHandler.getClock())).setReferenceTime(rtime
+          + duration + (deviation * duration));
+      scheduleNextWakeup(nextExecute);
+      return;
+    }
+
     /* Execute MSPSim-based mote */
     /* TODO Try-catch overhead */
-    try {
+    try { 
       nextExecute =
         t + duration +
         myCpu.stepMicros(t - lastExecute, duration);
@@ -333,6 +350,10 @@ public abstract class MspMote extends AbstractEmulatedMote implements Mote, Watc
     /*logger.debug(t + ": Schedule next wakeup at " + nextExecute);*/
     scheduleNextWakeup(nextExecute);
 
+    /* time deviation book keeping */
+    ((MspClock) (myMoteInterfaceHandler.getClock())).setReferenceTime(rtime
+        + deviation * (nextExecute - lastExecute));
+    
     if (stopNextInstruction) {
       stopNextInstruction = false;
       throw new RuntimeException("MSPSim requested simulation stop");
diff --git a/tools/cooja/apps/mspsim/src/org/contikios/cooja/mspmote/interfaces/MspClock.java b/tools/cooja/apps/mspsim/src/org/contikios/cooja/mspmote/interfaces/MspClock.java
index 7a07859fe..23d23010c 100644
--- a/tools/cooja/apps/mspsim/src/org/contikios/cooja/mspmote/interfaces/MspClock.java
+++ b/tools/cooja/apps/mspsim/src/org/contikios/cooja/mspmote/interfaces/MspClock.java
@@ -52,11 +52,13 @@ public class MspClock extends Clock {
   private Simulation simulation;
   
   private long timeDrift; /* Microseconds */
+  private double referenceTime; /* Microseconds */
   private double deviation;
 
   public MspClock(Mote mote) {
     simulation = mote.getSimulation();
-    deviation = 1.0;
+    referenceTime = 0.0;
+    deviation = 0.999;
   }
 
   public void setTime(long newTime) {
@@ -75,8 +77,16 @@ public class MspClock extends Clock {
     return timeDrift;
   }
 
+  public void setReferenceTime(double referenceTime) {
+    this.referenceTime = referenceTime;
+  }
+  
+  public double getReferenceTime() {
+    return referenceTime;
+  }
+  
   public void setDeviation(double deviation) {
-    assert deviation>0.0;
+    assert (deviation>0.0) && (deviation<=1.0);
     this.deviation = deviation;
   }