/* * Copyright (c) 2019-2023 Beijing Hanwei Innovation Technology Ltd. Co. and * its subsidiaries and affiliates (collectly called MKSEMI). * * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * 1. Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * * 2. Redistributions in binary form, except as embedded into an MKSEMI * integrated circuit in a product or a software update for such product, * must reproduce the above copyright notice, this list of conditions and * the following disclaimer in the documentation and/or other materials * provided with the distribution. * * 3. Neither the name of MKSEMI nor the names of its contributors may be used * to endorse or promote products derived from this software without * specific prior written permission. * * 4. This software, with or without modification, must only be used with a * MKSEMI integrated circuit. * * 5. Any software provided in binary form under this license must not be * reverse engineered, decompiled, modified and/or disassembled. * * THIS SOFTWARE IS PROVIDED BY MKSEMI "AS IS" AND ANY EXPRESS OR IMPLIED * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF * MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE * DISCLAIMED. IN NO EVENT SHALL MKSEMI OR CONTRIBUTORS BE LIABLE FOR ANY * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include "mk_trace.h" #include "mk_clock.h" #include "mk_uwb.h" #include "mk_calib.h" #include "mk_misc.h" #include "uwb_api.h" #include "lib_aoa.h" #include "lib_ranging.h" #include "ranging_fira.h" #if KF_EN #include "lib_kf.h" #endif #if PDOA_3D_EN #include "lib_pdoa_3d.h" #endif #include "board.h" #define PRINT_PAYLOAD_EN 0 #define PRINT_PDOA_IQ_EN 0 #if MCTT_TEST_EN #define PAYLOAD_BUF_MAX 512 #endif static const char *work_mode_table[5] = {"Unknown", "SS-TWR Deferred", "DS-TWR Deferred", "SS-TWR Non-deferred", "DS-TWR Non-deferred"}; static const char *fira_fsm_table[7] = {"Unknown", "RCM", "Poll", "Response", "Final", "Report", "Result"}; /*************************************************************************************************/ /*! * \brief WSF event handler for ranging task. * * \param event WSF event mask. * \param msg WSF message. * * \return None. */ /*************************************************************************************************/ void ranging_handler(wsfEventMask_t event, const void *param) { const wsfMsgHdr_t *msg = (const wsfMsgHdr_t *)param; // board_led_on(BOARD_LED_1); #if MCTT_TEST_EN static uint8_t payload_print_buf[PAYLOAD_BUF_MAX]; static uint16_t payload_print_len; #endif static uint8_t defer_to_report = 0; if (msg != NULL) { switch (msg->event) { case RANGING_DAEMON_TIMER_MSG: { if (ranging_env.count == ranging_env.count_last) { LOG_INFO(TRACE_MODULE_APP, "Ranging was suspended %u\r\n", ranging_env.count); ranging_restart(); } else { // LOG_INFO(TRACE_MODULE_APP, "Ranging count %u %u\r\n", ranging_env.count_last, ranging_env.count); ranging_env.count_last = ranging_env.count; } } break; case UWB_PKT_TX_DONE_MSG: { const struct UWB_PKT_TX_DONE_IND_T *ind = (const struct UWB_PKT_TX_DONE_IND_T *)param; if (ind->slot_idx == 0) { ranging_env.range_data.sequence_num = (ranging_env.phy_sts_index - fira_key.phyStsIdxInit) / ranging_env.slots_per_block; uint8_t work_mode_idx = uwb_app_config.session_param.ranging_round_usage < 5 ? uwb_app_config.session_param.ranging_round_usage : 0; #if MCTT_TEST_EN LOG_INFO(TRACE_NO_OPTION, "%s Init-seq:%u\r\n", work_mode_table[work_mode_idx], ranging_env.range_data.sequence_num + 1); #else LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "\r\n"); LOG_INFO(TRACE_MODULE_APP, "FiRa %s Initiator SEQ NUM %u\r\n", work_mode_table[work_mode_idx], ranging_env.range_data.sequence_num); #endif uwb_app_config.ranging_count++; // clear last round measurement ranging_measurements_clear(); } uint8_t stage_idx = (ind->ranging_stage >= RANGING_RCM && ind->ranging_stage <= RANGING_RRRM) ? (ind->ranging_stage - RANGING_RCM + 1) : 0; #if MCTT_TEST_EN LOG_INFO(TRACE_NO_LEVEL_TAG | TRACE_NO_MODULE_NAME, "t%s tp=%x\r\n", fira_fsm_table[stage_idx], ind->timestamp); #else LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_APP, "[TX][%u] %s\r\n", ind->tx_len, fira_fsm_table[stage_idx]); #endif if (defer_to_report) { defer_to_report = 0; // output result (notify) at last packet uwbapi_report_ranging_data(&ranging_env.range_data); } #if PRINT_PAYLOAD_EN if (ind->tx_len) { LOG_INFO(TRACE_MODULE_APP, " "); for (uint8_t i = 0; i < ind->tx_len; i++) { LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "%02x ", ind->tx_data[i]); } LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "\r\n"); } #endif //#if MCTT_TEST_EN #if 0 if (ind->tx_len) { if((payload_print_len + ind->tx_len) < PAYLOAD_BUF_MAX) { memcpy(payload_print_buf + payload_print_len, ind->tx_data, ind->tx_len); payload_print_len += ind->tx_len; } else { LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "Insufficient payload_print_buf, don't print out tx_data.\r\n"); } } #endif } break; case UWB_PKT_RX_DONE_MSG: { const struct UWB_PKT_RX_DONE_IND_T *ind = (const struct UWB_PKT_RX_DONE_IND_T *)param; if (ind->slot_idx == 0) { uwb_app_config.ranging_count++; } // PDOA AOA CSI RSSI uint8_t responder_idx = ind->slot_idx; if (ind->status == UWB_RX_OK) { if (ind->slot_idx == 0) { uint8_t work_mode_idx = uwb_app_config.session_param.ranging_round_usage < 5 ? uwb_app_config.session_param.ranging_round_usage : 0; ranging_env.range_data.sequence_num = (ranging_env.phy_sts_index - fira_key.phyStsIdxInit) / ranging_env.slots_per_block; #if MCTT_TEST_EN LOG_INFO(TRACE_NO_OPTION, "%s Resp-seq:%u Ridx:%d\r\n", work_mode_table[work_mode_idx], ranging_env.range_data.sequence_num + 1, ranging_env.next_round_index); #else LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_APP | TRACE_NO_OPTION, "\r\n"); LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_APP, "FiRa %s Responder SEQ NUM %u\r\n", work_mode_table[work_mode_idx], ranging_env.range_data.sequence_num); #endif // clear last round measurement ranging_measurements_clear(); int32_t freq_offset = phy_freq_offset_get(); ranging_env.freq_offset_filter = average_filter(freq_offset); #if MCTT_TEST_EN == 0 LOG_INFO(TRACE_MODULE_APP, "CH Freq Offset %d\r\n", ranging_env.freq_offset_filter); #endif #if XTAL_AUTO_TUNE_EN int32_t ppm = ranging_env.freq_offset_filter / (int32_t)(ch_center_freq_map[uwb_app_config.ppdu_params.ch_num] * 1e-6); calib_xtal38m4_load_cap_auto_tune(ppm); #endif } if (ind->ranging_stage == RANGING_RIM || ind->ranging_stage == RANGING_RRM) { struct RANGING_MEASUREMENT_T *range_result = 0; if (ind->ranging_stage == RANGING_RIM) { range_result = &ranging_env.range_data.measurements[0]; range_result->status = STATUS_OK; } else if (ind->ranging_stage == RANGING_RRM) { if (uwb_app_config.session_param.ranging_round_usage == SS_TWR_DEFERRED || uwb_app_config.session_param.ranging_round_usage == DS_TWR_DEFERRED) { responder_idx -= 2; } else if (uwb_app_config.session_param.ranging_round_usage == SS_TWR || uwb_app_config.session_param.ranging_round_usage == DS_TWR) { responder_idx -= 1; } // LOG_INFO(TRACE_MODULE_APP, "responder_idx: %d\r\n", responder_idx); ASSERT(responder_idx <= MEASUREMENT_NUM_MAX, "responder index is over range!"); range_result = &ranging_env.range_data.measurements[responder_idx]; // this status will be cleared after receive report packet range_result->status = STATUS_RANGING_RX_RESPONSE_OK; } if (uwb_app_config.session_param.aoa_result_req) { #if AOA_EN // update PDoA IQ and calculate AoA angles (depends on aoa_aux_cfg) // ~960us -- Keil (AoA - ranging_performance_mode == 0) // ~1ms -- Keil (AoA - ranging_performance_mode == 3) // ~458us -- Keil (PDoA - ranging_performance_mode == 3) // board_led_on(BOARD_LED_1); aoa_calculate(&range_result->aoa_elevation, &range_result->aoa_azimuth); // board_led_off(BOARD_LED_1); aoa_fom_get(&range_result->aoa_elevation_fom, &range_result->aoa_azimuth_fom); #elif PDOA_3D_EN // calculate PDoA angles // 324us ~ 349us -- Keil // board_led_on(BOARD_LED_1); pdoa_3d_calculate(range_result->mac_addr, &range_result->aoa_elevation, &range_result->aoa_azimuth); // board_led_off(BOARD_LED_1); pdoa_fom_get(&range_result->aoa_elevation_fom, &range_result->aoa_azimuth_fom); #endif #if PRINT_PDOA_IQ_EN #if AOA_EN || PDOA_3D_EN float *iq = sts_first_path_iq_get(); #else float *iq = NULL; pdoa_iq_get(&iq); #endif ////////// need to increase slot duration for log printing if (RX_ANT_PORTS_NUM == 2) { LOG_INFO(TRACE_MODULE_APP, "ANT IQ: %f %f\r\n", iq[0], iq[1]); LOG_INFO(TRACE_MODULE_APP, "ANT IQ: %f %f\r\n", iq[2], iq[3]); } else if (RX_ANT_PORTS_NUM == 3) { LOG_INFO(TRACE_MODULE_APP, "ANT IQ: %f %f\r\n", iq[0], iq[1]); LOG_INFO(TRACE_MODULE_APP, "ANT IQ: %f %f\r\n", iq[2], iq[3]); LOG_INFO(TRACE_MODULE_APP, "ANT IQ: %f %f\r\n", iq[4], iq[5]); } else if (RX_ANT_PORTS_NUM == 4) { LOG_INFO(TRACE_MODULE_APP, "ANT IQ: %f %f\r\n", iq[0], iq[1]); LOG_INFO(TRACE_MODULE_APP, "ANT IQ: %f %f\r\n", iq[2], iq[3]); LOG_INFO(TRACE_MODULE_APP, "ANT IQ: %f %f\r\n", iq[4], iq[5]); LOG_INFO(TRACE_MODULE_APP, "ANT IQ: %f %f\r\n", iq[6], iq[7]); } // float pdoa[3]; // pdoa[0] = pdoa_select_get(0, 3); // pdoa[1] = pdoa_select_get(1, 3); // pdoa[2] = pdoa_select_get(2, 3); // LOG_INFO(TRACE_MODULE_APP, "PDOA: %f %f %f\r\n", pdoa[0], pdoa[1], pdoa[2]); // float *sts_rssi = sts_rssi_output_get(); // LOG_INFO(TRACE_MODULE_APP, "STS RSSI: %f %f %f %f\r\n", sts_rssi[0], sts_rssi[1], sts_rssi[2], sts_rssi[3]); #endif } #if RSSI_EN && (MCTT_TEST_EN == 0) LOG_INFO(TRACE_MODULE_APP, "RSSI: %ddBm, SNR: %ddB \r\n", ind->rssi, ind->snr); #endif uint8_t NLoS, FoM; ranging_fom_get(&NLoS, &FoM); range_result->NLoS = NLoS; // LOG_INFO(TRACE_MODULE_APP, "NLoS: %u, FoM: %u\r\n", NLoS, FoM); #if CSI_EN ////////// need to increase slot duration for log printing struct RANGING_TAPS_INF_T taps_inf; ranging_taps_inf_get(&taps_inf); LOG_INFO(TRACE_MODULE_APP, "fap: %d, %f\r\n", taps_inf.fap_loc, taps_inf.fap_pow); LOG_INFO(TRACE_MODULE_APP, "tap1: %d, %f\r\n", taps_inf.tap1_loc, taps_inf.tap1_pow); LOG_INFO(TRACE_MODULE_APP, "tap2: %d, %f\r\n", taps_inf.tap2_loc, taps_inf.tap2_pow); LOG_INFO(TRACE_MODULE_APP, "tap3: %d, %f\r\n", taps_inf.tap3_loc, taps_inf.tap3_pow); // float chtaps_re[128]; // float chtaps_im[128]; // ranging_multi_taps_iq_get(chtaps_re, chtaps_im, 128); // for (uint8_t i = 0; i < 128; i++) //{ // LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "%f, %f\r\n", chtaps_re[i], chtaps_im[i]); //} #endif } else if (ind->ranging_stage == RANGING_RFM) { struct RANGING_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[0]; uint8_t NLoS, FoM; ranging_fom_get(&NLoS, &FoM); if (NLoS > range_result->NLoS) { range_result->NLoS = NLoS; } // LOG_INFO(TRACE_MODULE_APP, "NLoS: %u, FoM: %u\r\n", NLoS, FoM); } #if DYNAMIC_UPDATE_MAIN_ANTENNA_EN else if (ind->ranging_stage == RANGING_RRRM) { uint8_t initiator_tx_slot_count; if (uwb_app_config.session_param.ranging_round_usage == DS_TWR_DEFERRED) { initiator_tx_slot_count = 4; // rcm + poll + final + report } else if (uwb_app_config.session_param.ranging_round_usage == SS_TWR_DEFERRED) { initiator_tx_slot_count = 3; // rcm + poll + report } else if (uwb_app_config.session_param.ranging_round_usage == DS_TWR) { initiator_tx_slot_count = 2; // poll + final } else // SS_TWR { initiator_tx_slot_count = 1; // poll } responder_idx = ind->slot_idx - ranging_env.responder_num - initiator_tx_slot_count; } #endif if ((ind->ranging_stage == RANGING_RRM && uwb_app_config.session_param.ranging_round_usage == SS_TWR) || (ind->ranging_stage == RANGING_RFM && uwb_app_config.session_param.ranging_round_usage == DS_TWR) || (ind->ranging_stage == RANGING_MRM && uwb_app_config.session_param.ranging_round_usage == DS_TWR_DEFERRED) || (ind->ranging_stage == RANGING_MRM && uwb_app_config.session_param.ranging_round_usage == SS_TWR_DEFERRED)) { int res = measure_report_handler(ind); if (res) { // LOG_INFO(TRACE_MODULE_APP, "ToF %d\r\n", ranging_env.tof); struct RANGING_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[0]; ranging_env.range_data.measurements_num = 1; range_result->status = STATUS_OK; if (uwb_app_config.session_param.aoa_result_req == 0) { range_result->aoa_azimuth = 0; } #if FILTER_EN if (uwb_app_config.filter_en) { #if KF_EN int16_t *azimuth = (uwb_app_config.session_param.aoa_result_req ? &range_result->aoa_azimuth : NULL); #else int16_t *azimuth = &range_result->aoa_azimuth; #endif int16_t *elevation = NULL; #if PDOA_3D_EN if (ANT_PATTERN != ANT_PATTERN_LINEAR) { elevation = (uwb_app_config.session_param.aoa_result_req ? &range_result->aoa_elevation : NULL); } #endif // filter process ranging_result_filter(range_result->mac_addr, &range_result->distance, azimuth, elevation); } #endif if (uwb_app_config.session_param.aoa_result_req) { board_ranging_result_correct(&range_result->distance, &range_result->aoa_azimuth, &range_result->aoa_elevation); } #ifdef UWB_UCI_TEST_EN #if PDOA_3D_EN LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_APP, "Distance %ucm, PDoA Azimuth %d Elevation %d Azimuth FoM %u\r\n", range_result->distance, mk_q7_to_s16(range_result->aoa_azimuth), mk_q7_to_s16(range_result->aoa_elevation), range_result->aoa_azimuth_fom); #elif AOA_EN LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_APP, "Distance %ucm, AoA Azimuth %d Elevation %d Azimuth FoM %u\r\n", range_result->distance, mk_q7_to_s16(range_result->aoa_azimuth), mk_q7_to_s16(range_result->aoa_elevation), range_result->aoa_azimuth_fom); #else LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_APP, "Distance %ucm\r\n", (int16_t)range_result->distance); #endif #endif uint16_t peer_addr = ranging_initiator_addr_get(); range_result->mac_addr[0] = peer_addr & 0xff; range_result->mac_addr[1] = (peer_addr >> 8) & 0xff; if ((uwb_app_config.session_param.ranging_round_usage == DS_TWR_DEFERRED) || (uwb_app_config.session_param.ranging_round_usage == SS_TWR_DEFERRED)) { defer_to_report = 1; } else { // output result (notify) uwbapi_report_ranging_data(&ranging_env.range_data); } // int8_t expected_rssi = ranging_expected_rssi_get(ranging_tx_power_get(), range_result->distance, 2, 0); // LOG_INFO(TRACE_MODULE_APP, "Expected RSSI: %ddBm\r\n", expected_rssi); } } uint8_t stage_idx = (ind->ranging_stage >= RANGING_RCM && ind->ranging_stage <= RANGING_RRRM) ? (ind->ranging_stage - RANGING_RCM + 1) : 0; #if MCTT_TEST_EN if (ind->ranging_stage == RANGING_RCM || ind->ranging_stage == RANGING_MRM || ind->ranging_stage == RANGING_RRRM) { if (ind->ranging_stage != RANGING_RRRM) { LOG_INFO(TRACE_NO_LEVEL_TAG | TRACE_NO_MODULE_NAME, "r%s tp=%x,%x,%x,%d\r\n", fira_fsm_table[stage_idx], ind->timestamp, ind->phy_header, ranging_env.phy_sts_index, ind->slot_idx); } else { LOG_INFO(TRACE_NO_LEVEL_TAG | TRACE_NO_MODULE_NAME, "r%s,%x,%x,%d\r\n", fira_fsm_table[stage_idx], ind->phy_header, ranging_env.phy_sts_index, ind->slot_idx); } } else { LOG_INFO(TRACE_NO_LEVEL_TAG | TRACE_NO_MODULE_NAME, "r%s\r\n", fira_fsm_table[stage_idx]); } #else #if DYNAMIC_UPDATE_MAIN_ANTENNA_EN if ((ind->ranging_stage == RANGING_RRM) || (ind->ranging_stage == RANGING_RRRM)) { uint8_t main_ant = ranging_env.responder_list[responder_idx].main_ant_id; LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_APP, "[RX][%u][%u] %s\r\n", ind->rx_len, main_ant, fira_fsm_table[stage_idx]); } else #endif { LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_APP, "[RX][%u] %s\r\n", ind->rx_len, fira_fsm_table[stage_idx]); } #endif #if PRINT_PAYLOAD_EN if (ind->rx_len) { LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_APP, " "); for (uint8_t i = 0; i < ind->rx_len; i++) { LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "%02x ", ind->rx_data[i]); } LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "\r\n"); } #endif #if MCTT_TEST_EN if (ind->rx_len) { if ((payload_print_len + ind->rx_len) < PAYLOAD_BUF_MAX) { memcpy(payload_print_buf + payload_print_len, ind->rx_data, ind->rx_len); payload_print_len += ind->rx_len; } else { LOG_INFO(TRACE_NO_OPTION, "Insufficient payload_print_buf, don't print out rx_data.\r\n"); } } #endif } else { #if DYNAMIC_UPDATE_MAIN_ANTENNA_EN LOG_INFO(TRACE_MODULE_APP, "[%u] UWB RX fail 0x%04x\r\n", ranging_env.responder_list[responder_idx].main_ant_id, ind->status); #else LOG_INFO(TRACE_MODULE_APP, "UWB RX fail 0x%04x\r\n", ind->status); #endif #if RSSI_EN && (MCTT_TEST_EN == 0) if (ind->status != 0x0830) { LOG_INFO(TRACE_MODULE_APP, "RSSI: %ddBm, SNR: %ddB \r\n", ind->rssi, ind->snr); } #endif } if ((ind->ranging_stage == RANGING_RRRM) && (ranging_env.result_flag & 0xFF)) { int res = ranging_result_report_handler(ind); if (res) { // output result (notify) uwbapi_report_ranging_data(&ranging_env.range_data); } } } break; #if MCTT_TEST_EN case RANGING_ROUND_PRINT_MSG: { if (payload_print_len > 0) { for (uint16_t i = 0; i < payload_print_len; i++) { if (i > 0 && payload_print_buf[i] == 0x49 && payload_print_buf[i + 1] == 0x2B) LOG_INFO(TRACE_NO_OPTION, "\r\n"); LOG_INFO(TRACE_NO_OPTION, "%02x ", payload_print_buf[i]); } LOG_INFO(TRACE_NO_OPTION, "\r\n"); memset(payload_print_buf, 0x0, PAYLOAD_BUF_MAX); payload_print_len = 0; } } break; #endif default: break; } } // Handle events else { } // board_led_off(BOARD_LED_1); }