FHSS: Compensate delay from platform driver when starting timeout

Author: Jarkko Paso

source/Service_Libs/fhss/fhss_ws.c

@@ -115,7 +115,6 @@ static int32_t fhss_ws_calc_bc_channel(fhss_structure_t *fhss_structure)
 
 static void fhss_broadcast_handler(const fhss_api_t *fhss_api, uint16_t delay)
 {
-    (void) delay;
     int32_t next_channel;
     fhss_structure_t *fhss_structure = fhss_get_object_with_api(fhss_api);
     if (!fhss_structure) {
@@ -128,12 +127,12 @@ static void fhss_broadcast_handler(const fhss_api_t *fhss_api, uint16_t delay)
         return;
     }
     if (fhss_structure->ws->is_on_bc_channel == false) {
-        fhss_start_timer(fhss_structure, fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval*1000, fhss_broadcast_handler);
+        fhss_start_timer(fhss_structure, fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval*1000 - (delay * fhss_structure->platform_functions.fhss_resolution_divider), fhss_broadcast_handler);
         fhss_structure->ws->is_on_bc_channel = true;
         next_channel = fhss_structure->ws->bc_channel = fhss_ws_calc_bc_channel(fhss_structure);
     } else {
         uint32_t timeout = (fhss_structure->ws->fhss_configuration.fhss_broadcast_interval - fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval) * 1000;
-        fhss_start_timer(fhss_structure, timeout, fhss_broadcast_handler);
+        fhss_start_timer(fhss_structure, timeout - (delay * fhss_structure->platform_functions.fhss_resolution_divider), fhss_broadcast_handler);
         fhss_structure->ws->is_on_bc_channel = false;
         // Should return to own (unicast) listening channel after broadcast channel
         next_channel = fhss_structure->rx_channel;