FHSS: Use given TX time in synch calculation

Author: Jarkko Paso

source/Service_Libs/fhss/fhss.c

@@ -45,6 +45,7 @@ static int8_t fhss_beacon_create_tasklet(fhss_structure_t *fhss_structure);
 static void fhss_beacon_tasklet_func(arm_event_s *event);
 static int fhss_beacon_periodic_start(fhss_structure_t *fhss_structure, uint32_t time_to_first_beacon);
 static void fhss_beacon_periodic_stop(fhss_structure_t *fhss_structure);
+static int fhss_reset_synch_monitor(fhss_synch_monitor_s *synch_monitor);
 
 fhss_structure_t *fhss_enable(fhss_api_t *fhss_api, const fhss_configuration_t *fhss_configuration, const fhss_timer_t *fhss_timer, fhss_statistics_t *fhss_statistics)
 {
@@ -80,6 +81,7 @@ fhss_structure_t *fhss_enable(fhss_api_t *fhss_api, const fhss_configuration_t *
     if (!fhss_struct->bs->fhss_configuration.fhss_max_synch_interval) {
         fhss_struct->bs->fhss_configuration.fhss_max_synch_interval = 240;
     }
+    fhss_reset_synch_monitor(&fhss_struct->bs->synch_monitor);
     ns_list_init(&fhss_struct->fhss_failed_tx_list);
     fhss_struct->own_hop = 0xff;
     fhss_reset(fhss_struct);
@@ -1059,7 +1061,7 @@ static uint8_t *fhss_beacon_encode_raw(uint8_t *buffer, const fhss_synchronizati
     return buffer;
 }
 
-static void fhss_beacon_build(fhss_structure_t *fhss_structure, uint8_t *dest)
+static void fhss_beacon_build(fhss_structure_t *fhss_structure, uint8_t *dest, uint32_t tx_time)
 {
     fhss_synchronization_beacon_payload_s temp_payload;
     platform_enter_critical();
@@ -1075,8 +1077,12 @@ static void fhss_beacon_build(fhss_structure_t *fhss_structure, uint8_t *dest)
     temp_payload.number_of_broadcast_channels = config->fhss_number_of_bc_channels;
     temp_payload.number_of_tx_slots = config->fhss_number_of_tx_slots;
     temp_payload.time_since_last_beacon = 0; // XXX not available yet
-    uint32_t tx_time = fhss_get_tx_time(fhss_structure, 71, 0, 0);
-    temp_payload.processing_delay = fhss_structure->bs->fhss_configuration.fhss_tuning_parameters.tx_processing_delay + tx_time;
+    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;
+    }
+    temp_payload.processing_delay = fhss_structure->bs->fhss_configuration.fhss_tuning_parameters.tx_processing_delay + time_to_tx;
     temp_payload.superframe_length = config->fhss_superframe_length;
     temp_payload.number_of_superframes_per_channel = config->fhss_number_of_superframes;
     platform_exit_critical();
@@ -1091,7 +1097,7 @@ static int16_t fhss_write_synch_info_callback(const fhss_api_t *api, uint8_t *pt
     if (!fhss_structure || !ptr || (frame_type != FHSS_SYNCH_FRAME)) {
         return -1;
     }
-    fhss_beacon_build(fhss_structure, ptr);
+    fhss_beacon_build(fhss_structure, ptr, tx_time);
     return FHSS_SYNCH_INFO_LENGTH;
 }