成功移植g_com_map表逻辑，初步测试能读能写，并且TDOA效果和官方一致

chen

7 天以前 adfc7e798b9cbdd022bf8df971843436912a0fe5

 keil/include/components/app/src/ranging_custom_task.c

@@ -1,5 +1,5 @@
/*
 * Copyright (c) 2019-2023 Beijing Hanwei Innovation Technology Ltd. Co. and
 * Copyright (c) 2019-2025 Beijing Hanwei Innovation Technology Ltd. Co. and
 * its subsidiaries and affiliates (collectly called MKSEMI).
 *
 * All rights reserved.
@@ -46,7 +46,7 @@
#include "ranging_custom.h"
#include "lib_aoa.h"
#include "lib_ranging.h"
#if KF_EN
#if FILTER_EN
#include "lib_kf.h"
#endif
#if PDOA_3D_EN
@@ -72,7 +72,6 @@
        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);
@@ -102,19 +101,6 @@

    // 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

@@ -155,9 +141,16 @@
            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->ranging_stage == RANGING_POLL)
                {
                    uint16_t seq_num = READ_SHORT(&ind->tx_data[2]);
                    ranging_env.range_data.sequence_num = READ_SHORT(&ind->tx_data[2]);

#ifdef SE_DEMO_EN
                    if (ind->tx_len > (MSG_HEADER_LEN + MSG_POLL_USER_DATA_IDX))
@@ -169,11 +162,11 @@
                    LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "\r\n");
                    if (uwb_app_config.ranging_flow_mode == (uint8_t)RANGING_FLOW_CONTENTION)
                    {
                        LOG_INFO(TRACE_MODULE_APP, "Custom DS-TWR Contention Initiator SEQ NUM %u\r\n", seq_num);
                        LOG_INFO(TRACE_MODULE_APP, "Custom DS-TWR Contention Initiator SEQ NUM %u\r\n", ranging_env.range_data.sequence_num);
                    }
                    else
                    {
                        LOG_INFO(TRACE_MODULE_APP, "Custom DS-TWR Initiator SEQ NUM %u\r\n", seq_num);
                        LOG_INFO(TRACE_MODULE_APP, "Custom DS-TWR Initiator SEQ NUM %u\r\n", ranging_env.range_data.sequence_num);
                    }
                    LOG_INFO(TRACE_MODULE_APP, "[TX][%u] Poll %s\r\n", ind->tx_len, ind->status == UWB_TX_OK ? "" : "TX Fail");
                }
@@ -191,9 +184,9 @@
                {
                    LOG_INFO(TRACE_MODULE_APP, "[TX][%u] Final\r\n", ind->tx_len);

                    // calculate_first_tap_power(1, 1);
                    // print_preamble_chest(1, 1);
                    // print_sts_ch_taps(1);
                    // calculate_first_tap_power(0, 1);
                    // print_preamble_cir(ranging_env.range_data.sequence_num, 0, 1);
                    // print_sts_cir(0, 1);
                }
                else if (ind->ranging_stage == RANGING_RESULT)
                {
@@ -219,8 +212,14 @@
                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 (ranging_env.synced == 0)
                    {
                        LOG_INFO(TRACE_MODULE_APP, "[RX][%u] Not a sync packet\r\n", ind->rx_len);
                        break;
                    }

                    const struct RANGING_USER_PKT_T *usr_pkt = (const struct RANGING_USER_PKT_T *)ind->rx_data;
                    if ((ind->ranging_stage == RANGING_POLL) && ranging_env.synced)
                    if (ind->ranging_stage == RANGING_POLL)
                    {
                        ranging_env.range_data.sequence_num = usr_pkt->seq_num;
                        LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "\r\n");
@@ -235,13 +234,6 @@
                        LOG_INFO(TRACE_MODULE_APP, "[RX][%u][%d] Poll\r\n", ind->rx_len, ranging_env.main_ant_id);
                        // LOG_INFO(TRACE_MODULE_APP, "[RX]Peer Tx Power Level %d\r\n", usr_pkt->msg.poll_msg[MSG_POLL_TX_PWR_IDX]);

                        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
                        uint8_t user_data_len = (uint8_t)(ind->rx_len - MSG_HEADER_LEN - MSG_POLL_USER_DATA_IDX);
                        if (user_data_len)
@@ -262,15 +254,7 @@
                        LOG_INFO(TRACE_MODULE_APP, "RSSI: %ddBm, SNR: %ddB \r\n", ind->rssi, ind->snr);
#endif

#if CSI_EN
                        struct RANGING_TAPS_INF_T taps_inf;
                        ranging_taps_inf_get(&taps_inf);
                        ranging_env.frame[0].NLoS = taps_inf.NLoS;
                        ranging_env.frame[0].fom = taps_inf.FoM;
                        LOG_INFO(TRACE_MODULE_APP, "NLoS %u FoM %u\r\n", taps_inf.NLoS, taps_inf.FoM);
#else
                        ranging_fom_get(&ranging_env.frame[0].NLoS, &ranging_env.frame[0].fom);
#endif
                    }
                    else if (ind->ranging_stage == RANGING_RESPONSE)
                    {
@@ -289,15 +273,7 @@
                        LOG_INFO(TRACE_MODULE_APP, "RSSI: %ddBm, SNR: %ddB\r\n", ind->rssi, ind->snr);
#endif

#if CSI_EN
                        struct RANGING_TAPS_INF_T taps_inf;
                        ranging_taps_inf_get(&taps_inf);
                        ranging_env.frame[responder_idx].NLoS = taps_inf.NLoS;
                        ranging_env.frame[responder_idx].fom = taps_inf.FoM;
                        LOG_INFO(TRACE_MODULE_APP, "NLoS %u FoM %u\r\n", taps_inf.NLoS, taps_inf.FoM);
#else
                        ranging_fom_get(&ranging_env.frame[responder_idx].NLoS, &ranging_env.frame[responder_idx].fom);
#endif
                    }
                    else if (ind->ranging_stage == RANGING_FINAL)
                    {
@@ -346,7 +322,7 @@
                            uint32_t distance = (uint32_t)(tof_f * 0.299702547 * VP_VAL - RANGING_CORR);

                            // update distance result
                            struct RANGING_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[0];
                            struct TWR_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[0];
                            range_result->status = STATUS_OK;
                            range_result->distance = (uint16_t)distance;

@@ -390,9 +366,9 @@
                                }

                                // float pdoa[3];
                                // pdoa[0] = pdoa_select_get(0, 3);
                                // pdoa[1] = pdoa_select_get(1, 3);
                                // pdoa[2] = pdoa_select_get(2, 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();
@@ -438,51 +414,19 @@
                            range_result->mac_addr[1] = (target_addr >> 8) & 0xff;
                            range_result->slot_idx = ranging_env.responder_slot_idx;

#if CSI_EN
                            // need to increase slot duration for log printing
                            struct RANGING_TAPS_INF_T taps_inf;
                            ranging_taps_inf_get(&taps_inf);
                            ranging_env.frame[1].NLoS = taps_inf.NLoS;
                            ranging_env.frame[1].fom = taps_inf.FoM;
                            LOG_INFO(TRACE_MODULE_APP, "NLoS %u FoM %u\r\n", taps_inf.NLoS, taps_inf.FoM);

                            // 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]);
                            // }
#else
                            ranging_fom_get(&ranging_env.frame[1].NLoS, &ranging_env.frame[1].fom);
#endif

                            // Retrieve Response-FoM and Response-NLoS from final packet
                            uint8_t FoM = usr_pkt->msg.final_msg[MSG_FINAL_FOM_IDX(ranging_env.responder_slot_idx)];
                            uint8_t NLoS = usr_pkt->msg.final_msg[MSG_FINAL_NLOS_IDX(ranging_env.responder_slot_idx)];
#if RANGING_FOM_FILTER_EN
                            uint8_t fap_valid = usr_pkt->msg.final_msg[MSG_FINAL_FAP_VALID_IDX(ranging_env.responder_slot_idx)];
                            uint8_t gaps_num = usr_pkt->msg.final_msg[MSG_FINAL_GAPS_NUM_IDX(ranging_env.responder_slot_idx)];
                            uint8_t gaps[CIR_LEN / CE_WIN - 1];
                            memcpy(gaps, &usr_pkt->msg.final_msg[MSG_FINAL_GAPS_IDX(ranging_env.responder_slot_idx)], (CIR_LEN / CE_WIN - 1));
                            debug_csi.ranging_fom = ranging_fom_calculate(&debug_csi, fap_valid, gaps, gaps_num);
#endif

                            LOG_INFO(TRACE_MODULE_APP, "Poll-FoM %d, Response-FoM %d Final-FoM %d, Poll-NLoS %d, Response-NLoS %d Final-NLoS %d\r\n",
                                     ranging_env.frame[0].fom, FoM, ranging_env.frame[1].fom, ranging_env.frame[0].NLoS, NLoS, ranging_env.frame[1].NLoS);
                            LOG_INFO(TRACE_MODULE_APP, "FoM %d-%d-%d, NLoS %d-%d-%d\r\n", ranging_env.frame[0].fom, FoM, ranging_env.frame[1].fom,
                                     ranging_env.frame[0].NLoS, NLoS, ranging_env.frame[1].NLoS);

                            range_result->NLoS = MAX(NLoS, MAX(ranging_env.frame[0].NLoS, ranging_env.frame[1].NLoS));

                            // output result
                            uwbapi_report_ranging_data(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);
                        }
                        else
                        {
@@ -495,9 +439,17 @@
                        LOG_INFO(TRACE_MODULE_APP, "RSSI: %ddBm, SNR: %ddB \r\n", ind->rssi, ind->snr);
#endif

                        // calculate_first_tap_power(2, 2);
                        // print_preamble_chest(2, 2);
                        // print_sts_ch_taps(2);
                        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 (X38M4_AUTO_TUNE_EN == 1)
                        int32_t ppm = 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
                        // calculate_first_tap_power(0, 2);
                        // print_preamble_cir(ranging_env.range_data.sequence_num, 0, 2);
                        // print_sts_cir(0, 2);
                    }
#if RANGING_RESULT_REPORT_EN
                    else if (ind->ranging_stage == RANGING_RESULT)
@@ -510,10 +462,9 @@
                        if (ranging_env.initiator_result_flag & (1 << responder_idx))
                        {
                            uint16_t distance = READ_SHORT(&usr_pkt->msg.result_msg[2]);
                            LOG_INFO(TRACE_MODULE_APP,
                                     "Address %X, Distance %ucm, Poll-FoM %d, Response-FoM %d Final-FoM %d, Poll-NLoS %d, Response-NLoS %d Final-NLoS %d\r\n",
                                     responder_addr, distance, usr_pkt->msg.result_msg[4], ranging_env.frame[responder_idx].fom, usr_pkt->msg.result_msg[6],
                                     usr_pkt->msg.result_msg[5], ranging_env.frame[responder_idx].NLoS, usr_pkt->msg.result_msg[7]);
                            LOG_INFO(TRACE_MODULE_APP, "Address %X, Distance %ucm, FoM %d-%d-%d, NLoS %d-%d-%d\r\n", responder_addr, distance,
                                     usr_pkt->msg.result_msg[4], ranging_env.frame[responder_idx].fom, usr_pkt->msg.result_msg[6], usr_pkt->msg.result_msg[5],
                                     ranging_env.frame[responder_idx].NLoS, usr_pkt->msg.result_msg[7]);
                        }
                    }
#endif
@@ -533,6 +484,7 @@
                else
                {
                    LOG_INFO(TRACE_MODULE_APP, "UWB RX fail  0x%04x\r\n", ind->status);
                    // print_preamble_cir(ranging_env.range_data.sequence_num, 0, 2);
                }
            }
            break;