FHSS: Traces in set parent

Author: Jarkko Paso

source/Service_Libs/fhss/fhss_ws.c

@@ -730,8 +730,10 @@ int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8],
     if ((S_TO_US(fhss_structure->ws->min_synch_interval) > (fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api) - fhss_structure->ws->synchronization_time)) && !force_synch) {
         return 0;
     }
-    fhss_structure->ws->synchronization_time = fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api);
     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 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);
     uint32_t true_bc_interval_offset = (bc_timing_info->broadcast_interval_offset + time_from_reception_ms) % bc_timing_info->broadcast_interval;
@@ -756,6 +758,9 @@ int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8],
     platform_exit_critical();
     //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));
+    }
     return 0;
 }