FHSS: Fixed calculating UFSI

source/Service_Libs/fhss/fhss_ws.c

@@ -173,7 +173,11 @@ static uint32_t fhss_ws_calculate_ufsi(fhss_structure_t *fhss_structure, uint32_
     }
     cur_slot--;
     uint32_t remaining_time = (fhss_structure->platform_functions.fhss_get_remaining_slots(fhss_unicast_handler, fhss_structure->fhss_api) / 1000);
-    uint32_t time_to_tx = (tx_time - fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api)) / 1000;
+    uint32_t time_to_tx = 0;
+    uint32_t cur_time = fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api);
+    if (cur_time < tx_time) {
+        time_to_tx = (tx_time - cur_time) / 1000;
+    }
     uint64_t ms_since_seq_start = (cur_slot * dwell_time) + (dwell_time-remaining_time) + time_to_tx;
     uint32_t seq_length = 0x10000;
     if (fhss_structure->ws->fhss_configuration.ws_channel_function == WS_TR51CF) {
@@ -205,7 +209,7 @@ static uint16_t fhss_ws_calculate_destination_slot(fhss_ws_neighbor_timing_info_
     if (neighbor_timing_info->uc_timing_info.unicast_channel_function  == WS_TR51CF) {
         seq_length = neighbor_timing_info->uc_timing_info.unicast_number_of_channels;
     }
-    uint32_t dest_ms_since_seq_start = own_ceil((float)(ufsi*seq_length*dwell_time) / DEF_2E24);
+    uint32_t dest_ms_since_seq_start = own_ceil((float)((uint64_t)ufsi*seq_length*dwell_time) / DEF_2E24);
     return (own_floor(((float)((tx_time - ufsi_timestamp)/1000 + dest_ms_since_seq_start) / dwell_time)) % seq_length);
 }
 
@@ -241,7 +245,7 @@ static void fhss_ws_synch_state_set_callback(const fhss_api_t *api, fhss_states
 static void fhss_ws_update_uc_channel_callback(fhss_structure_t *fhss_structure)
 {
     uint8_t mac_address[8];
-    int32_t next_channel;
+    int32_t next_channel = fhss_structure->rx_channel;
     fhss_structure->callbacks.read_mac_address(fhss_structure->fhss_api, mac_address);
     if (fhss_structure->ws->fhss_configuration.ws_channel_function == WS_FIXED_CHANNEL) {
         return;