FHSS WS: Clock drift compensation implemented

Author: Jarkko Paso

source/Service_Libs/fhss/fhss_ws.c

@@ -54,6 +54,14 @@ void (*fhss_bc_switch)(void) = NULL;
 #define MS_TO_US(x) (((uint32_t)x)*1000)
 // Microseconds to milliseconds
 #define US_TO_MS(x) divide_integer(x, 1000)
+// Milliseconds to nanoseconds
+#define MS_TO_NS(x) (((uint32_t)x)*1000000)
+// Nanoseconds to milliseconds
+#define NS_TO_MS(x) divide_integer(x, 1000000)
+// Microseconds to nanoseconds
+#define US_TO_NS(x) (((uint32_t)x)*1000)
+// Nanoseconds to microseconds
+#define NS_TO_US(x) divide_integer(x, 1000)
 #define DEF_2E24 0x1000000
 #define IE_HEADER_LENGTH_MASK 0x007f
 #define IE_HEADER_ID_MASK 0x7f80
@@ -92,6 +100,15 @@ static uint32_t get_remaining_slots_us(fhss_structure_t *fhss_structure, void (*
     return remaining_time_us;
 }
 
+void fhss_ws_start_timer(fhss_structure_t *fhss_structure, uint32_t time, void (*callback)(const fhss_api_t *fhss_api, uint16_t))
+{
+    // Number of millisecond slots in timeout(us)
+    int32_t time_in_ms = divide_integer(time, 1000);
+    // Reduce the compensation (per millisecond) from timeout.
+    time -= NS_TO_US(time_in_ms*fhss_structure->ws->drift_per_millisecond_ns);
+    fhss_start_timer(fhss_structure, time, callback);
+}
+
 fhss_structure_t *fhss_ws_enable(fhss_api_t *fhss_api, const fhss_ws_configuration_t *fhss_configuration, const fhss_timer_t *fhss_timer)
 {
     if (!fhss_api || !fhss_configuration || !fhss_timer) {
@@ -196,7 +213,7 @@ static void fhss_broadcast_handler(const fhss_api_t *fhss_api, uint16_t delay)
         return;
     }
     if (fhss_structure->ws->is_on_bc_channel == false) {
-        fhss_start_timer(fhss_structure, MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval) - (delay * fhss_structure->platform_functions.fhss_resolution_divider), fhss_broadcast_handler);
+        fhss_ws_start_timer(fhss_structure, MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval) - (delay * fhss_structure->platform_functions.fhss_resolution_divider), fhss_broadcast_handler);
         fhss_structure->ws->is_on_bc_channel = true;
         next_channel = fhss_structure->ws->bc_channel = fhss_ws_calc_bc_channel(fhss_structure);
 
@@ -211,7 +228,7 @@ static void fhss_broadcast_handler(const fhss_api_t *fhss_api, uint16_t delay)
         eventOS_callback_timer_start(fhss_structure->fhss_event_timer, randLIB_get_random_in_range(bc_min_random, bc_max_random));
     } else {
         uint32_t timeout = MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_broadcast_interval - fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval);
-        fhss_start_timer(fhss_structure, timeout - (delay * fhss_structure->platform_functions.fhss_resolution_divider), fhss_broadcast_handler);
+        fhss_ws_start_timer(fhss_structure, timeout - (delay * fhss_structure->platform_functions.fhss_resolution_divider), fhss_broadcast_handler);
         fhss_structure->ws->is_on_bc_channel = false;
         // Should return to own (unicast) listening channel after broadcast channel
         next_channel = fhss_structure->rx_channel;
@@ -353,7 +370,7 @@ static int16_t fhss_ws_synch_state_set_callback(const fhss_api_t *api, fhss_stat
         // Start unicast schedule
         if ((fhss_structure->ws->fhss_configuration.ws_uc_channel_function != WS_FIXED_CHANNEL)) {
             fhss_ws_update_uc_channel_callback(fhss_structure);
-            fhss_start_timer(fhss_structure, MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_uc_dwell_interval), fhss_unicast_handler);
+            fhss_ws_start_timer(fhss_structure, MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_uc_dwell_interval), fhss_unicast_handler);
             fhss_structure->ws->unicast_timer_running = true;
         }
     }
@@ -689,7 +706,7 @@ static void fhss_unicast_handler(const fhss_api_t *fhss_api, uint16_t delay)
         fhss_structure->ws->unicast_timer_running = false;
         return;
     }
-    fhss_start_timer(fhss_structure, timeout - (delay * fhss_structure->platform_functions.fhss_resolution_divider), fhss_unicast_handler);
+    fhss_ws_start_timer(fhss_structure, timeout - (delay * fhss_structure->platform_functions.fhss_resolution_divider), fhss_unicast_handler);
     fhss_structure->ws->unicast_timer_running = true;
     fhss_ws_update_uc_channel_callback(fhss_structure);
     // Unless we have broadcast schedule, we have to poll unicast queue when changing channel. This is randomized by the unicast schedule.
