FHSS: Fixed updating broadcast synch

Author: Jarkko Paso

source/MAC/IEEE802_15_4/mac_pd_sap.c

@@ -437,7 +437,7 @@ int8_t mac_pd_sap_data_cb(void *identifier, arm_phy_sap_msg_t *message)
             return -3;
         }
         if (rf_ptr->fhss_api) {
-            buffer->timestamp = rf_ptr->fhss_api->read_timestamp(rf_ptr->fhss_api);
+            buffer->timestamp = mac_pd_sap_get_phy_rx_time(rf_ptr);
         }
         buffer->ack_pendinfg_status = mac_data_interface_read_last_ack_pending_status(rf_ptr);
         ptr = mac_header_parse_fcf_dsn(&buffer->fcf_dsn, ptr);

source/Service_Libs/fhss/fhss_ws.c

@@ -111,7 +111,7 @@ static void fhss_broadcast_handler(const fhss_api_t *fhss_api, uint16_t delay)
     if (!fhss_structure) {
         return;
     }
-    // TODO: check parent broadcast timing constantly and calculate drift error
+
     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_structure->ws->is_on_bc_channel = true;
@@ -503,13 +503,20 @@ int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8],
     if (fhss_structure->ws->fhss_configuration.ws_channel_function == WS_FIXED_CHANNEL) {
         return 0;
     }
-    // Start broadcast schedule when BC intervals are known
-    if (bc_timing_info->broadcast_interval && bc_timing_info->broadcast_dwell_interval) {
-        fhss_start_timer(fhss_structure, (bc_timing_info->broadcast_interval-bc_timing_info->broadcast_interval_offset)*1000, fhss_broadcast_handler);
-        fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval = bc_timing_info->broadcast_dwell_interval;
-        fhss_structure->ws->fhss_configuration.fhss_broadcast_interval = bc_timing_info->broadcast_interval;
-        fhss_structure->ws->bc_slot = bc_timing_info->broadcast_slot;
+    if (!bc_timing_info->broadcast_interval || !bc_timing_info->broadcast_dwell_interval) {
+        return -1;
     }
+
+    uint32_t time_from_reception_ms = (fhss_structure->fhss_api->read_timestamp(fhss_structure->fhss_api) - bc_timing_info->bt_rx_timestamp)/1000;
+    uint32_t true_bc_interval_offset = (bc_timing_info->broadcast_interval_offset + time_from_reception_ms) % bc_timing_info->broadcast_interval;
+    uint32_t timeout = ((bc_timing_info->broadcast_interval-true_bc_interval_offset)*1000);
+    fhss_start_timer(fhss_structure, timeout, fhss_broadcast_handler);
+    uint16_t slots_since_reception = (bc_timing_info->broadcast_interval_offset + time_from_reception_ms) / bc_timing_info->broadcast_interval;
+    // TODO: Calculate drift error
+    fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval = bc_timing_info->broadcast_dwell_interval;
+    fhss_structure->ws->fhss_configuration.fhss_broadcast_interval = bc_timing_info->broadcast_interval;
+    fhss_structure->ws->bc_slot = bc_timing_info->broadcast_slot + slots_since_reception;
+
     //TODO: support multiple parents
     fhss_structure->ws->parent_bc_info = bc_timing_info;
     return 0;

test/nanostack/unittest/mac/mac_pd_sap/test_mac_pd_sap.c

@@ -550,15 +550,7 @@ bool test_mac_pd_sap_data_cb()
     mac_header_helper_functions_stub.fcf = fcf;
     phy_message.id = MAC15_4_PD_SAP_DATA_IND;
     phy_message.message.generic_data_ind.data_ptr = buffer;
-    // Test returning timestamp
-    fhss_config_stub.uint32_value = 1234567890;
-    ret = mac_pd_sap_data_cb(&rf_ptr, &phy_message);
-    if (ret != 0) {
-        return false;
-    }
-    if (mac_mcps_sap_stub.pre_parsed->timestamp != 1234567890){
-        return false;
-    }
+
     // Test TX confirm
     phy_message.id = MAC15_4_PD_SAP_DATA_TX_CONFIRM;
     rf_ptr.macTxProcessActive = true;