FHSS WS: Fixed drifting TX trig timer

source/Service_Libs/fhss/fhss_ws.c

@@ -308,6 +308,10 @@ static void fhss_event_timer_cb(int8_t timer_id, uint16_t slots)
     } else {
         // On unicast, start timer to trigger polling event on next TX slot
         uint32_t delay_between_tx_slots_us = MS_TO_US(fhss_structure->ws->txrx_slot_length_ms) * 2;
+        // Timer could drift to RX slot when broadcast interval is high. Return timer to TX slot.
+        if (fhss_ws_check_tx_allowed(fhss_structure) == false) {
+            delay_between_tx_slots_us -= MS_TO_US(fhss_structure->ws->txrx_slot_length_ms - (calc_own_tx_trig_slot(fhss_structure->own_hop) * (fhss_structure->ws->txrx_slot_length_ms / 2)));
+        }
         if (delay_between_tx_slots_us < get_remaining_slots_us(fhss_structure, fhss_broadcast_handler, MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_broadcast_interval))) {
             eventOS_callback_timer_start(fhss_structure->fhss_event_timer, delay_between_tx_slots_us / 50);
         }
@@ -529,16 +533,12 @@ static bool fhss_ws_check_tx_allowed(fhss_structure_t *fhss_structure)
     }
 
     uint32_t remaining_time_ms = get_remaining_slots_us(fhss_structure, fhss_broadcast_handler, MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_broadcast_interval)) / 1000;
-    uint32_t tx_slot_begin = (fhss_structure->ws->fhss_configuration.fhss_broadcast_interval - fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval) - (fhss_structure->ws->txrx_slot_length_ms * (fhss_structure->own_hop & 1));
-    uint32_t rx_slot_begin = tx_slot_begin - fhss_structure->ws->txrx_slot_length_ms;
-    uint32_t n_o_tx_slots = number_of_tx_slots;
-
-    while (n_o_tx_slots--) {
-        if ((remaining_time_ms <= tx_slot_begin) && (remaining_time_ms > rx_slot_begin)) {
-            return true;
-        }
-        tx_slot_begin -= (2 * fhss_structure->ws->txrx_slot_length_ms);
-        rx_slot_begin = tx_slot_begin - fhss_structure->ws->txrx_slot_length_ms;
+    uint32_t tx_slot_begin_ms = (fhss_structure->ws->fhss_configuration.fhss_broadcast_interval - fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval) - (fhss_structure->ws->txrx_slot_length_ms * (fhss_structure->own_hop & 1));
+    tx_slot_begin_ms = tx_slot_begin_ms - (((tx_slot_begin_ms - remaining_time_ms) / (2 * fhss_structure->ws->txrx_slot_length_ms)) * (2 * fhss_structure->ws->txrx_slot_length_ms));
+    uint32_t rx_slot_begin_ms = tx_slot_begin_ms - fhss_structure->ws->txrx_slot_length_ms;
+    // Check if we are currently on TX slot.
+    if ((remaining_time_ms <= tx_slot_begin_ms) && (remaining_time_ms > rx_slot_begin_ms)) {
+        return true;
     }
 
     return false;