From 7689c8c8efd57091caa1c84be430f0e1570ad8f2 Mon Sep 17 00:00:00 2001 From: Jarkko Paso Date: Fri, 5 Apr 2019 13:16:08 +0300 Subject: [PATCH] FHSS WS: Added maximum drift compensation step --- source/Service_Libs/fhss/fhss_ws.c | 8 +++++++- source/Service_Libs/fhss/fhss_ws.h | 2 ++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/source/Service_Libs/fhss/fhss_ws.c b/source/Service_Libs/fhss/fhss_ws.c index 2f73cc2ed386..5c0907a4056e 100644 --- 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 index 9e6dfbff5ec6..917b599878f9 100644 --- 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 {