/* * 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_ss_twr.h" #include "lib_aoa.h" #include "lib_ranging.h" #if KF_EN #include "lib_kf.h" #endif #include "board.h" #define PRINT_PAYLOAD_EN 0 #define PDOA_PRINT_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 /*************************************************************************************************/ /*! * \brief WSF event handler for ranging task. * * \param event WSF event mask. * \param msg WSF message. * * \return None. */ /*************************************************************************************************/ int32_t tof_i; double tof_f; int64_t Tround; int64_t Treply; 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, 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->ranging_stage == RANGING_POLL) { uint16_t seq_num = READ_SHORT(&ind->tx_data[2]); LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "\r\n"); LOG_INFO(TRACE_MODULE_APP, "SS-TWR Initiator SEQ NUM %u\r\n", seq_num); 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); LOG_INFO(TRACE_MODULE_APP, "Treply %u \r\n", (uint32_t)ranging_treply()); } #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) { uint16_t session_id = READ_SHORT(&ind->rx_data[0]); if (session_id == ranging_user_pkt.session_id) { if (ind->ranging_stage == RANGING_POLL) { uint16_t seq_num = READ_SHORT(&ind->rx_data[2]); LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "\r\n"); LOG_INFO(TRACE_MODULE_APP, "SS-TWR Responder SEQ NUM %u\r\n", seq_num); LOG_INFO(TRACE_MODULE_APP, "[RX][%u] Poll\r\n", ind->rx_len); #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); // LOG_INFO(TRACE_MODULE_APP, "NLoS: %u, FoM: %u\r\n", NLoS, FoM); #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 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 #if USER_DEFINED_DATA_REPORT_EN if (ind->rx_len - POLL_MSG_USER_DATA_IDX) { uwbapi_report_user_defined_data((uint8_t)(ind->rx_len - POLL_MSG_USER_DATA_IDX), &ind->rx_data[POLL_MSG_USER_DATA_IDX]); } #endif } else if (ind->ranging_stage == RANGING_RESPONSE) { LOG_INFO(TRACE_MODULE_APP, "[RX][%u] Response\r\n", ind->rx_len); #if RSSI_EN LOG_INFO(TRACE_MODULE_APP, "RSSI: %ddBm, SNR: %ddB \r\n", ind->rssi, ind->snr); #endif 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); Tround = ranging_tround(); Treply = 0; for (int i = 4; i >= 0; i--) { Treply = (Treply << 8) | ind->rx_data[MSG_TREPLY_IDX + i]; } LOG_INFO(TRACE_MODULE_APP, "Tround %u Treply %u \r\n", (uint32_t)Tround, (uint32_t)Treply); if ((Tround) && (Treply)) { // corrected by frequency offset Tround = (int64_t)((double)Tround * (1 - (double)freq_offset_filter / ch_center_freq_map[uwb_app_config.ppdu_params.ch_num])); tof_i = (int32_t)(Tround - Treply) / 2; // outlier filter if (tof_i < 0) { tof_i = 0; } ranging_env.tof = (uint32_t)(tof_i); tof_f = (double)TIMESTAMP_UNIT_TO_NS(ranging_env.tof); struct RANGING_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[0]; // update distance result range_result->distance = (uint16_t)(tof_f * 0.299702547 * VP_VAL - RANGING_CORR); LOG_INFO(TRACE_MODULE_APP, "tof_f %lf,tof_i %d\r\n", tof_f,tof_i); range_result->status = STATUS_OK; if (ranging_frame_type_get() == SP1) { range_result->status = sts_valid_check() ? STATUS_OK : STATUS_FAILED; } // STS valid if (range_result->status == STATUS_OK) { #if AOA_EN || PDOA_PRINT_EN // update PDoA IQ and calculate AoA angles (depends on aoa_aux_cfg) aoa_calculate(NULL, &range_result->aoa_azimuth); #endif #if PDOA_PRINT_EN 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]); // float *iq = sts_first_path_iq_get(); // LOG_INFO(TRACE_MODULE_APP, "ANT0 IQ: %f %f\r\n", iq[0], iq[1]); // LOG_INFO(TRACE_MODULE_APP, "ANT1 IQ: %f %f\r\n", iq[2], iq[3]); // LOG_INFO(TRACE_MODULE_APP, "ANT2 IQ: %f %f\r\n", iq[4], iq[5]); // LOG_INFO(TRACE_MODULE_APP, "ANT3 IQ: %f %f\r\n", iq[6], iq[7]); #endif #if FILTER_EN if (uwb_app_config.filter_en) { // filter process ranging_result_filter(&range_result->distance, &range_result->aoa_azimuth, &range_result->aoa_elevation); } #endif uint8_t aoa_en = uwb_app_config.session_param.aoa_result_req; if (aoa_en) { uint8_t fom; aoa_fom_get(NULL, &fom); board_ranging_result_correct(&range_result->distance, &range_result->aoa_azimuth, &range_result->aoa_elevation); LOG_INFO(TRACE_MODULE_APP, "Distance %ucm, AoA Azimuth %d FoM %u\r\n", range_result->distance, mk_q7_to_s16(range_result->aoa_azimuth), fom); } else { range_result->aoa_azimuth = 0; LOG_INFO(TRACE_MODULE_APP, "Distance %ucm\r\n", range_result->distance); } struct RANGE_DATA_T *range_data = &ranging_env.range_data; range_data->ranging_type = 0x01; range_data->measurements_num = 1; uint16_t target_addr = uwbs_peer_short_addr_get(); range_result->mac_addr[0] = target_addr & 0xff; range_result->mac_addr[1] = (target_addr >> 8) & 0xff; // uint8_t NLoS, FoM; // ranging_fom_get(&NLoS, &FoM); // LOG_INFO(TRACE_MODULE_APP, "NLoS: %u, FoM: %u\r\n", NLoS, FoM); // output result uwbapi_report_ranging_data(range_data); } else { LOG_INFO(TRACE_MODULE_APP, "STS Invalid\r\n"); } } else { LOG_INFO(TRACE_MODULE_APP, "Timestamp error\r\n"); } } #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 { LOG_INFO(TRACE_MODULE_APP, "Session ID mismatch 0x%02x\r\n", session_id); } } else { LOG_INFO(TRACE_MODULE_APP, "UWB RX fail 0x%04x\r\n", ind->status); } } break; default: break; } } // Handle events else if (event) { } }