1111111

WXK

2025-02-11 e328ebef585cea2351b37117b2d5ac4978ecd3c0

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

@@ -61,7 +61,7 @@
#include "board.h"

#if PDOA_3D_EN
#define PDOA_3D_SUPPORT_NUM 100
#define PDOA_3D_SUPPORT_NUM 10
#define PDOA_3D_TIMEOUT_MS 2000
static struct PDOA_3D_MAC_ADDR_T mac_addr_cache[PDOA_3D_SUPPORT_NUM];
static struct PDOA_3D_PDOA_DATA_T pdoa_data_cache[PDOA_3D_SUPPORT_NUM];
@@ -91,6 +91,8 @@
    .session_responder_num_get = ranging_responder_num_get,
    .session_responder_addr_get = ranging_responder_addr_get,
    .session_dynamic_update_responder_list = ranging_dynamic_update_respoder_list,
    .session_initiator_addr_set = ranging_initiator_addr_set,
    .session_controller_addr_set = ranging_controller_addr_set,
    .session_set_ccc_ursk = NULL,
#ifdef RADAR_EN
    .vendor_session_configure = uwb_radar_configure,
@@ -148,13 +150,17 @@

    uwb_app_config.ranging_stage = RANGING_IDLE;

    ranging_env.uwb_period_prefetch_time = UWB_PERIOD_PREFETCH_TIME + LPM_PPM_COMPENSATION(LOW_POWER_CLOCK_PPM, uwb_app_config.session_param.ranging_interval);
    ranging_env.uwb_evt_prefetch_time = UWB_EVT_PREFETCH_TIME;
    ranging_env.uwb_rx_open_in_advance = UWB_RX_OPEN_IN_ADVANCE;
    ranging_env.uwb_rx_window = UWB_RX_WINDOW;
    ranging_env.uwb_rx_open_in_advance_wakeup =
        UWB_RX_OPEN_IN_ADVANCE + LPM_PPM_COMPENSATION(LOW_POWER_CLOCK_PPM, uwb_app_config.session_param.ranging_interval);
    ranging_env.uwb_rx_window_wakeup = UWB_RX_WINDOW + LPM_PPM_COMPENSATION(LOW_POWER_CLOCK_PPM, uwb_app_config.session_param.ranging_interval);

    uint32_t compensation = 0;
#if LOW_POWER_EN
    compensation += LPM_PPM_COMPENSATION(LOW_POWER_CLOCK_PPM, uwb_app_config.session_param.ranging_interval) + US_TO_PHY_TIMER_COUNT(80);
#endif
    ranging_env.uwb_period_prefetch_time = UWB_PERIOD_PREFETCH_TIME + compensation;
    ranging_env.uwb_rx_open_in_advance_wakeup = UWB_RX_OPEN_IN_ADVANCE + compensation;
    ranging_env.uwb_rx_window_wakeup = UWB_RX_WINDOW + 2 * compensation;
    ranging_env.ranging_period = MS_TO_PHY_TIMER_COUNT(uwb_app_config.session_param.ranging_interval);

    ranging_env.slots_per_block = (uint16_t)(uwb_app_config.session_param.ranging_interval / RSTU_TO_MS(uwb_app_config.session_param.slot_duration));
@@ -165,13 +171,19 @@
    ranging_env.nround_inblock = ranging_env.slots_per_block / uwb_app_config.session_param.slots_per_round;
    ranging_env.enable = 0;

    ranging_env.slot_idx = 0;
    // initial value of one-to-one case
    ranging_env.responder_slot_idx[SLOT_RESPONSE] = 2; // for response
    ranging_env.responder_slot_idx[SLOT_FINAL] = 3;    // for final
    ranging_env.responder_slot_idx[SLOT_REPORT] = 4;   // for report
    ranging_env.responder_slot_idx[SLOT_RESULT] = 5;   // for result
    ranging_env.tof = 0;
    ranging_env.slot_idx = 0;

    if ((uwb_app_config.session_param.device_type == DEV_TYPE_CONTROLLER) && (uwb_app_config.session_param.device_role == DEV_ROLE_RESPONDER))
    {
        uint8_t responder_idx = ranging_responder_idx_get(uwbs_local_short_addr_get());
        uint8_t responder_num = ranging_responder_num_get();

        ranging_env.responder_slot_idx[SLOT_RESPONSE] = RESPONSE_SLOT_IDX(responder_idx, responder_num); // for response
        ranging_env.responder_slot_idx[SLOT_FINAL] = FINAL_SLOT_IDX(responder_idx, responder_num);       // for final
        ranging_env.responder_slot_idx[SLOT_REPORT] = REPORT_SLOT_IDX(responder_idx, responder_num);     // for report
        ranging_env.responder_slot_idx[SLOT_RESULT] = RESULT_SLOT_IDX(responder_idx, responder_num);     // for result
    }

    ranging_env.range_data.ranging_type = 0x1; // TWR (SS-TWR, DS-TWR)
    ranging_env.range_data.ranging_interval = uwb_app_config.session_param.ranging_interval;
@@ -235,6 +247,24 @@

    LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_FIRA, "slot duration %d slots per round %d ranging interval %d\r\n",
             uwb_app_config.session_param.slot_duration, uwb_app_config.session_param.slots_per_round, uwb_app_config.session_param.ranging_interval);

#if RANGING_FOM_FILTER_EN
    ranging_debug_csi_en_set(1);
