FHSS WS: Fixed compensation when on BC channel

Author: Jarkko Paso

source/Service_Libs/fhss/fhss_ws.c

@@ -197,7 +197,7 @@ static int32_t fhss_ws_calc_bc_channel(fhss_structure_t *fhss_structure)
         }
     }
 #ifdef FHSS_CHANNEL_DEBUG
-    tr_info("%"PRIu32" BC %u %u", fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api), next_channel, fhss_structure->ws->bc_slot);
+    tr_info("%"PRIu32" BC %u %u %u", fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api), next_channel, fhss_structure->ws->bc_slot, fhss_structure->ws->fhss_configuration.bsi);
 #endif /*FHSS_CHANNEL_DEBUG*/
     return next_channel;
 }
@@ -486,7 +486,7 @@ static int fhss_ws_tx_handle_callback(const fhss_api_t *api, bool is_broadcast_a
             }
         }
 #ifdef FHSS_CHANNEL_DEBUG
-        tr_debug("TX channel: %u %u", tx_channel, destination_slot + 1);
+        tr_debug("TX channel: %u %u %s", tx_channel, destination_slot + 1, trace_array(destination_address, 8));
 #endif /*FHSS_CHANNEL_DEBUG*/
         fhss_structure->callbacks.change_channel(fhss_structure->fhss_api, tx_channel);
     }
@@ -772,6 +772,7 @@ int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8],
         return 0;
     }
     platform_enter_critical();
+    uint16_t own_bc_slot = fhss_structure->ws->bc_slot;
     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;
@@ -803,9 +804,9 @@ int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8],
     if (prev_synchronization_time) {
         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), US_TO_MS(time_since_last_synch_us));
+            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));
         }
-        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, US_TO_S(time_since_last_synch_us), fhss_structure->ws->drift_per_millisecond_ns);
+        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);
     }
     return 0;
 }
@@ -859,5 +860,4 @@ int fhss_ws_set_hop_count(fhss_structure_t *fhss_structure, const uint8_t hop_co
     fhss_structure->own_hop = hop_count;
     return 0;
 }
-
 #endif // HAVE_WS