@@ -733,6 +750,7 @@ int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8],
     platform_enter_critical();
     uint32_t prev_synchronization_time = fhss_structure->ws->synchronization_time;
     fhss_structure->ws->synchronization_time = fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api);
+    uint32_t time_since_last_synch_us = fhss_structure->ws->synchronization_time - prev_synchronization_time;
     uint32_t own_bc_interval_offset = fhss_ws_calculate_broadcast_interval_offset(fhss_structure, fhss_structure->ws->synchronization_time);
     fhss_stop_timer(fhss_structure, fhss_broadcast_handler);
     uint32_t time_from_reception_ms = US_TO_MS(fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api) - bc_timing_info->bt_rx_timestamp);
@@ -745,7 +763,7 @@ int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8],
     if (fhss_structure->ws->is_on_bc_channel) {
         timeout -= MS_TO_US(bc_timing_info->broadcast_interval - bc_timing_info->broadcast_dwell_interval);
     }
-    fhss_start_timer(fhss_structure, timeout, fhss_broadcast_handler);
+    fhss_ws_start_timer(fhss_structure, timeout, fhss_broadcast_handler);
     uint16_t slots_since_reception = (bc_timing_info->broadcast_interval_offset + time_from_reception_ms) / bc_timing_info->broadcast_interval;
     // TODO: Calculate drift error
     fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval = bc_timing_info->broadcast_dwell_interval;
@@ -759,7 +777,17 @@ int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8],
     //TODO: support multiple parents
     fhss_structure->ws->parent_bc_info = bc_timing_info;
     if (prev_synchronization_time) {
-        tr_debug("synch to parent: %s, drift: %ims in %u seconds", trace_array(eui64, 8), true_bc_interval_offset - own_bc_interval_offset, US_TO_S(fhss_structure->ws->synchronization_time - prev_synchronization_time));
+        if (SYNCH_COMPENSATION_MIN_INTERVAL <= US_TO_S(time_since_last_synch_us)) {
+            // Compensate clock drift
+            if (true_bc_interval_offset > own_bc_interval_offset) {
+                // Our clock runs too slow compared to parent, mark as positive drift.
+                fhss_structure->ws->drift_per_millisecond_ns += divide_integer(MS_TO_NS(true_bc_interval_offset - own_bc_interval_offset), US_TO_MS(time_since_last_synch_us));
+            } else {
+                // Our clock runs too fast compared to parent, mark as negative drift.
+                fhss_structure->ws->drift_per_millisecond_ns -= divide_integer(MS_TO_NS(own_bc_interval_offset - true_bc_interval_offset), US_TO_MS(time_since_last_synch_us));
+            }
+        }
+        tr_debug("synch to parent: %s, drift: %ims in %u seconds, compensation: %ins per ms", trace_array(eui64, 8), true_bc_interval_offset - own_bc_interval_offset, US_TO_S(time_since_last_synch_us), fhss_structure->ws->drift_per_millisecond_ns);
     }
     return 0;
 }
@@ -786,7 +814,7 @@ int fhss_ws_configuration_set(fhss_structure_t *fhss_structure, const fhss_ws_co
         fhss_structure->ws->unicast_timer_running = false;
     }
     if ((fhss_structure->ws->unicast_timer_running == false) && (fhss_configuration->ws_uc_channel_function != WS_FIXED_CHANNEL) && fhss_configuration->fhss_uc_dwell_interval) {
-        fhss_start_timer(fhss_structure, MS_TO_US(fhss_configuration->fhss_uc_dwell_interval), fhss_unicast_handler);
+        fhss_ws_start_timer(fhss_structure, MS_TO_US(fhss_configuration->fhss_uc_dwell_interval), fhss_unicast_handler);
         fhss_structure->ws->unicast_timer_running = true;
     }
     fhss_structure->ws->fhss_configuration = *fhss_configuration;

source/Service_Libs/fhss/fhss_ws.h

@@ -25,6 +25,8 @@
 #define WS_MAX_TXRX_SLOT_LEN_MS         100
 // Default minimum broadcast synchronization interval in seconds
 #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
 typedef struct fhss_ws fhss_ws_t;
 
 struct fhss_ws {
@@ -34,6 +36,7 @@ struct fhss_ws {
     uint16_t min_synch_interval;
     uint32_t txrx_slot_length_ms;
     uint32_t synchronization_time;
+    int32_t drift_per_millisecond_ns;
     int16_t *tr51_channel_table;
     uint8_t *tr51_output_table;
     bool unicast_timer_running;