Merge pull request ARMmbed#2052 from ARMmbed/IOTTHD-3375

FHSS WS: Added maximum drift compensation step

Author: Jarkko Paso

source/Service_Libs/fhss/fhss_ws.c

@@ -851,7 +851,13 @@ int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8],
         //TODO: Compensation for fixed channel configuration
         if (SYNCH_COMPENSATION_MIN_INTERVAL <= US_TO_S(time_since_last_synch_us)) {
             // Update clock drift
-            fhss_structure->ws->drift_per_millisecond_ns += divide_integer(MS_TO_NS((true_bc_interval_offset - own_bc_interval_offset) + ((int32_t)(fhss_structure->ws->bc_slot - own_bc_slot) * bc_timing_info->broadcast_interval)), US_TO_MS(time_since_last_synch_us));
+            int32_t drift_per_ms_tmp = divide_integer(MS_TO_NS((true_bc_interval_offset - own_bc_interval_offset) + ((int32_t)(fhss_structure->ws->bc_slot - own_bc_slot) * bc_timing_info->broadcast_interval)), US_TO_MS(time_since_last_synch_us));
+            if (drift_per_ms_tmp > MAX_DRIFT_COMPENSATION_STEP) {
+                drift_per_ms_tmp = MAX_DRIFT_COMPENSATION_STEP;
+            } else if (drift_per_ms_tmp < -MAX_DRIFT_COMPENSATION_STEP) {
+                drift_per_ms_tmp = -MAX_DRIFT_COMPENSATION_STEP;
+            }
+            fhss_structure->ws->drift_per_millisecond_ns += drift_per_ms_tmp;
         }
         tr_debug("synch to parent: %s, drift: %"PRIi32"ms in %"PRIu32" seconds, compensation: %"PRIi32"ns per ms", trace_array(eui64, 8), true_bc_interval_offset - own_bc_interval_offset + ((int32_t)(fhss_structure->ws->bc_slot - own_bc_slot) * bc_timing_info->broadcast_interval), US_TO_S(time_since_last_synch_us), fhss_structure->ws->drift_per_millisecond_ns);
     }

source/Service_Libs/fhss/fhss_ws.h

@@ -27,6 +27,8 @@
 #define DEFAULT_MIN_SYNCH_INTERVAL      60
 // Drift compensation allowed if at least SYNCH_COMPENSATION_MIN_INTERVAL (seconds) since last synchronization
 #define SYNCH_COMPENSATION_MIN_INTERVAL 60
+// MAX compensation per received synchronization info in ns
+#define MAX_DRIFT_COMPENSATION_STEP     10
 typedef struct fhss_ws fhss_ws_t;
 
 struct fhss_ws {