Iotthd 4308 (ARMmbed#2473)

* Fixed UFSI update print function

* More test traces

* More traces

* Fixed ufsi function and more traces

* Cleaning traces

* Cleaning

Author: Jarkko Paso

source/6LoWPAN/ws/ws_neighbor_class.c

@@ -103,18 +103,30 @@ static void ws_neighbor_calculate_ufsi_drift(ws_neighbor_class_entry_t *ws_neigh
 {
     if (ws_neighbor->fhss_data.uc_timing_info.utt_rx_timestamp && ws_neighbor->fhss_data.uc_timing_info.ufsi) {
         uint32_t seq_length = 0x10000;
-        if (ws_neighbor->fhss_data.uc_timing_info.unicast_channel_function  == WS_TR51CF) {
+        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;
+        if (ws_neighbor->fhss_data.uc_timing_info.unicast_channel_function == WS_DH1CF) {
+            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)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;
+
+        if (ws_neighbor->fhss_data.uc_timing_info.unicast_channel_function == WS_TR51CF) {
+            uint32_t full_uc_schedule_ms = ws_neighbor->fhss_data.uc_timing_info.unicast_dwell_interval * ws_neighbor->fhss_data.uc_timing_info.unicast_number_of_channels;
+            uint32_t temp_ms = (time_since_last_ufsi_us / 1000) / full_uc_schedule_ms;
+            if (time_since_seq_start_cur_ms >= time_since_seq_start_prev_ms) {
+                temp_ms--;
+            }
+            time_since_seq_start_cur_ms += temp_ms * full_uc_schedule_ms + (full_uc_schedule_ms - time_since_seq_start_prev_ms) + time_since_seq_start_prev_ms;
+        }
+
         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);
         // Only trace if there is significant error

source/Service_Libs/fhss/fhss_ws.c

@@ -456,8 +456,10 @@ static uint32_t fhss_ws_calculate_ufsi(fhss_structure_t *fhss_structure, uint32_
 {
     uint8_t dwell_time = fhss_structure->ws->fhss_configuration.fhss_uc_dwell_interval;
     uint16_t cur_slot = fhss_structure->ws->uc_slot;
-    if (cur_slot == 0) {
-        cur_slot = fhss_structure->number_of_uc_channels;
+    if (fhss_structure->ws->fhss_configuration.ws_uc_channel_function == WS_TR51CF) {
+        if (cur_slot == 0) {
+            cur_slot = fhss_structure->number_of_uc_channels;
+        }
     }
     cur_slot--;
     uint32_t remaining_time_ms = 0;