From 65d96c4e7f12c591533414a62d288df9ca751d6b Mon Sep 17 00:00:00 2001 From: Jarkko Paso Date: Tue, 28 Aug 2018 13:29:30 +0300 Subject: [PATCH] FHSS: Fixed calculating UFSI --- source/Service_Libs/fhss/fhss_ws.c | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/source/Service_Libs/fhss/fhss_ws.c b/source/Service_Libs/fhss/fhss_ws.c index c1f04512b57..820969bcd35 100644 --- a/source/Service_Libs/fhss/fhss_ws.c +++ b/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;