1111111

WXK

2025-02-11 e328ebef585cea2351b37117b2d5ac4978ecd3c0

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

@@ -118,10 +118,6 @@
 *  \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;
@@ -148,6 +144,12 @@
            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]);
@@ -160,6 +162,15 @@
                {
                    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());

                    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
                    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 PRINT_PAYLOAD_EN
@@ -208,13 +219,7 @@
                            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)
                            {
@@ -234,8 +239,8 @@
                            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;
                            int64_t Tround = ranging_tround();
                            int64_t Treply = 0;

                            for (int i = 4; i >= 0; i--)
                            {
@@ -248,25 +253,23 @@
                            {
                                // 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;
                                int32_t 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];
                                ranging_env.tof = (uint32_t)(tof_i);

                                double tof_f = (double)TIMESTAMP_UNIT_TO_NS(ranging_env.tof);

                                struct TWR_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)
@@ -323,7 +326,7 @@

                                    struct RANGE_DATA_T *range_data = &ranging_env.range_data;

                                    range_data->ranging_type = 0x01;
                                    range_data->ranging_type = 0x01; // TWR (SS-TWR, DS-TWR)
                                    range_data->measurements_num = 1;
                                    uint16_t target_addr = uwbs_peer_short_addr_get();
                                    range_result->mac_addr[0] = target_addr & 0xff;