/*
|
* 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_power.h"
|
#include "mk_gpio.h"
|
#include "mk_uwb.h"
|
#include "mk_clock.h"
|
#include "mk_trng.h"
|
#include "wsf_msg.h"
|
#include "ranging_custom.h"
|
#include "lib_aoa.h"
|
#include "libc_rom.h"
|
#if KF_EN
|
#include "lib_kf.h"
|
#endif
|
#if PDOA_3D_EN
|
#include "lib_pdoa_3d.h"
|
#endif
|
|
#include "board.h"
|
|
#if PDOA_3D_EN
|
#define PDOA_3D_SUPPORT_NUM 100
|
#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];
|
#endif
|
|
#if KF_EN && FILTER_EN
|
#define KF_SUPPORT_NUM 6
|
#define KF_TIMEOUT_MS 2000
|
static struct KF_MAC_ADDR_T kf_mac_addr_cache[KF_SUPPORT_NUM];
|
static struct KF_CHANNEL_CACHE_T kf_channel_cache[KF_SUPPORT_NUM];
|
static struct KF_MAT_VALUE_CACHE_T kf_mat_value_cache[KF_SUPPORT_NUM];
|
#endif
|
|
static uint16_t user_poll_data_len = 0;
|
static uint8_t user_poll_data[MSG_POLL_USER_DATA_LEN];
|
|
static uint16_t user_response_data_len = 0;
|
static uint8_t user_response_data[MSG_RESPONSE_USER_DATA_LEN];
|
|
static struct RANGING_USER_PKT_T ranging_user_pkt;
|
|
struct RANGING_ENV_T ranging_env;
|
|
static struct RANGING_CB_T ranging_cb;
|
|
static struct UWB_OP_T op = {
|
.session_configure = ranging_configure,
|
.session_start = ranging_start,
|
.session_stop = ranging_stop,
|
.session_local_addr_set = ranging_local_addr_set,
|
.session_peer_addr_set = ranging_peer_addr_set,
|
.session_responder_addr_add = ranging_responder_addr_add,
|
.session_responder_list_clr = ranging_responder_list_clr,
|
.session_responder_num_get = ranging_responder_num_get,
|
.session_responder_addr_get = ranging_responder_addr_get,
|
.session_set_ccc_ursk = NULL,
|
};
|
|
static void ranging_timer_callback(void *dev, uint32_t time);
|
|
static void uwb_pkt_tx_done_ind(const struct MAC_HW_REPORT_T *tx, enum RANGING_STAGE_T stage);
|
static uint16_t uwb_pkt_rx_done_ind(const struct MAC_HW_REPORT_T *rx, enum RANGING_STAGE_T stage);
|
|
static void ranging_tx_process(struct MAC_HW_REPORT_T *tx_report);
|
static void ranging_rx_process(struct MAC_HW_REPORT_T *rx_report);
|
void app_session_init(void);
|
|
//------------------------------------------------------------------------------
|
int ranging_init(uint8_t handle_id)
|
{
|
/* store handler ID */
|
ranging_cb.handle_id = handle_id;
|
|
/* init rx queue */
|
WSF_QUEUE_INIT(&ranging_cb.msg_queue);
|
|
ranging_cb.daemon_timer.handlerId = handle_id;
|
ranging_cb.daemon_timer.msg.event = RANGING_DAEMON_TIMER_MSG;
|
|
LOG_INFO(TRACE_MODULE_APP, "Ranging lib version: %s\r\n", MK8000_get_rangelib_version());
|
LOG_INFO(TRACE_MODULE_APP, "AoA lib version: %s\r\n", MK8000_get_aoalib_version());
|
|
LOG_INFO(TRACE_MODULE_APP, "UWB_RX_OPEN_IN_ADVANCE %d\r\n", UWB_RX_OPEN_IN_ADVANCE);
|
LOG_INFO(TRACE_MODULE_APP, "UWB_RX_WINDOW %d\r\n", UWB_RX_WINDOW);
|
LOG_INFO(TRACE_MODULE_APP, "UWB_EVT_PREFETCH_TIME %d\r\n", UWB_EVT_PREFETCH_TIME);
|
|
return 0;
|
}
|
|
int ranging_deinit(void)
|
{
|
return 0;
|
}
|
|
// This function will be called by the uwbapi_session_init()
|
void app_session_init(void)
|
{
|
// register process handler for MAC TX done and RX done
|
mac_register_process_handler(ranging_tx_process, ranging_rx_process);
|
|
uwbs_handler_init(&op);
|
uwb_app_config.responder_num_max = RESPONDER_NUM_MAX;
|
}
|
|
void ranging_configure(void)
|
{
|
ranging_env.ranging_period = MS_TO_PHY_TIMER_COUNT(uwb_app_config.session_param.ranging_interval);
|
ranging_env.slot_interval = RSTU_TO_PHY_TIMER_COUNT(uwb_app_config.session_param.slot_duration);
|
ranging_env.enable = 0;
|
ranging_env.tof = 0;
|
ranging_env.range_data.measurements_num = 0;
|
for (uint8_t i = 0; i < MEASUREMENT_NUM_MAX; i++)
|
{
|
ranging_env.range_data.measurements[i].status = STATUS_VENDOR_RESERVED;
|
}
|
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;
|
ranging_env.range_data.mac_addr_mode = uwbs_mac_addr_mode_get();
|
ranging_env.range_data.session_id = uwb_app_config.session_id;
|
|
uwbs_configure(PHY_TX | PHY_RX, uwb_app_config.session_param.tx_power_level);
|
|
#if CSI_EN
|
ranging_aux_out_opt_set(CH_LEN_DEFAULT, 3);
|
#endif
|
|
#if (ANT_PATTERN == ANT_PATTERN_SQUARE)
|
struct AOA_ANGLE_SPAN_T aoa_span;
|
#if AOA_3D_EN
|
aoa_span.Ndim = 2;
|
aoa_span.el_low = 0;
|
aoa_span.el_high = 90;
|
aoa_span.el_step = 5;
|
aoa_span.az_low = 0;
|
aoa_span.az_high = 359;
|
aoa_span.az_step = 3;
|
#else
|
aoa_span.Ndim = 2;
|
aoa_span.el_low = 90;
|
aoa_span.el_high = 90;
|
aoa_span.el_step = 5;
|
aoa_span.az_low = 0;
|
aoa_span.az_high = 359;
|
aoa_span.az_step = 1;
|
#endif
|
aoa_angle_search_span_set(&aoa_span);
|
#endif
|
|
#if AOA_EN
|
aoa_aux_info_set(AOA_AUX_ANT_IQ_RSSI_PDOA_AOA_FOM);
|
aoa_steering_vector_set((const float *)((uint32_t)((uwb_app_config.ppdu_params.ch_num == 9) ? svec_ch9_ptr : svec_ch5_ptr) | SRAM_BASE));
|
#else
|
aoa_aux_info_set(AOA_AUX_ANT_IQ_RSSI);
|
#endif
|
|
aoa_param_config();
|
|
#if PDOA_3D_EN
|
pdoa_3d_param_config(ANT_PATTERN, ANT_LAYOUT, PDOA_3D_AMBIGUITY_LEVEL_HIGH, mac_addr_cache, pdoa_data_cache, PDOA_3D_SUPPORT_NUM, PDOA_3D_TIMEOUT_MS);
|
// pdoa_angle_reverse_set(1, 0);
|
#endif
|
|
#if FILTER_EN
|
if (uwb_app_config.filter_en)
|
{
|
#if KF_EN
|
loc_post_kf_config(uwb_app_config.session_param.ranging_interval, kf_mac_addr_cache, kf_channel_cache, kf_mat_value_cache, KF_SUPPORT_NUM,
|
KF_TIMEOUT_MS);
|
#else
|
loc_post_filter_config(uwb_app_config.session_param.ranging_interval, 1, uwb_app_config.session_param.aoa_result_req);
|
#endif
|
}
|
#endif
|
|
#if RANGING_FOM_FILTER_EN
|
ranging_debug_csi_en_set(1);
|
#endif
|
|
ranging_user_pkt.session_id = (uint16_t)uwb_app_config.session_id;
|
}
|
|
void ranging_start(void)
|
{
|
ranging_env.enable = 1;
|
ranging_env.anchor_point = phy_timer_count_get();
|
enum DEV_ROLE_T dev_role = uwb_app_config.session_param.device_role;
|
if (dev_role == DEV_ROLE_INITIATOR)
|
{
|
uwb_app_config.ranging_stage = RANGING_POLL;
|
}
|
else
|
{
|
uwb_app_config.ranging_stage = RANGING_SYNC;
|
}
|
phy_timer_target_set(ranging_env.anchor_point + ranging_env.ranging_period - UWB_EVT_PREFETCH_TIME, ranging_timer_callback);
|
power_mode_request(POWER_UNIT_APP, POWER_MODE_POWER_DOWN);
|
ranging_env.count = 0;
|
ranging_env.count_last = 0;
|
ranging_env.lost_cnt = 0;
|
|
WsfTimerStartMs(&ranging_cb.daemon_timer, uwb_app_config.session_param.ranging_interval * 10, WSF_TIMER_PERIODIC);
|
LOG_INFO(TRACE_MODULE_APP, "Ranging start, role %d\r\n", dev_role);
|
}
|
|
void ranging_stop(void)
|
{
|
ranging_env.enable = 0;
|
WsfTimerStop(&ranging_cb.daemon_timer);
|
}
|
|
void ranging_restart(void)
|
{
|
ranging_stop();
|
mac_restart();
|
ranging_start();
|
LOG_INFO(TRACE_MODULE_APP, "Ranging restart\r\n");
|
}
|
|
int8_t ranging_tx_power_get(void)
|
{
|
return uwb_tx_power_get(uwb_app_config.ppdu_params.ch_num, uwb_app_config.session_param.tx_power_level);
|
}
|
|
void ranging_local_addr_set(uint16_t short_addr)
|
{
|
uwbs_local_short_addr_set(short_addr);
|
}
|
|
void ranging_peer_addr_set(uint16_t short_addr)
|
{
|
uwbs_peer_short_addr_set(short_addr);
|
}
|
|
uint8_t ranging_responder_addr_add(uint16_t addr)
|
{
|
if (ranging_env.responder_num < RESPONDER_NUM_MAX)
|
{
|
LOG_INFO(TRACE_MODULE_APP, "Responder %d %04x\r\n", ranging_env.responder_num, addr);
|
ranging_env.responder_list[ranging_env.responder_num] = addr;
|
ranging_env.responder_num++;
|
ranging_env.main_ant_id = uwb_app_config.ppdu_params.rx_ant_id;
|
return 1;
|
}
|
return 0;
|
}
|
|
uint8_t ranging_responder_addr_set(uint8_t idx, uint16_t addr)
|
{
|
if (idx < ranging_env.responder_num)
|
{
|
ranging_env.responder_list[idx] = addr;
|
return 1;
|
}
|
return 0;
|
}
|
|
void ranging_responder_list_clr(void)
|
{
|
ranging_env.responder_num = 0;
|
}
|
|
uint16_t ranging_responder_addr_get(uint8_t idx)
|
{
|
return ranging_env.responder_list[idx];
|
}
|
|
uint8_t ranging_responder_idx_get(uint16_t addr)
|
{
|
for (uint8_t i = 0; i < ranging_env.responder_num; i++)
|
{
|
if (ranging_env.responder_list[i] == addr)
|
{
|
return i;
|
}
|
}
|
return (0xFF);
|
}
|
|
uint8_t ranging_responder_num_get(void)
|
{
|
return ranging_env.responder_num;
|
}
|
|
uint8_t ranging_poll_msg_set(uint16_t data_len, const uint8_t *data)
|
{
|
user_poll_data_len = sizeof(user_poll_data);
|
|
if (user_poll_data_len > data_len)
|
{
|
user_poll_data_len = data_len;
|
}
|
|
for (uint8_t ii = 0; ii < user_poll_data_len; ii++)
|
{
|
user_poll_data[ii] = data[ii];
|
}
|
|
// The return value is 0, the poll message is set successfully
|
// The return value is 1 is set failed
|
return (user_poll_data_len == data_len) ? 0 : 1;
|
}
|
|
uint8_t ranging_response_msg_set(uint16_t data_len, const uint8_t *data)
|
{
|
user_response_data_len = sizeof(user_response_data);
|
|
if (user_response_data_len > data_len)
|
{
|
user_response_data_len = data_len;
|
}
|
|
for (uint8_t ii = 0; ii < user_response_data_len; ii++)
|
{
|
user_response_data[ii] = data[ii];
|
}
|
|
// The return value is 0, the response message is set successfully
|
// The return value is 1 is set failed
|
return (user_response_data_len == data_len) ? 0 : 1;
|
}
|
|
// ts_a - ts_b
|
int64_t ranging_timestamp_diff(int64_t ts_a, int64_t ts_b)
|
{
|
if (ts_a < ts_b)
|
{
|
return (0x20000000000 - ts_b + ts_a);
|
}
|
else
|
{
|
return (ts_a - ts_b);
|
}
|
}
|
|
// unit: 15.6ps
|
int64_t ranging_tround(enum DEV_ROLE_T role, uint8_t dev_idx)
|
{
|
int64_t tx_timestamp;
|
int64_t rx_timestamp;
|
if (role == DEV_ROLE_INITIATOR)
|
{
|
tx_timestamp = ranging_env.tx_poll_time;
|
rx_timestamp = ranging_env.rx_response_time[dev_idx];
|
}
|
else
|
{
|
tx_timestamp = ranging_env.tx_response_time;
|
rx_timestamp = ranging_env.rx_final_time;
|
}
|
|
int64_t tround = ranging_timestamp_diff(rx_timestamp, tx_timestamp);
|
|
// correct antenna delay
|
tround -= ranging_ant_delays_get(uwb_app_config.ppdu_params.rx_ant_id);
|
|
return tround;
|
}
|
|
// unit: 15.6ps
|
int64_t ranging_treply(enum DEV_ROLE_T role, uint8_t dev_idx)
|
{
|
int64_t tx_timestamp;
|
int64_t rx_timestamp;
|
if (role == DEV_ROLE_INITIATOR)
|
{
|
tx_timestamp = ranging_env.tx_final_time;
|
rx_timestamp = ranging_env.rx_response_time[dev_idx];
|
}
|
else
|
{
|
tx_timestamp = ranging_env.tx_response_time;
|
rx_timestamp = ranging_env.rx_poll_time;
|
}
|
|
int64_t treply = ranging_timestamp_diff(tx_timestamp, rx_timestamp);
|
|
// correct antenna delay
|
treply += ranging_ant_delays_get(uwb_app_config.ppdu_params.rx_ant_id);
|
|
return treply;
|
}
|
|
uint8_t ranging_count_get(void)
|
{
|
return ranging_env.count;
|
}
|
|
void ranging_poll_pkt_construct(void)
|
{
|
ranging_user_pkt.msg_type = RANGING_POLL;
|
ranging_user_pkt.msg.poll_msg[MSG_POLL_RESPONDER_NUM_IDX] = ranging_env.responder_num;
|
|
for (uint8_t i = 0; i < ranging_env.responder_num; i++)
|
{
|
if (uwb_app_config.ranging_flow_mode == (uint8_t)RANGING_FLOW_CONTENTION)
|
{
|
ranging_env.responder_list[i] = 0;
|
}
|
ranging_user_pkt.msg.poll_msg[MSG_POLL_RESPONDER_ID_IDX(i)] = ranging_env.responder_list[i] & 0xff;
|
ranging_user_pkt.msg.poll_msg[MSG_POLL_RESPONDER_ID_IDX(i) + 1] = (ranging_env.responder_list[i] >> 8) & 0xff;
|
ranging_user_pkt.msg.poll_msg[MSG_POLL_RESPONDER_SLOT_IDX(i)] = i;
|
}
|
|
ranging_user_pkt.msg.poll_msg[MSG_POLL_TX_PWR_IDX] = uwb_app_config.session_param.tx_power_level;
|
memcpy(&ranging_user_pkt.msg.poll_msg[MSG_POLL_USER_DATA_IDX], user_poll_data, user_poll_data_len);
|
}
|
|
void ranging_response_pkt_construct(void)
|
{
|
ranging_user_pkt.msg_type = RANGING_RESPONSE;
|
uint16_t responder_id = uwbs_local_short_addr_get();
|
ranging_user_pkt.msg.response_msg[MSG_RESPONSE_RESPONDER_ID_IDX] = responder_id & 0xff;
|
ranging_user_pkt.msg.response_msg[MSG_RESPONSE_RESPONDER_ID_IDX + 1] = (responder_id >> 8) & 0xff;
|
ranging_user_pkt.msg.response_msg[MSG_RESPONSE_TX_PWR_IDX] = uwb_app_config.session_param.tx_power_level;
|
memcpy(&ranging_user_pkt.msg.response_msg[MSG_RESPONSE_USER_DATA_IDX], user_response_data, user_response_data_len);
|
}
|
|
void ranging_final_pkt_construct(void)
|
{
|
ranging_user_pkt.msg_type = RANGING_FINAL;
|
ranging_user_pkt.msg.final_msg[MSG_FINAL_RESPONDER_NUM_IDX] = ranging_env.responder_num;
|
|
int64_t Ttotal = ranging_timestamp_diff(ranging_env.tx_final_time, ranging_env.tx_poll_time);
|
|
for (int i = 0; i < 5; i++)
|
{
|
ranging_user_pkt.msg.final_msg[MSG_FINAL_TREPLY_IDX + i] = (Ttotal >> (i * 8)) & 0xff;
|
}
|
|
for (uint8_t n = 0; n < ranging_env.responder_num; n++)
|
{
|
int64_t tround = 0;
|
|
if (ranging_env.responder_response_flag & (1 << n))
|
{
|
tround = ranging_tround(DEV_ROLE_INITIATOR, n);
|
}
|
|
// update final packet
|
for (int i = 0; i < 5; i++)
|
{
|
ranging_user_pkt.msg.final_msg[MSG_FINAL_TROUND_IDX(n) + i] = (tround >> (i * 8)) & 0xff;
|
}
|
}
|
|
for (uint8_t n = 0; n < ranging_env.responder_num; n++)
|
{
|
memcpy(&ranging_user_pkt.msg.final_msg[MSG_FINAL_FOM_IDX(n)], &ranging_env.frame[n], RFRAME_INF_SIZE);
|
}
|
}
|
|
void ranging_result_pkt_construct(void)
|
{
|
ranging_user_pkt.msg_type = RANGING_RESULT;
|
uint16_t responder_id = uwbs_local_short_addr_get();
|
ranging_user_pkt.msg.result_msg[0] = responder_id & 0xff;
|
ranging_user_pkt.msg.result_msg[1] = (responder_id >> 8) & 0xff;
|
|
struct RANGING_MEASUREMENT_T *range_result = &ranging_env.range_data.measurements[0];
|
ranging_user_pkt.msg.result_msg[2] = range_result->distance & 0xff;
|
ranging_user_pkt.msg.result_msg[3] = (range_result->distance >> 8) & 0xff;
|
|
ranging_user_pkt.msg.result_msg[4] = ranging_env.frame[0].fom;
|
ranging_user_pkt.msg.result_msg[5] = ranging_env.frame[0].NLoS;
|
|
ranging_user_pkt.msg.result_msg[6] = ranging_env.frame[1].fom;
|
ranging_user_pkt.msg.result_msg[7] = ranging_env.frame[1].NLoS;
|
}
|
|
uint8_t ranging_poll_pkt_process(uint8_t *pkt_data, uint16_t pkt_len)
|
{
|
uint8_t ret = 0;
|
struct RANGING_USER_PKT_T *ranging_recv_pkt = (struct RANGING_USER_PKT_T *)pkt_data;
|
|
ranging_env.responder_num = 0;
|
ranging_env.responder_slot_idx = 0;
|
|
if ((ranging_recv_pkt->session_id == ranging_user_pkt.session_id) && (ranging_recv_pkt->msg_type == RANGING_POLL))
|
{
|
ranging_env.responder_num = ranging_recv_pkt->msg.poll_msg[MSG_POLL_RESPONDER_NUM_IDX];
|
|
if (uwb_app_config.ranging_flow_mode == (uint8_t)RANGING_FLOW_CONTENTION)
|
{
|
ranging_env.responder_slot_idx = ranging_recv_pkt->msg.poll_msg[MSG_POLL_RESPONDER_SLOT_IDX((uint8_t)(rand() % ranging_env.responder_num))];
|
ret = 1;
|
}
|
else
|
{
|
for (uint8_t i = 0; i < ranging_env.responder_num; i++)
|
{
|
uint16_t responder_id = READ_SHORT(&ranging_recv_pkt->msg.poll_msg[MSG_POLL_RESPONDER_ID_IDX(i)]);
|
if (uwbs_local_short_addr_get() == responder_id)
|
{
|
ranging_env.responder_slot_idx = ranging_recv_pkt->msg.poll_msg[MSG_POLL_RESPONDER_SLOT_IDX(i)];
|
ret = 1;
|
break;
|
}
|
}
|
}
|
}
|
return ret;
|
}
|
|
#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
|
#pragma clang diagnostic push
|
#pragma clang diagnostic ignored "-Wswitch-enum"
|
#pragma clang diagnostic ignored "-Wunreachable-code-break"
|
#pragma clang diagnostic ignored "-Wcast-align"
|
#endif
|
|
static void ranging_timer_callback(void *dev, uint32_t time)
|
{
|
// board_led_on(BOARD_LED_1);
|
if (ranging_env.enable)
|
{
|
enum RANGING_STAGE_T stage = uwb_app_config.ranging_stage;
|
uint32_t target_time;
|
if (uwb_app_config.session_param.device_role == DEV_ROLE_INITIATOR)
|
{
|
switch (stage)
|
{
|
case RANGING_IDLE:
|
ASSERT(0, "RANGING_IDLE\r\n");
|
break;
|
|
case RANGING_POLL:
|
{
|
ranging_env.anchor_point += ranging_env.ranging_period;
|
ranging_user_pkt.seq_num++;
|
ranging_env.slot_idx = 0;
|
ranging_env.responder_response_flag = 0;
|
ranging_env.initiator_result_flag = 0;
|
// send poll
|
ranging_poll_pkt_construct();
|
|
target_time = ranging_env.anchor_point;
|
|
if (uwb_app_config.ranging_flow_mode == (uint8_t)RANGING_FLOW_CONTENTION)
|
{
|
power_on_radio(1, 1);
|
mac_tx(EVT_MODE_MAC_FIX_PHY_ASAP, target_time, 0, (uint8_t *)&ranging_user_pkt,
|
user_poll_data_len + MSG_HEADER_LEN + MSG_POLL_USER_DATA_IDX);
|
}
|
else
|
{
|
power_on_radio(1, 0);
|
mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time, 0, (uint8_t *)&ranging_user_pkt,
|
user_poll_data_len + MSG_HEADER_LEN + MSG_POLL_USER_DATA_IDX);
|
}
|
|
power_mode_request(POWER_UNIT_APP, POWER_MODE_SLEEP);
|
}
|
break;
|
|
case RANGING_RESPONSE:
|
{
|
ranging_env.slot_idx++;
|
// receive response
|
power_on_radio(0, 1);
|
sts_lsp_store();
|
|
target_time = ranging_env.anchor_point + ranging_env.slot_idx * ranging_env.slot_interval;
|
mac_rx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time - UWB_RX_OPEN_IN_ADVANCE, UWB_RX_WINDOW);
|
}
|
break;
|
|
case RANGING_FINAL:
|
{
|
ranging_env.slot_idx++;
|
// Store tx timestamp
|
target_time = ranging_env.anchor_point + (1 + ranging_env.responder_num) * ranging_env.slot_interval;
|
ranging_env.tx_final_time = ranging_tx_time(target_time + phy_shr_duration());
|
|
// send final
|
ranging_final_pkt_construct();
|
power_on_radio(1, 0);
|
mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time, 0, (uint8_t *)&ranging_user_pkt, MSG_HEADER_LEN + MSG_FINAL_USER_DATA_IDX);
|
}
|
break;
|
|
#if RANGING_RESULT_REPORT_EN
|
case RANGING_RESULT:
|
{
|
ranging_env.slot_idx++;
|
// receive result
|
power_on_radio(0, 1);
|
sts_lsp_store();
|
|
target_time = ranging_env.anchor_point + ranging_env.slot_idx * ranging_env.slot_interval;
|
mac_rx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time - UWB_RX_OPEN_IN_ADVANCE, UWB_RX_WINDOW);
|
}
|
break;
|
#endif
|
|
default:
|
ASSERT(0, "UNKNOWN STAGE\r\n");
|
break;
|
}
|
}
|
else
|
{
|
switch (stage)
|
{
|
case RANGING_IDLE:
|
ASSERT(0, "RANGING_IDLE\r\n");
|
break;
|
|
case RANGING_SYNC:
|
{
|
ranging_env.anchor_point += ranging_env.ranging_period;
|
// receive sync
|
power_on_radio(0, 1);
|
|
if (uwb_app_config.session_param.aoa_result_req)
|
{
|
#if DYNAMIC_UPDATE_MAIN_ANTENNA_EN
|
// dynamic switch main antenna
|
phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 1, ranging_env.main_ant_id);
|
aoa_param_update(ranging_env.main_ant_id);
|
#else
|
phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
|
#endif
|
}
|
sts_lsp_store();
|
|
mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, ranging_env.ranging_period);
|
power_mode_request(POWER_UNIT_APP, POWER_MODE_SLEEP);
|
ranging_env.responder_final_flag = 0;
|
}
|
break;
|
|
case RANGING_POLL:
|
{
|
ranging_env.anchor_point += ranging_env.ranging_period;
|
// receive poll
|
power_on_radio(0, 1);
|
|
if (uwb_app_config.session_param.aoa_result_req)
|
{
|
#if DYNAMIC_UPDATE_MAIN_ANTENNA_EN
|
// dynamic switch main antenna
|
phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 1, ranging_env.main_ant_id);
|
aoa_param_update(ranging_env.main_ant_id);
|
#else
|
phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
|
#endif
|
}
|
sts_lsp_store();
|
|
target_time = ranging_env.anchor_point - LPM_PPM_COMPENSATION(LOW_POWER_CLOCK_PPM, uwb_app_config.session_param.ranging_interval);
|
mac_rx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time - UWB_RX_OPEN_IN_ADVANCE,
|
UWB_RX_WINDOW + LPM_PPM_COMPENSATION(LOW_POWER_CLOCK_PPM, uwb_app_config.session_param.ranging_interval));
|
power_mode_request(POWER_UNIT_APP, POWER_MODE_SLEEP);
|
ranging_env.responder_final_flag = 0;
|
}
|
break;
|
|
case RANGING_RESPONSE:
|
{
|
// send response
|
ranging_response_pkt_construct();
|
|
target_time = ranging_env.anchor_point + (1 + ranging_env.responder_slot_idx) * ranging_env.slot_interval;
|
|
if (uwb_app_config.ranging_flow_mode == (uint8_t)RANGING_FLOW_CONTENTION)
|
{
|
power_on_radio(1, 1);
|
mac_tx(EVT_MODE_MAC_FIX_PHY_ASAP, target_time, 0, (uint8_t *)&ranging_user_pkt,
|
user_response_data_len + MSG_HEADER_LEN + MSG_RESPONSE_USER_DATA_IDX);
|
}
|
else
|
{
|
power_on_radio(1, 0);
|
mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time, 0, (uint8_t *)&ranging_user_pkt,
|
user_response_data_len + MSG_HEADER_LEN + MSG_RESPONSE_USER_DATA_IDX);
|
}
|
}
|
break;
|
|
case RANGING_FINAL:
|
{
|
// receive final
|
power_on_radio(0, 1);
|
|
if (uwb_app_config.session_param.aoa_result_req)
|
{
|
#if DYNAMIC_UPDATE_MAIN_ANTENNA_EN
|
// dynamic switch main antenna
|
phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 1, ranging_env.main_ant_id);
|
aoa_param_update(ranging_env.main_ant_id);
|
#else
|
phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
|
#endif
|
}
|
sts_lsp_store();
|
|
target_time = ranging_env.anchor_point + (1 + ranging_env.responder_num) * ranging_env.slot_interval;
|
mac_rx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time - UWB_RX_OPEN_IN_ADVANCE, UWB_RX_WINDOW);
|
}
|
break;
|
|
#if RANGING_RESULT_REPORT_EN
|
case RANGING_RESULT:
|
{
|
// send result
|
ranging_result_pkt_construct();
|
power_on_radio(1, 0);
|
target_time = ranging_env.anchor_point + (1 + ranging_env.responder_num + 1 + ranging_env.responder_slot_idx) * ranging_env.slot_interval;
|
mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time, 0, (uint8_t *)&ranging_user_pkt,
|
user_response_data_len + MSG_HEADER_LEN + MSG_RESULT_INFO_LEN);
|
}
|
break;
|
#endif
|
|
default:
|
ASSERT(0, "UNKNOWN STAGE\r\n");
|
break;
|
}
|
}
|
|
mac_start();
|
}
|
else
|
{
|
power_mode_clear(POWER_UNIT_APP);
|
LOG_INFO(TRACE_MODULE_APP, "Ranging stop\r\n");
|
}
|
// board_led_off(BOARD_LED_1);
|
}
|
|
enum RANGING_STAGE_T ranging_fsm(const struct MAC_HW_REPORT_T *ind)
|
{
|
uint16_t status;
|
struct RANGING_USER_PKT_T *ranging_recv_pkt = (struct RANGING_USER_PKT_T *)&ind->pkt_data[0];
|
enum RANGING_STAGE_T stage = uwb_app_config.ranging_stage;
|
|
// board_led_on(BOARD_LED_2);
|
power_off_radio();
|
if (ranging_env.enable)
|
{
|
ranging_env.count++;
|
uint32_t target_time = ranging_env.anchor_point;
|
|
if (uwb_app_config.session_param.device_role == DEV_ROLE_INITIATOR)
|
{
|
switch (stage)
|
{
|
case RANGING_IDLE:
|
break;
|
|
case RANGING_POLL:
|
{
|
// Store tx timestamp
|
ranging_env.tx_poll_time = ranging_tx_time(ind->timestamp);
|
ranging_env.anchor_point = ind->timestamp - phy_shr_duration();
|
|
if (ind->err_code == UWB_TX_OK)
|
{
|
target_time = ranging_env.anchor_point + ranging_env.slot_interval;
|
uwb_app_config.ranging_stage = RANGING_RESPONSE;
|
}
|
else
|
{
|
target_time = ranging_env.anchor_point + ranging_env.ranging_period;
|
uwb_app_config.ranging_stage = RANGING_POLL;
|
}
|
|
// process tx-poll packet
|
uwb_pkt_tx_done_ind(ind, stage);
|
}
|
break;
|
|
case RANGING_RESPONSE:
|
{
|
// process rx-response packet
|
status = uwb_pkt_rx_done_ind(ind, stage);
|
if (status == UWB_RX_OK)
|
{
|
if ((ranging_recv_pkt->session_id == ranging_user_pkt.session_id) && (ranging_recv_pkt->msg_type == RANGING_RESPONSE))
|
{
|
uint16_t responder_id = READ_SHORT(&ranging_recv_pkt->msg.response_msg[MSG_RESPONSE_RESPONDER_ID_IDX]);
|
uint8_t responder_idx = ranging_env.slot_idx - 1;
|
|
if (uwb_app_config.ranging_flow_mode == (uint8_t)RANGING_FLOW_CONTENTION)
|
{
|
ranging_responder_addr_set(responder_idx, responder_id);
|
ranging_env.rx_response_time[responder_idx] = ranging_rx_time(ind);
|
ranging_env.responder_response_flag |= (1 << responder_idx);
|
}
|
else
|
{
|
if (ranging_env.responder_list[responder_idx] == responder_id)
|
{
|
// board_led_on(BOARD_LED_1);
|
// fast - 74us
|
// regular - 144us
|
ranging_env.rx_response_time[responder_idx] = ranging_rx_time(ind);
|
ranging_env.responder_response_flag |= (1 << responder_idx);
|
// board_led_off(BOARD_LED_1);
|
}
|
else
|
{
|
ranging_env.rx_response_time[responder_idx] = 0;
|
}
|
}
|
#if RANGING_FOM_FILTER_EN
|
// update response taps
|
ranging_env.frame[responder_idx].fap_valid =
|
first_path_align(ranging_env.frame[responder_idx].gaps, &ranging_env.frame[responder_idx].gaps_num, debug_csi.rframe[0].cir,
|
debug_csi.rframe[0].first_tap, PWR_TH, TAP_MARGIN);
|
#endif
|
}
|
}
|
|
if (ranging_env.slot_idx < ranging_env.responder_num)
|
{
|
target_time += (ranging_env.slot_idx + 1) * ranging_env.slot_interval;
|
uwb_app_config.ranging_stage = RANGING_RESPONSE;
|
}
|
else
|
{
|
if (ranging_env.responder_response_flag)
|
{
|
target_time += (ranging_env.slot_idx + 1) * ranging_env.slot_interval;
|
uwb_app_config.ranging_stage = RANGING_FINAL;
|
}
|
else
|
{
|
target_time += ranging_env.ranging_period;
|
uwb_app_config.ranging_stage = RANGING_POLL;
|
}
|
}
|
sts_lsp_store_stop();
|
}
|
break;
|
|
case RANGING_FINAL:
|
{
|
#if RANGING_RESULT_REPORT_EN
|
target_time += (ranging_env.slot_idx + 1) * ranging_env.slot_interval;
|
uwb_app_config.ranging_stage = RANGING_RESULT;
|
#else
|
target_time += ranging_env.ranging_period;
|
uwb_app_config.ranging_stage = RANGING_POLL;
|
#endif
|
// process tx-final packet
|
uwb_pkt_tx_done_ind(ind, stage);
|
}
|
break;
|
#if RANGING_RESULT_REPORT_EN
|
case RANGING_RESULT:
|
{
|
// process rx-result packet
|
status = uwb_pkt_rx_done_ind(ind, stage);
|
if (status == UWB_RX_OK)
|
{
|
uint8_t responder_idx = ranging_env.slot_idx - ranging_env.responder_num - 2;
|
if ((ranging_recv_pkt->session_id == ranging_user_pkt.session_id) && (ranging_recv_pkt->msg_type == RANGING_RESULT))
|
{
|
ranging_env.initiator_result_flag |= (1 << responder_idx);
|
}
|
}
|
|
if (ranging_env.slot_idx < (ranging_env.responder_num * 2 + 1))
|
{
|
target_time += (ranging_env.slot_idx + 1) * ranging_env.slot_interval;
|
uwb_app_config.ranging_stage = RANGING_RESULT;
|
}
|
else
|
{
|
target_time += ranging_env.ranging_period;
|
uwb_app_config.ranging_stage = RANGING_POLL;
|
}
|
}
|
break;
|
#endif
|
default:
|
break;
|
}
|
}
|
else
|
{
|
switch (stage)
|
{
|
case RANGING_IDLE:
|
break;
|
case RANGING_SYNC:
|
{
|
// process rx-poll packet
|
status = uwb_pkt_rx_done_ind(ind, RANGING_POLL);
|
|
if (status == UWB_RX_OK)
|
{
|
if (ranging_poll_pkt_process(ind->pkt_data, ind->pkt_len))
|
{
|
ranging_user_pkt.seq_num = ranging_recv_pkt->seq_num;
|
// update anchor point
|
ranging_env.anchor_point = ind->timestamp - phy_shr_duration();
|
ranging_env.rx_poll_time = ranging_rx_time(ind);
|
ranging_env.lost_cnt = 0;
|
|
target_time = ranging_env.anchor_point + (1 + ranging_env.responder_slot_idx) * ranging_env.slot_interval;
|
uwb_app_config.ranging_stage = RANGING_RESPONSE;
|
ranging_env.synced = 1;
|
|
#if DYNAMIC_UPDATE_MAIN_ANTENNA_EN
|
if (uwb_app_config.session_param.aoa_result_req)
|
{
|
// update main antenna
|
sts_rx_main_ant_get(&ranging_env.main_ant_id);
|
}
|
#endif
|
}
|
}
|
|
if (uwb_app_config.session_param.aoa_result_req)
|
{
|
phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_NEVER_SWITCH, 0, 0);
|
}
|
sts_lsp_store_stop();
|
}
|
break;
|
|
case RANGING_POLL:
|
{
|
// process rx-poll packet
|
status = uwb_pkt_rx_done_ind(ind, stage);
|
|
if ((status == UWB_RX_OK) && ranging_poll_pkt_process(ind->pkt_data, ind->pkt_len))
|
{
|
ranging_user_pkt.seq_num = ranging_recv_pkt->seq_num;
|
// update anchor point
|
ranging_env.anchor_point = ind->timestamp - phy_shr_duration();
|
ranging_env.rx_poll_time = ranging_rx_time(ind);
|
ranging_env.lost_cnt = 0;
|
|
target_time = ranging_env.anchor_point + (1 + ranging_env.responder_slot_idx) * ranging_env.slot_interval;
|
uwb_app_config.ranging_stage = RANGING_RESPONSE;
|
|
#if DYNAMIC_UPDATE_MAIN_ANTENNA_EN
|
if (uwb_app_config.session_param.aoa_result_req)
|
{
|
// update main antenna
|
sts_rx_main_ant_get(&ranging_env.main_ant_id);
|
}
|
#endif
|
}
|
else
|
{
|
target_time += ranging_env.ranging_period;
|
ranging_env.lost_cnt++;
|
}
|
|
if (ranging_env.lost_cnt > 3)
|
{
|
// receive sync again
|
uwb_app_config.ranging_stage = RANGING_SYNC;
|
}
|
|
if (uwb_app_config.session_param.aoa_result_req)
|
{
|
phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_NEVER_SWITCH, 0, 0);
|
}
|
sts_lsp_store_stop();
|
}
|
break;
|
|
case RANGING_RESPONSE:
|
{
|
// Store tx timestamp
|
ranging_env.tx_response_time = ranging_tx_time(ind->timestamp);
|
|
if (ind->err_code == UWB_TX_OK)
|
{
|
target_time += ranging_env.slot_interval * (1 + ranging_env.responder_num);
|
uwb_app_config.ranging_stage = RANGING_FINAL;
|
}
|
else
|
{
|
target_time += ranging_env.ranging_period;
|
#if ASYNC_MODE_EN
|
uwb_app_config.ranging_stage = RANGING_SYNC;
|
#else
|
uwb_app_config.ranging_stage = RANGING_POLL;
|
#endif
|
}
|
|
// process tx-response packet
|
uwb_pkt_tx_done_ind(ind, stage);
|
}
|
break;
|
|
case RANGING_FINAL:
|
{
|
// process rx-final packet
|
status = uwb_pkt_rx_done_ind(ind, stage);
|
|
if ((status == UWB_RX_OK) && (ranging_recv_pkt->session_id == ranging_user_pkt.session_id) && (ranging_recv_pkt->msg_type == RANGING_FINAL))
|
{
|
ranging_env.rx_final_time = ranging_rx_time(ind);
|
ranging_env.lost_cnt = 0;
|
ranging_env.responder_final_flag = 0x1;
|
|
#if DYNAMIC_UPDATE_MAIN_ANTENNA_EN
|
if (uwb_app_config.session_param.aoa_result_req)
|
{
|
// update main antenna
|
sts_rx_main_ant_get(&ranging_env.main_ant_id);
|
}
|
#endif
|
}
|
else
|
{
|
ranging_env.lost_cnt++;
|
}
|
#if RANGING_RESULT_REPORT_EN
|
if (ranging_env.lost_cnt == 0)
|
{
|
target_time =
|
ranging_env.anchor_point + (1 + ranging_env.responder_num + 1 + ranging_env.responder_slot_idx) * ranging_env.slot_interval;
|
uwb_app_config.ranging_stage = RANGING_RESULT;
|
}
|
else
|
#endif
|
{
|
target_time += ranging_env.ranging_period;
|
|
#if ASYNC_MODE_EN
|
uwb_app_config.ranging_stage = RANGING_SYNC;
|
#else
|
uwb_app_config.ranging_stage = RANGING_POLL;
|
#endif
|
}
|
|
if (uwb_app_config.session_param.aoa_result_req)
|
{
|
phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_NEVER_SWITCH, 0, 0);
|
}
|
sts_lsp_store_stop();
|
}
|
break;
|
|
#if RANGING_RESULT_REPORT_EN
|
case RANGING_RESULT:
|
{
|
target_time += ranging_env.ranging_period;
|
|
#if ASYNC_MODE_EN
|
uwb_app_config.ranging_stage = RANGING_SYNC;
|
#else
|
uwb_app_config.ranging_stage = RANGING_POLL;
|
#endif
|
// process tx-result packet
|
uwb_pkt_tx_done_ind(ind, stage);
|
}
|
break;
|
#endif
|
|
default:
|
break;
|
}
|
}
|
|
stage = uwb_app_config.ranging_stage;
|
if (RANGING_POLL == stage)
|
{
|
power_mode_request(POWER_UNIT_APP, POWER_MODE_POWER_DOWN);
|
phy_timer_target_set(
|
target_time - UWB_PERIOD_PREFETCH_TIME - LPM_PPM_COMPENSATION(LOW_POWER_CLOCK_PPM, uwb_app_config.session_param.ranging_interval),
|
ranging_timer_callback);
|
}
|
else if (RANGING_SYNC == stage)
|
{
|
sts_lsp_store();
|
// receive sync again
|
power_on_radio(0, 1);
|
mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, ranging_env.ranging_period);
|
mac_start();
|
ranging_env.synced = 0;
|
}
|
else
|
{
|
phy_timer_target_set(target_time - UWB_EVT_PREFETCH_TIME, ranging_timer_callback);
|
}
|
}
|
else
|
{
|
power_mode_clear(POWER_UNIT_APP);
|
LOG_INFO(TRACE_MODULE_APP, "Ranging stop\r\n");
|
}
|
// board_led_off(BOARD_LED_2);
|
|
return stage;
|
}
|
#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
|
#pragma clang diagnostic pop
|
#endif
|
|
static void uwb_pkt_tx_done_ind(const struct MAC_HW_REPORT_T *tx, enum RANGING_STAGE_T stage)
|
{
|
struct UWB_PKT_TX_DONE_IND_T *ind;
|
if ((ind = WsfMsgAlloc(sizeof(struct UWB_PKT_TX_DONE_IND_T) + tx->pkt_len)) != NULL)
|
{
|
ind->hdr.event = UWB_PKT_TX_DONE_MSG;
|
ind->ranging_stage = (uint8_t)stage;
|
ind->status = tx->err_code;
|
|
ind->tx_len = tx->pkt_len;
|
|
if ((ind->tx_len) && (tx->pkt_data != NULL))
|
{
|
memcpy(ind->tx_data, tx->pkt_data, tx->pkt_len);
|
}
|
|
// Send the message
|
WsfMsgSend(ranging_cb.handle_id, ind);
|
}
|
else
|
{
|
LOG_WARNING(TRACE_MODULE_UWB, "memory is not enough for UWB_PKT_TX_DONE_IND_T\r\n");
|
}
|
}
|
|
#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
|
#pragma clang diagnostic push
|
#pragma clang diagnostic ignored "-Wcast-qual"
|
#endif
|
static uint16_t uwb_pkt_rx_done_ind(const struct MAC_HW_REPORT_T *rx, enum RANGING_STAGE_T stage)
|
{
|
// send an indication to application
|
struct UWB_PKT_RX_DONE_IND_T *ind = WsfMsgAlloc(sizeof(struct UWB_PKT_RX_DONE_IND_T) + rx->pkt_len);
|
|
if (ind != NULL)
|
{
|
ind->hdr.event = UWB_PKT_RX_DONE_MSG;
|
ind->ranging_stage = (uint8_t)stage;
|
ind->status = rx->err_code;
|
ind->rssi = rx->rssi;
|
ind->snr = rx->snr;
|
|
if (rx->err_code == UWB_RX_OK)
|
{
|
uint8_t sts_pkt_cfg = (rx->phy_header >> 28) & 0x3;
|
if ((sts_pkt_cfg != SP0) && (sts_valid_check() == 0))
|
{
|
ind->status |= UWB_STS_ERR;
|
}
|
|
if ((rx->pkt_len) && (rx->pkt_data != NULL))
|
{
|
memcpy(ind->rx_data, rx->pkt_data, rx->pkt_len);
|
}
|
ind->rx_len = rx->pkt_len;
|
}
|
|
#if RANGING_FOM_FILTER_EN
|
if (ranging_debug_csi_en_get())
|
{
|
uint8_t frame_idx = (ind->ranging_stage == RANGING_FINAL ? 1 : 0);
|
debug_csi.frame_idx = frame_idx;
|
debug_csi.rframe_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;
|
|
dump_preamble_cir(frame_idx, 128);
|
dump_sts_cir(frame_idx);
|
}
|
}
|
#endif
|
|
// Send the message
|
WsfMsgSend(ranging_cb.handle_id, ind);
|
return ind->status;
|
}
|
else
|
{
|
LOG_WARNING(TRACE_MODULE_UWB, "memory is not enough for UWB_PKT_RX_DONE_IND_T %d\r\n", rx->pkt_len);
|
return 0xFFFF;
|
}
|
}
|
#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
|
#pragma clang diagnostic pop
|
#endif
|
|
static void ranging_tx_process(struct MAC_HW_REPORT_T *tx_report)
|
{
|
ranging_fsm(tx_report);
|
}
|
|
static void ranging_rx_process(struct MAC_HW_REPORT_T *rx_report)
|
{
|
ranging_fsm(rx_report);
|
}
|
