FHSS: Write BT-IE

source/Service_Libs/fhss/fhss_ws.c

@@ -38,6 +38,7 @@
 #define IE_HEADER_ID_MASK 0x7f80
 #define WH_IE_ID        0x2a
 #define WH_SUB_ID_UTT   1
+#define WH_SUB_ID_BT    2
 
 struct ws_ie_t {
     uint8_t *content_ptr;
@@ -156,6 +157,19 @@ static uint32_t fhss_ws_calculate_ufsi(fhss_structure_t *fhss_structure, uint32_
     return own_floor((float)(ms_since_seq_start * DEF_2E24) / (seq_length*dwell_time));
 }
 
+static uint32_t fhss_ws_calculate_broadcast_interval_offset(fhss_structure_t *fhss_structure, uint32_t tx_time)
+{
+    uint8_t dwell_time = fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval;
+    uint32_t broadcast_interval = fhss_structure->ws->fhss_configuration.fhss_broadcast_interval;
+
+    uint32_t remaining_time = (fhss_structure->platform_functions.fhss_get_remaining_slots(fhss_broadcast_handler, fhss_structure->fhss_api) / 1000);
+    if (fhss_structure->ws->is_on_bc_channel == true) {
+        remaining_time += (broadcast_interval - dwell_time);
+    }
+    uint32_t time_to_tx = (tx_time - fhss_structure->fhss_api->read_timestamp(fhss_structure->fhss_api)) / 1000;
+    return (broadcast_interval-remaining_time) + time_to_tx;
+}
+
 static uint16_t fhss_ws_calculate_destination_slot(fhss_ws_neighbor_timing_info_t *neighbor_timing_info, uint32_t tx_time)
 {
     uint_fast24_t ufsi = neighbor_timing_info->uc_timing_info.ufsi;
@@ -320,6 +334,11 @@ static int16_t fhss_ws_write_synch_info_callback(const fhss_api_t *api, uint8_t
         uint32_t ufsi = fhss_ws_calculate_ufsi(fhss_structure, tx_time);
         common_write_24_bit_inverse(ufsi, header_ie.content_ptr+1);
     }
+    if (fhss_ws_ie_header_discover(ptr, length, &header_ie, WH_SUB_ID_BT)) {
+        uint32_t broadcast_interval_offset = fhss_ws_calculate_broadcast_interval_offset(fhss_structure, tx_time);
+        common_write_16_bit_inverse(fhss_structure->ws->bc_slot, header_ie.content_ptr);
+        common_write_24_bit_inverse(broadcast_interval_offset, header_ie.content_ptr+2);
+    }
     return 0;
 }