/* * Copyright (c) 2019-2025 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 FILTER_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 FILTER_EN extern void ranging_result_filter(uint8_t *mac_addr, uint16_t *distance, int16_t *azimuth, int16_t *elevation); #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); 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->status != UWB_TX_OK) { LOG_INFO(TRACE_MODULE_APP, "UWB TX fail 0x%04x\r\n", ind->status); break; } 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; LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "\r\n"); LOG_INFO(TRACE_MODULE_APP, "FiRa %s Controller SEQ NUM %u\r\n", work_mode_table[work_mode_idx], ranging_env.range_data.sequence_num); 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; LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_APP, "[TX][%u] %s\r\n", ind->tx_len, fira_fsm_table[stage_idx]); 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 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; 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 Controlee SEQ NUM %u\r\n", work_mode_table[work_mode_idx], ranging_env.range_data.sequence_num); // clear last round measurement ranging_measurements_clear(); int32_t freq_offset = phy_freq_offset_get(); ranging_env.freq_offset_filter = average_filter(freq_offset); LOG_INFO(TRACE_MODULE_APP, "CH Freq Offset %d\r\n", ranging_env.freq_offset_filter); #if (X38M4_AUTO_TUNE_EN == 1) int32_t ppm = ranging_env.freq_offset_filter / (int32_t)(uwb_ch_freq_table[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 TWR_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) // ~530us -- Keil (AoA - ranging_performance_mode == 3) // board_led_on(BOARD_LED_2); aoa_calculate(&range_result->aoa_elevation, &range_result->aoa_azimuth); // board_led_off(BOARD_LED_2); aoa_fom_get(&range_result->aoa_elevation_fom, &range_result->aoa_azimuth_fom); #elif PDOA_3D_EN // calculate PDoA angles // ~410us -- Keil // board_led_on(BOARD_LED_2); pdoa_3d_calculate(range_result->mac_addr, &range_result->aoa_elevation, &range_result->aoa_azimuth); // board_led_off(BOARD_LED_2); 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_result_get(0, 3); // pdoa[1] = pdoa_result_get(1, 3); // pdoa[2] = pdoa_result_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 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); } else if (ind->ranging_stage == RANGING_RFM) { struct TWR_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 TWR_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) { int16_t *azimuth = (uwb_app_config.session_param.aoa_result_req ? &range_result->aoa_azimuth : NULL); 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); } // calculate_first_tap_power(1, 2); // print_preamble_cir(ranging_env.range_data.sequence_num, 1, 2); // print_sts_cir(1, 2); } } uint8_t stage_idx = (ind->ranging_stage >= RANGING_RCM && ind->ranging_stage <= RANGING_RRRM) ? (ind->ranging_stage - RANGING_RCM + 1) : 0; #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]); } #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 } 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 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); // calculate_first_tap_power(0, 1); // print_preamble_cir(ranging_env.range_data.sequence_num, 0, 1); // print_sts_cir(0, 1); } } } break; default: break; } } // Handle events else { } // board_led_off(BOARD_LED_1); }