/* * 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 "ranging_ccc.h" #include "uwb_api.h" #include "lib_ccc.h" #include "lib_aoa.h" #include "lib_ranging.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 FILTER_EN static void ranging_result_filter(uint16_t *distance, int16_t *azimuth, int16_t *elevation) { if ((distance == NULL) || (azimuth == NULL) || (elevation == NULL)) { return; } #if KF_EN float post_range, post_azimuth, post_elevation; float azimuth_meas = mk_q7_to_f32(*azimuth); float elevation_meas = mk_q7_to_f32(*elevation); float range_meas = (float)*distance / 100; // call filter uint16_t target_addr = uwbs_peer_short_addr_get(); uint8_t mac_addr[8]; memset(mac_addr, 0, 8); mac_addr[0] = target_addr & 0xff; mac_addr[1] = (target_addr >> 8) & 0xff; loc_kf_filter(range_meas, KF_DATA_TYPE_RANGING, mac_addr, &post_range); if (uwb_app_config.session_param.aoa_result_req) { loc_kf_filter(azimuth_meas, KF_DATA_TYPE_AZIMUTH, mac_addr, &post_azimuth); loc_kf_filter(elevation_meas, KF_DATA_TYPE_ELEVATION, mac_addr, &post_elevation); } else { post_azimuth = azimuth_meas; post_elevation = elevation_meas; } // update distance *distance = (uint16_t)(post_range * 100); // update angle *azimuth = mk_f32_to_q7(post_azimuth); *elevation = mk_f32_to_q7(post_elevation); // LOG_INFO(TRACE_MODULE_APP, "$%u %u %d %d %d %d;\r\n", (uint16_t)(range_meas*100),(uint16_t)(post_range*100),(int16_t)azimuth_meas,(int16_t)post_azimuth, // (int16_t)elevation_meas, (int16_t)post_elevation); #else float post_range, post_azimuth; int azimuth_meas = mk_q7_to_s16(*azimuth); float range_meas = (float)*distance; // call filter loc_post_filter(0, range_meas, azimuth_meas, &post_range, &post_azimuth); // update distance *distance = (uint16_t)(post_range); // update angle *azimuth = mk_f32_to_q7(post_azimuth); // LOG_INFO(TRACE_MODULE_APP, "$%u %u %d %d;\r\n", (uint16_t)(range_meas*100), (uint16_t)(post_range*100),(int16_t)azimuth_meas, (int16_t)post_azimuth); #endif } #endif #if RANGING_FOM_FILTER_EN static void ranging_report_debug_csi_while_failed(void) { if (ranging_debug_csi_en_get()) { if (uwb_app_config.session_param.device_role == DEV_ROLE_RESPONDER) { uwbapi_report_debug_csi_data(&debug_csi); } } else { LOG_INFO(TRACE_MODULE_APP, "debug_csi_en is disabled, message drop!!!\r\n"); } } static void ranging_report_debug_csi_while_successed(uint16_t distance, bool overflow) { if (ranging_debug_csi_en_get()) { if (overflow) { // Transaction overflow, RESPONSE SP3 frame from this responder cannot be processed by the device debug_csi.ranging_status = 0x01; } debug_csi.distance_cm = distance; debug_csi.azimuth = ranging_env.azimuth; debug_csi.ranging_fom = ranging_fom_calculate(&debug_csi, 0, NULL, 0); debug_csi.azimuth_fom = ranging_env.azimuth_fom; calculate_first_tap_power(4, 2); // print_preamble_chest(4, 2); uwbapi_report_debug_csi_data(&debug_csi); } else { LOG_INFO(TRACE_MODULE_APP, "debug_csi_en is disabled, message drop!!!\r\n"); } } #endif /*************************************************************************************************/ /*! * \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; 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, count); ranging_env.count_last = ranging_env.count; } } break; case RANGING_UPDATE_KEYS: { const struct RANGING_UPD_KEYS_T *upd_keys = (const struct RANGING_UPD_KEYS_T *)param; ranging_update_keys(upd_keys->sts_idx, upd_keys->key_type); } 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->ranging_stage == RANGING_PRE_POLL) { LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "\r\n"); LOG_INFO(TRACE_MODULE_APP, "CCC DS-TWR Deferred Initiator SEQ NUM %u ridx %u sidx %u\r\n", ind->block_idx, ind->round_idx, ind->sts_idx); LOG_INFO(TRACE_MODULE_APP, "[TX][%u] Pre-Poll\r\n", ind->tx_len); } else if (ind->ranging_stage == RANGING_POLL) { LOG_INFO(TRACE_MODULE_APP, "[TX][%u] Poll\r\n", ind->tx_len); } else if (ind->ranging_stage == RANGING_RESPONSE) { LOG_INFO(TRACE_MODULE_APP, "[TX][%u] Response\r\n", ind->tx_len); } else if (ind->ranging_stage == RANGING_FINAL) { LOG_INFO(TRACE_MODULE_APP, "[TX][%u] Final\r\n", ind->tx_len); } else if (ind->ranging_stage == RANGING_FINAL_DATA) { LOG_INFO(TRACE_MODULE_APP, "[TX][%u] Final-Data\r\n", ind->tx_len); } else { LOG_ERROR(TRACE_MODULE_APP, "unknown stage 0x%x\r\n", ind->ranging_stage); } #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 } 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->status == UWB_RX_OK) { if (ind->ranging_stage == RANGING_PRE_POLL) { LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "\r\n"); LOG_INFO(TRACE_MODULE_APP, "CCC DS-TWR Deferred Responder SEQ NUM %u ridx %u sidx %u\r\n", ind->block_idx, ind->round_idx, ind->sts_idx); LOG_INFO(TRACE_MODULE_APP, "[RX][%u] Pre-Poll\r\n", ind->rx_len); int32_t freq_offset = phy_freq_offset_get(); int32_t freq_offset_filter = average_filter(freq_offset); LOG_INFO(TRACE_MODULE_APP, "CH Freq Offset %d\r\n", freq_offset_filter); #if XTAL_AUTO_TUNE_EN int32_t ppm = 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 } else if (ind->ranging_stage == RANGING_POLL) { #if DYNAMIC_UPDATE_MAIN_ANTENNA_EN LOG_INFO(TRACE_MODULE_APP, "[%u][%u][RX][%u] Poll\r\n", ranging_env.responder_slot_idx, ranging_env.main_ant_id[0], ind->rx_len); #else LOG_INFO(TRACE_MODULE_APP, "[RX][%u] Poll\r\n", ind->rx_len); #endif } else if (ind->ranging_stage == RANGING_RESPONSE) { #if DYNAMIC_UPDATE_MAIN_ANTENNA_EN LOG_INFO(TRACE_MODULE_APP, "[%u][%u][RX][%u] Response\r\n", ind->slot_idx + 1, ranging_env.main_ant_id[ind->slot_idx], ind->rx_len); #else LOG_INFO(TRACE_MODULE_APP, "[RX][%u] Response\r\n", ind->rx_len); #endif } else if (ind->ranging_stage == RANGING_FINAL) { #if DYNAMIC_UPDATE_MAIN_ANTENNA_EN LOG_INFO(TRACE_MODULE_APP, "[%u][%u][RX][%u] Final\r\n", ranging_env.responder_slot_idx, ranging_env.main_ant_id[0], ind->rx_len); #else LOG_INFO(TRACE_MODULE_APP, "[RX][%u] Final\r\n", ind->rx_len); #endif 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(&ranging_env.elevation, &ranging_env.azimuth); // board_led_off(BOARD_LED_1); aoa_fom_get(&ranging_env.elevation_fom, &ranging_env.azimuth_fom); #elif PDOA_3D_EN // calculate PDoA angles // 324us ~ 349us -- Keil // board_led_on(BOARD_LED_1); pdoa_3d_calculate(uwbs_local_long_addr_get(), &ranging_env.elevation, &ranging_env.azimuth); // board_led_off(BOARD_LED_1); pdoa_fom_get(&ranging_env.elevation_fom, &ranging_env.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 CSI_EN 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); #endif } else if (ind->ranging_stage == RANGING_FINAL_DATA) { LOG_INFO(TRACE_MODULE_APP, "[RX][%u] Final-Data\r\n", ind->rx_len); } else { LOG_ERROR(TRACE_MODULE_APP, "unknown stage 0x%x\r\n", ind->ranging_stage); } #if PRINT_PAYLOAD_EN if (ind->rx_len) { LOG_INFO(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][%u] UWB RX fail 0x%04x\r\n", ind->slot_idx + 1, ranging_env.main_ant_id[ind->slot_idx], ind->status); #else LOG_INFO(TRACE_MODULE_APP, "UWB RX fail 0x%04x\r\n", ind->status); #endif #if RANGING_FOM_FILTER_EN ranging_report_debug_csi_while_failed(); memset(&debug_csi, 0x00, sizeof(debug_csi)); #endif } #if RSSI_EN LOG_INFO(TRACE_MODULE_APP, "RSSI: %ddBm, SNR: %ddB \r\n", ind->rssi, ind->snr); #endif } break; case RANGING_REPORT_MSG: { const struct RANGING_REPORT_IND_T *ind = (const struct RANGING_REPORT_IND_T *)param; int64_t tof_i = (ind->Tround1 * ind->Tround2 - ind->Treply1 * ind->Treply2) / (ind->Tround1 + ind->Tround2 + ind->Treply1 + ind->Treply2); // outlier filter if (tof_i < 0) { tof_i = 0; } double tof_f = (double)TIMESTAMP_UNIT_TO_NS((uint32_t)tof_i); uint16_t distance = (uint16_t)(tof_f * 0.299702547 * VP_VAL - RANGING_CORR); #if FILTER_EN if (uwb_app_config.filter_en) { // filter process LOG_INFO(TRACE_MODULE_APP, "Raw Distance %ucm\r\n", distance); ranging_result_filter(&distance, &ranging_env.azimuth, &ranging_env.elevation); } #endif if (uwb_app_config.session_param.aoa_result_req) { board_ranging_result_correct(&distance, &ranging_env.azimuth, &ranging_env.elevation); LOG_INFO(TRACE_MODULE_APP, "Peer %X, Slot idx %d, Distance %ucm Azimuth %d FoM %d\r\n", uwbs_peer_short_addr_get(), ranging_env.responder_slot_idx, distance, mk_q7_to_s16(ranging_env.azimuth), ranging_env.azimuth_fom); } else { LOG_INFO(TRACE_MODULE_APP, "Peer %X, Slot idx %d, Distance %ucm\r\n", uwbs_peer_short_addr_get(), ranging_env.responder_slot_idx, distance); } if (!ranging_debug_csi_en_get()) { struct RANGE_DATA_T *range_data = &ranging_env.range_data; struct RANGING_MEASUREMENT_T *range_result = &range_data->measurements[0]; range_result->NLoS = MAX(ranging_env.poll_nlos, ranging_env.final_nlos); range_result->distance = distance; range_result->aoa_azimuth = ranging_env.azimuth; range_result->aoa_azimuth_fom = ranging_env.azimuth_fom; range_data->measurements_num = 1; range_data->ranging_type = 0x01; range_data->mac_addr_mode = 0; range_data->sequence_num = ind->block_idx; range_data->session_id = ranging_env.session_id; range_data->ranging_interval = PHY_TIMER_COUNT_TO_MS(ranging_env.block_duration); uint16_t target_addr = ranging_responder_addr_get(0); range_result->mac_addr[0] = target_addr & 0xff; range_result->mac_addr[1] = (target_addr >> 8) & 0xff; range_result->slot_idx = ranging_env.responder_slot_idx; range_result->status = STATUS_OK; // output result uwbapi_report_ranging_data(range_data); } #if RANGING_FOM_FILTER_EN ranging_report_debug_csi_while_successed(distance, (ind->status == 0) && (distance == 0)); LOG_INFO(TRACE_MODULE_APP, "Poll-FoM %d, Response-FoM %d Final-FoM %d, Poll-NLoS %d, Response-NLoS %d Final-NLoS %d, C-FoM %d\r\n", ranging_env.poll_fom, ind->resp_fom, ranging_env.final_fom, ranging_env.poll_nlos, 0, ranging_env.final_nlos, debug_csi.ranging_fom); memset(&debug_csi, 0x00, sizeof(debug_csi)); #endif } break; default: break; } } // Handle events else if (event) { } }