FHSS: Compensate delay from platform driver when starting timeout

cmonr · Aug 28, 2018 · eedd746 · eedd746
1 parent 7d537ea
commit eedd746
Showing 1 changed file with 2 additions and 3 deletions.
diff --git a/source/Service_Libs/fhss/fhss_ws.c b/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;