Fixed UFSI update print function (ARMmbed#2470)

artokin · Oct 7, 2020 · 5491a6b · 5491a6b
1 parent 86f64c5
commit 5491a6b
Showing 1 changed file with 7 additions and 2 deletions.
diff --git a/source/6LoWPAN/ws/ws_neighbor_class.c b/source/6LoWPAN/ws/ws_neighbor_class.c
@@ -106,9 +106,14 @@ static void ws_neighbor_calculate_ufsi_drift(ws_neighbor_class_entry_t *ws_neigh
         if (ws_neighbor->fhss_data.uc_timing_info.unicast_channel_function  == WS_TR51CF) {
             seq_length = ws_neighbor->fhss_data.uc_timing_info.unicast_number_of_channels;
         }
+        uint32_t ufsi_prev_tmp = ws_neighbor->fhss_data.uc_timing_info.ufsi;
+        uint32_t ufsi_cur_tmp = ws_utt->ufsi;
+        if (ufsi_cur_tmp < ufsi_prev_tmp) {
+            ufsi_cur_tmp += 0xffffff;
+        }
         // Convert 24-bit UFSI to real time before drift calculation
-        uint32_t time_since_seq_start_prev_ms = own_ceil((float)((uint64_t)ws_neighbor->fhss_data.uc_timing_info.ufsi * seq_length * ws_neighbor->fhss_data.uc_timing_info.unicast_dwell_interval) / 0x1000000);
-        uint32_t time_since_seq_start_cur_ms = own_ceil((float)((uint64_t)ws_utt->ufsi * seq_length * ws_neighbor->fhss_data.uc_timing_info.unicast_dwell_interval) / 0x1000000);
+        uint32_t time_since_seq_start_prev_ms = own_ceil((float)((uint64_t)ufsi_prev_tmp * seq_length * ws_neighbor->fhss_data.uc_timing_info.unicast_dwell_interval) / 0x1000000);
+        uint32_t time_since_seq_start_cur_ms = own_ceil((float)((uint64_t)ufsi_cur_tmp * seq_length * ws_neighbor->fhss_data.uc_timing_info.unicast_dwell_interval) / 0x1000000);
         uint32_t time_since_last_ufsi_us = timestamp - ws_neighbor->fhss_data.uc_timing_info.utt_rx_timestamp;
         uint32_t ufsi_diff_ms = time_since_seq_start_cur_ms - time_since_seq_start_prev_ms;
         int32_t ufsi_drift_ms = (int32_t)(time_since_last_ufsi_us / 1000 - ufsi_diff_ms);