/* * 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_simple.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. */ /*************************************************************************************************/ 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: { uint8_t count = ranging_count_get(); if (count == ranging_env.count_last) { LOG_INFO(TRACE_MODULE_APP, "Ranging was suspended %u\r\n", count); ranging_restart(); } else { // LOG_INFO(TRACE_MODULE_APP, "Ranging count %u %u\r\n", ranging_env.count_last, count); ranging_env.count_last = 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; uint8_t flag_print = 1; switch (ind->ranging_stage) { case RANGING_SYNC: { LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "\r\n"); LOG_INFO(TRACE_MODULE_APP, "[TX][%u] Sync ", ind->tx_len); } break; case RANGING_CFG: { LOG_INFO(TRACE_MODULE_APP, "[TX][%u] Config ", ind->tx_len); } break; case RANGING_POLL: { LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "\r\n"); struct RANGING_COMMON_MSG_T *p_msg = (struct RANGING_COMMON_MSG_T *)ind->tx_data; LOG_INFO(TRACE_MODULE_APP, "DS-TWR Initiator SEQ NUM %u\r\n", p_msg->seqNum); LOG_INFO(TRACE_MODULE_APP, "[TX][%u] Poll ", ind->tx_len); } break; case RANGING_RESPONSE: { LOG_INFO(TRACE_MODULE_APP, "[TX][%u] Response ", ind->tx_len); } break; case RANGING_FINAL: { LOG_INFO(TRACE_MODULE_APP, "[TX][%u] Final", ind->tx_len); } break; default: flag_print = 2; break; } if (flag_print == 1) { #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]); } } #endif LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "\r\n"); } } 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) { uint8_t flag_print = 1; switch (ind->ranging_stage) { case RANGING_SYNC: { LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "\r\n"); LOG_INFO(TRACE_MODULE_APP, "[RX][%u] Sync ", ind->rx_len); } break; case RANGING_CFG: { LOG_INFO(TRACE_MODULE_APP, "[RX][%u] Config ", ind->rx_len); } break; case RANGING_POLL: { LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "\r\n"); LOG_INFO(TRACE_MODULE_APP, "[RX][%u] Poll ", ind->rx_len); } break; case RANGING_RESPONSE: { LOG_INFO(TRACE_MODULE_APP, "[RX][%u] Response ", ind->rx_len); } break; case RANGING_FINAL: { LOG_INFO(TRACE_MODULE_APP, "[RX][%u] Final ", ind->rx_len); } break; default: flag_print = 2; break; } if (flag_print == 1) { #if PRINT_PAYLOAD_EN for (uint8_t ii = 0; ii < ind->rx_len; ii++) { LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "%02x ", ind->rx_data[ii]); } #endif LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "\r\n"); #if RSSI_EN LOG_INFO(TRACE_MODULE_APP, "RSSI: %ddBm, SNR: %ddB \r\n", ind->rssi, ind->snr); #endif } if (ind->ranging_stage == RANGING_POLL) { 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 struct RANGING_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[0]; 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_RESPONSE) { uint8_t NLoS, FoM; ranging_fom_get(&NLoS, &FoM); // LOG_INFO(TRACE_MODULE_APP, "NLoS: %u, FoM: %u\r\n", NLoS, FoM); } else if (ind->ranging_stage == RANGING_FINAL) { struct RANGING_COMMON_MSG_T *p_final_msg = (struct RANGING_COMMON_MSG_T *)ind->rx_data; int64_t Tround1 = 0; int64_t Tround2 = 0; int64_t Treply1 = 0; int64_t Treply2 = 0; if (ranging_env.responder_final_flag) { Tround2 = ranging_tround(DEV_ROLE_RESPONDER, 0); Treply1 = ranging_treply(DEV_ROLE_RESPONDER, 0); // recv tx_poll_timestamp, rx_response_timestamp, tx_final_timestamp int64_t buff_timestamp[3] = {0}; for (int j = 0; j < 3; j++) { for (int i = 4; i >= 0; i--) { buff_timestamp[j] = (buff_timestamp[j] << 8) | p_final_msg->user_data[j * 5 + i]; } } Tround1 = buff_timestamp[1] - buff_timestamp[0]; Treply2 = buff_timestamp[2] - buff_timestamp[1]; } if ((Tround1) && (Treply1) && (Tround2) && (Treply2)) { int64_t tof_i = (Tround1 * Tround2 - Treply1 * Treply2) / (Tround1 + Tround2 + Treply1 + Treply2); // outlier filter if (tof_i < 0) { tof_i = 0; } ranging_env.tof = (uint32_t)tof_i; double 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); 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, "Raw Distance %ucm\r\n", range_result->distance); } struct RANGE_DATA_T *range_data = &ranging_env.range_data; range_data->mac_addr_mode = MAC_ADDR_LONG; range_data->measurements_num = 1; memcpy(range_result->mac_addr, uwbs_peer_long_addr_get(), 8); range_result->slot_idx = ranging_env.responder_slot_idx; 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 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 // 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"); } } } else { LOG_INFO(TRACE_MODULE_APP, "UWB RX fail 0x%04x, stage %d\r\n", ind->status, ind->ranging_stage); } } break; default: break; } } // Handle events else if (event) { } }