#endif

    if (uwb_app_config.session_param.sts_config == STS_DYNAMIC)
    {
        mac_ifs_set(PHY_LIFS_PERIOD_MACCLK_LONG, PHY_SIFS_PERIOD_MACCLK_LONG);
    }

    LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_FIRA, "Local %x\r\n", uwbs_local_short_addr_get());
    LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_FIRA, "Peer %x\r\n", uwbs_peer_short_addr_get());
    LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_FIRA, "Controller %x\r\n", ranging_controller_addr_get());
    LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_FIRA, "Initiator %x\r\n", ranging_initiator_addr_get());
    for (uint8_t i = 0; i < ranging_responder_num_get(); i++)
    {
        LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_FIRA, "Responder[%d] %x\r\n", i, ranging_responder_addr_get(i));
    }
}

void ranging_start(void)
@@ -245,11 +275,10 @@
    ranging_env.slot_idx = 0;
#if FIRA_TEST_EN
    ranging_env.num_of_measure = 0;
    uwb_app_config.session_param.result_report_config = (uwb_app_config.session_param.aoa_result_req) ? 0x0F : 1;
    uwb_app_config.session_param.result_report_config = (uwb_app_config.session_param.aoa_result_req) ? uwb_app_config.session_param.result_report_config : 1;
#endif
    ranging_env.is_hopping = uwb_app_config.session_param.hopping_mode;
    enum DEV_ROLE_T role = uwb_app_config.session_param.device_role;
    if ((role == DEV_ROLE_INITIATOR) || (role == DEV_ROLE_GATE_CONTROLLER))
    if (uwb_app_config.session_param.device_type == DEV_TYPE_CONTROLLER)
    {
        uwb_app_config.ranging_stage = RANGING_RCM;
        phy_timer_target_set(ranging_env.anchor_point + ranging_env.ranging_period - ranging_env.uwb_period_prefetch_time, session_timer_callback);
@@ -262,12 +291,8 @@
        ranging_env.round_offset_in_block = 0;
        ranging_env.next_round_index = 0;
        uwb_app_config.ranging_stage = RANGING_SYNC;
        phy_sts_pkt_cfg_set(SP0);
        ranging_update_slot_index(ranging_env.slot_idx);
        power_on_radio(0, 1);
        mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, ranging_env.ranging_period);
        mac_start();
        power_mode_request(POWER_UNIT_APP, POWER_MODE_SLEEP);
        phy_timer_target_set(ranging_env.anchor_point + ranging_env.ranging_period / 2, session_timer_callback);
        power_mode_request(POWER_UNIT_APP, POWER_MODE_POWER_DOWN);
    }

    ranging_env.count = 0;
@@ -381,8 +406,9 @@
        ind->ranging_stage = (uint8_t)stage;
        ind->slot_idx = slot_idx;
        ind->status = rx->err_code;
        ind->rssi = rx->rssi;
        ind->snr = rx->snr;
        // ind->rssi = rx->rssi;
        ind->rssi = correct_rssi(rx->rssi);
        ind->snr = correct_snr(rx->snr);

#if MCTT_TEST_EN
        ind->timestamp = rx->timestamp;
@@ -406,6 +432,51 @@
#if MCTT_TEST_EN
        ind->phy_header = rx->phy_header;
#endif

#if RANGING_FOM_FILTER_EN
        if (ranging_debug_csi_en_get())
        {
            uint8_t frame_idx = 0;
            if (ind->ranging_stage == RANGING_RIM)
            {
                frame_idx = 1;
            }
            else if (ind->ranging_stage == RANGING_RFM)
            {
                frame_idx = 2;
            }
            else if (ind->ranging_stage == RANGING_MRM)
            {
                frame_idx = 3;
            }

            debug_csi.frame_idx = frame_idx;

            uint32_t val = REG_READ(0x40003050);
            debug_csi.frame[frame_idx].rf_gain = (val & 0x07);
            debug_csi.frame[frame_idx].bb_gain = ((val >> 3) & 0x1f);
            debug_csi.frame[frame_idx].bd_cnt = phy_bd_cnt_get();
            debug_csi.frame[frame_idx].sfd_cnt = phy_sfd_cnt_get();
            debug_csi.frame[frame_idx].error_code = ind->status;

            if (rx->err_code != 0x0830)
            {
                debug_csi.frame[frame_idx].rssi = ind->rssi;
                debug_csi.frame[frame_idx].snr = ind->snr;
                debug_csi.frame[frame_idx].channel_power = REG_READ(0x40002064);
                debug_csi.frame[frame_idx].noise_power = REG_READ(0x40002078);
                debug_csi.frame[frame_idx].freq_offset = phy_freq_offset_get();

                dump_preamble_cir(frame_idx, 128);

                if ((frame_idx == 1) || (frame_idx == 2))
                {
                    dump_sts_cir(frame_idx);
                }
            }
        }
#endif

        // Send the message
        WsfMsgSend(ranging_cb.handle_id, ind);
        return ind->status;
@@ -495,7 +566,7 @@
int measure_report_handler(const struct UWB_PKT_RX_DONE_IND_T *ind)
{
    int ret = 0;
    struct RANGING_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[0];
    struct TWR_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[0];

    if (uwb_app_config.session_param.ranging_round_usage == DS_TWR_DEFERRED || uwb_app_config.session_param.ranging_round_usage == DS_TWR)
    {
@@ -711,7 +782,7 @@
        uint16_t target_addr = ranging_responder_addr_get(responder_idx);
        double tof_f = (double)TIMESTAMP_UNIT_TO_NS(tof);

        struct RANGING_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[responder_idx];
        struct TWR_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[responder_idx];

        range_result->mac_addr[0] = target_addr & 0xff;
        range_result->mac_addr[1] = (target_addr >> 8) & 0xff;