FHSS WS: Added maximum drift compensation step

artokin · Apr 5, 2019 · 7689c8c · 7689c8c
1 parent 47f65aa
commit 7689c8c
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 1 deletion.
diff --git a/source/Service_Libs/fhss/fhss_ws.c b/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);
     }

diff --git a/source/Service_Libs/fhss/fhss_ws.h b/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 {