/*
|
* 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_uwb.h"
|
#include "mk_aes.h"
|
#include "mk_power.h"
|
#include "mk_misc.h"
|
|
#include "lib_aoa.h"
|
#include "lib_ranging.h"
|
#include "ranging_fira.h"
|
#include "uwb_twr.h"
|
#ifdef RADAR_EN
|
#include "uwb_radar.h"
|
#endif
|
#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 3
|
#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
|
|
struct RANGING_ENV_T ranging_env;
|
uint8_t fira_uwb_tx_buf[PHY_PAYLOAD_LEN_MAX];
|
|
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_dynamic_update_responder_list = ranging_dynamic_update_respoder_list,
|
.session_set_ccc_ursk = NULL,
|
#ifdef RADAR_EN
|
.vendor_session_configure = uwb_radar_configure,
|
.vendor_session_start = uwb_radar_start,
|
.vendor_session_stop = uwb_radar_stop,
|
#else
|
.vendor_session_configure = NULL,
|
.vendor_session_start = NULL,
|
.vendor_session_stop = NULL,
|
#endif
|
};
|
|
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 normal_uwb_change_to_fira(void);
|
void fira_uwb_change_to_normal(void);
|
void uwb_normal_init(void);
|
void uwb_fira_init(void);
|
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());
|
|
return 0;
|
}
|
|
int ranging_deinit(void)
|
{
|
return 0;
|
}
|
|
// This function will be called by uwbapi_session_init()
|
void app_session_init(void)
|
{
|
normal_uwb_change_to_fira();//±äΪfiraÅäÖõÄuwb
|
// register process handler for MAC TX done and RX done
|
mac_register_process_handler(ranging_tx_process, ranging_rx_process);
|
|
uwbs_handler_init(&op);
|
}
|
|
void ranging_configure(void)
|
{
|
|
fira_keys_generate();
|
|
aes_update_key(AES_ID0, &fira_key.devPayKey.ukey.keyByte[0]);
|
mac_update_ccm_key((uint32_t *)&fira_key.devPayKey.ukey.keyWord[0]);
|
|
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);
|
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));
|
ASSERT(ranging_env.slots_per_block >= uwb_app_config.session_param.slots_per_round, "Ranging block parameters configure wrong");
|
|
ranging_env.slot_interval = RSTU_TO_PHY_TIMER_COUNT(uwb_app_config.session_param.slot_duration);
|
ranging_env.round_duration = ranging_env.slot_interval * uwb_app_config.session_param.slots_per_round;
|
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.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;
|
ranging_env.stride_length = uwb_app_config.session_param.stride_length;
|
ranging_env.round_offset_in_block = 0;
|
ranging_env.next_round_index = 0;
|
if (ranging_env.stride_length > 0)
|
{
|
ranging_env.phy_sts_index = fira_key.phyStsIdxInit - (ranging_env.stride_length + 1) * ranging_env.slots_per_block;
|
}
|
else
|
{
|
ranging_env.phy_sts_index = fira_key.phyStsIdxInit - ranging_env.slots_per_block;
|
}
|
|
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;
|
aoa_span.Ndim = 2;
|
aoa_span.el_low = 90;
|
aoa_span.el_high = 90;
|
aoa_span.el_step = 1;
|
aoa_span.az_low = 0;
|
aoa_span.az_high = 359;
|
aoa_span.az_step = 1;
|
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
|
|
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);
|
}
|
struct mk_uwb_configure
|
{
|
uint8_t phy_work_mode; /* PHY_TX / PHY_RX / PHT_TX|PHY_RX */
|
struct UWB_CONFIG_T phy_cfg;
|
};
|
#ifdef DW1000
|
/* Default communication configuration. */
|
static struct mk_uwb_configure config = {//yuan
|
.phy_work_mode = (uint8_t)(PHY_TX | PHY_RX),
|
.phy_cfg.ch_num = UWB_CH_NUM, /* Channel number. */
|
.phy_cfg.code_index = 9, /* TRX preamble code */
|
.phy_cfg.mean_prf = MEAN_PRF_64M, /* Mean prf 64/128/256M */
|
.phy_cfg.data_bit_rate = DATA_BR_6M8, /* Data rate 6.8M */
|
.phy_cfg.sync_sym = PREAM_LEN_128, /* Preamble duration, length of preamble 128 */
|
.phy_cfg.sfd_sym = NON_STD_NSFD5_8, /* Identifier for SFD sequence */
|
.phy_cfg.ranging_bit = 1, /* ranging bit set 1 */
|
.phy_cfg.trx_mode = TRX_MODE_15_4A, /* IEEE802.15.4z - BPRF mode */
|
.phy_cfg.sts_pkt_cfg = STS_PKT_CFG_0, /* SP0 Frame */
|
.phy_cfg.sts_segnum = STS_SEGNUM_BPRF_1, /* Number of STS segments in the frame */
|
.phy_cfg.sts_seglen = STS_SEGLEN_BPRF_64, /* Number of symbols in an STS segment */
|
.phy_cfg.rx_ant_id = UWB_RX_ANT_3, /* UWB RX antenna port */
|
};
|
#elif defined STS_MODE
|
static struct mk_uwb_configure config = {
|
.phy_work_mode = (uint8_t)(PHY_TX | PHY_RX),
|
.phy_cfg.ch_num = 5, /* Channel number. */
|
.phy_cfg.code_index = 9, /* TRX preamble code */
|
.phy_cfg.mean_prf = MEAN_PRF_64M, /* Mean prf 64/128/256M */
|
.phy_cfg.data_bit_rate = DATA_BR_6M8, /* Data rate 6.8M */
|
.phy_cfg.sync_sym = PREAM_LEN_128, /* Preamble duration, length of preamble 128 */
|
.phy_cfg.sfd_sym = BPRF_NSFD2_8, /* Identifier for SFD sequence */
|
.phy_cfg.ranging_bit = 1, /* ranging bit set 1 */
|
.phy_cfg.trx_mode = TRX_MODE_15_4Z_BPRF, /* IEEE802.15.4z - BPRF mode */
|
.phy_cfg.sts_pkt_cfg = STS_PKT_CFG_1, /* SP1 Frame */
|
.phy_cfg.sts_segnum = STS_SEGNUM_BPRF_1, /* Number of STS segments in the frame */
|
.phy_cfg.sts_seglen = STS_SEGLEN_BPRF_64, /* Number of symbols in an STS segment */
|
.phy_cfg.rx_ant_id = UWB_RX_ANT_3, /* UWB RX antenna port */
|
};
|
/* Use the default key and IV specified in the IEEE 802.15.4z attachment */
|
static struct UWB_STS_KEY_CONFIG_T sts_iv_key = {
|
.sts_vcounter = 0x1F9A3DE4,
|
.sts_vupper0 = 0xD37EC3CA,
|
.sts_vupper1 = 0xC44FA8FB,
|
.sts_vupper2 = 0x362EEB34,
|
.sts_key0 = 0x14EB220F,
|
.sts_key1 = 0xF86050A8,
|
.sts_key2 = 0xD1D336AA,
|
.sts_key3 = 0x14148674,
|
};
|
#else //MKmode
|
/* Default communication configuration. */
|
static struct mk_uwb_configure config = {
|
.phy_work_mode = (uint8_t)(PHY_TX | PHY_RX),
|
.phy_cfg.ch_num = 9, /* Channel number. */
|
.phy_cfg.code_index = 9, /* TX preamble code. */
|
.phy_cfg.mean_prf = MEAN_PRF_64M, /* Data rate 6.8M */
|
.phy_cfg.data_bit_rate = DATA_BR_6M8, /* data rate 6.8M. */
|
.phy_cfg.sync_sym = PREAM_LEN_128, /* Preamble duration, length of preamble 128 */
|
.phy_cfg.sfd_sym = BPRF_NSFD2_8, /* Identifier for SFD sequence */
|
.phy_cfg.ranging_bit = 1, /* ranging bit set. */
|
.phy_cfg.trx_mode = TRX_MODE_15_4Z_BPRF, /* IEEE802.15.4z - BPRF mode */
|
.phy_cfg.sts_pkt_cfg = STS_PKT_CFG_0, /* SP0 Frame */
|
.phy_cfg.sts_segnum = STS_SEGNUM_BPRF_1, /* Number of STS segments in the frame */
|
.phy_cfg.sts_seglen = STS_SEGLEN_BPRF_64, /* Number of symbols in an STS segment */
|
.phy_cfg.rx_ant_id = UWB_RX_ANT_3, /* UWB RX antenna port */
|
};
|
#endif
|
uint8_t normal_flag=1;
|
void uwb_fira_init(void)
|
{
|
uwb_open();
|
|
// set advanced parameters
|
struct PHY_ADV_CONFIG_T adv_config =
|
{
|
// RPM0: 40, RPM3: 60
|
.thres_fap_detect = 60,
|
// RPM0: 4, RPM3: 8
|
.nth_scale_factor = 8,
|
// RFrame SP0: 0/1, Others: 0/1/2/3
|
.ranging_performance_mode = 3,
|
#if RX_ANT_PORTS_NUM == 4
|
.skip_weakest_port_en = 1,
|
#else
|
.skip_weakest_port_en = 0,
|
#endif
|
};
|
phy_adv_params_configure(&adv_config);
|
|
// which RX ports will be used for AoA/PDoA
|
phy_rx_ant_mode_set(RX_ANT_PORTS_COMBINATION);
|
|
uwbs_init();
|
uwb_app_config.ranging_flow_mode = (uint8_t)(RANGING_FLOW_FIRA);
|
uwb_app_config.filter_en = (uint8_t)(FILTER_EN);
|
uwb_app_config.session_param.tx_power_level = board_param.tx_power_fcc[CALIB_CH(uwb_app_config.ppdu_params.ch_num)];
|
uwb_app_config.ppdu_params.rx_ant_id = (uint8_t)(RX_MAIN_ANT_PORT);
|
}
|
void uwb_normal_init(void)
|
{
|
Uwb_init();
|
}
|
void fira_uwb_change_to_normal(void)
|
{
|
//uwb_close();
|
CloseUWB();
|
uwb_normal_init();
|
normal_flag=1;
|
}
|
void normal_uwb_change_to_fira(void)//ÉÙ¸ö»Øµ÷ÉèÖÃ
|
{
|
//uwb_close();
|
CloseUWB();
|
uwb_fira_init();
|
normal_flag=0;
|
}
|
void ranging_start(void)
|
{
|
ranging_env.enable = 1;
|
ranging_env.lost_cnt = 0xFF;
|
ranging_env.anchor_point = phy_timer_count_get();
|
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;
|
#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))
|
{
|
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);
|
power_mode_request(POWER_UNIT_APP, POWER_MODE_POWER_DOWN);
|
}
|
else
|
{
|
ranging_env.is_hopping = 0;
|
ranging_env.stride_length = 0;
|
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);
|
}
|
|
ranging_env.count = 0;
|
ranging_env.count_last = 0;
|
uwb_app_config.ranging_count = 0;
|
|
ranging_monitor_start(uwb_app_config.session_param.ranging_interval * 10);
|
LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_APP, "Ranging start, role %d\r\n", uwb_app_config.session_param.device_role);
|
}
|
|
void ranging_stop(void)
|
{
|
ranging_env.enable = 0;
|
fira_uwb_change_to_normal();//±äΪnormalÅäÖõÄuwb
|
ranging_monitor_stop();
|
LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_APP, "Ranging stop\r\n");
|
}
|
|
void ranging_restart(void)
|
{
|
ranging_stop();
|
mac_restart();
|
mac_update_ccm_key((uint32_t *)&fira_key.devPayKey.ukey.keyWord[0]);
|
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_measurements_clear(void)
|
{
|
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;
|
}
|
}
|
|
void ranging_monitor_start(uint32_t time_ms)
|
{
|
WsfTimerStartMs(&ranging_cb.daemon_timer, time_ms, WSF_TIMER_PERIODIC);
|
}
|
|
void ranging_monitor_stop(void)
|
{
|
WsfTimerStop(&ranging_cb.daemon_timer);
|
}
|
|
#if MCTT_TEST_EN
|
extern void ranging_round_print_ind(void);
|
void ranging_round_print_ind(void)
|
{
|
struct RANGING_ROUND_PRINT_IND_T *ind;
|
if ((ind = WsfMsgAlloc(sizeof(struct RANGING_ROUND_PRINT_IND_T))) != NULL)
|
{
|
ind->hdr.event = RANGING_ROUND_PRINT_MSG;
|
|
// Send the message
|
WsfMsgSend(ranging_cb.handle_id, ind);
|
}
|
else
|
{
|
LOG_WARNING(TRACE_MODULE_UWB, "memory is not enough for RANGING_ROUND_PRINT_IND_T\r\n");
|
}
|
}
|
#endif
|
|
void uwb_pkt_tx_done_ind(const struct MAC_HW_REPORT_T *tx, enum RANGING_STAGE_T stage, uint8_t slot_idx)
|
{
|
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->slot_idx = slot_idx;
|
ind->status = tx->err_code;
|
#if MCTT_TEST_EN
|
ind->timestamp = tx->timestamp;
|
ind->phy_shr_duration = phy_shr_duration();
|
#endif
|
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
|
uint16_t uwb_pkt_rx_done_ind(const struct MAC_HW_REPORT_T *rx, enum RANGING_STAGE_T stage, uint8_t slot_idx)
|
{
|
// 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->slot_idx = slot_idx;
|
ind->status = rx->err_code;
|
ind->rssi = rx->rssi;
|
ind->snr = rx->snr;
|
|
#if MCTT_TEST_EN
|
ind->timestamp = rx->timestamp;
|
#endif
|
|
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 MCTT_TEST_EN
|
ind->phy_header = rx->phy_header;
|
#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\r\n");
|
return 0xFFFF;
|
}
|
}
|
#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
|
#pragma clang diagnostic pop
|
#endif
|
|
void distance_result_filter(uint8_t *mac_addr, uint16_t *distance)
|
{
|
if (distance == NULL)
|
{
|
return;
|
}
|
|
#if KF_EN
|
float post_range;
|
float range_meas = (float)*distance / 100;
|
// call filter
|
loc_kf_filter(range_meas, KF_DATA_TYPE_RANGING, mac_addr, &post_range);
|
// update distance
|
*distance = (uint16_t)(post_range * 100);
|
// LOG_INFO(TRACE_MODULE_APP, "Peer %X, $%u %u;\r\n", READ_SHORT(mac_addr), (uint16_t)(range_meas*100),(uint16_t)(post_range*100));
|
#endif
|
}
|
|
void angle_result_filter(uint8_t *mac_addr, int16_t *angle, uint8_t type)
|
{
|
if (angle == NULL)
|
{
|
return;
|
}
|
|
#if KF_EN
|
float post_angle;
|
float angle_meas = mk_q7_to_f32(*angle);
|
// call filter
|
loc_kf_filter(angle_meas, (enum KF_DATA_TYPE_T)type, mac_addr, &post_angle);
|
// update angle
|
*angle = mk_f32_to_q7(post_angle);
|
// LOG_INFO(TRACE_MODULE_APP, "Peer %X, $%d %d;\r\n", READ_SHORT(mac_addr), (int16_t)angle_meas,(int16_t)post_angle);
|
#endif
|
}
|
|
void ranging_result_filter(uint8_t *mac_addr, uint16_t *distance, int16_t *azimuth, int16_t *elevation)
|
{
|
#if KF_EN
|
if (distance)
|
{
|
distance_result_filter(mac_addr, distance);
|
}
|
if (azimuth)
|
{
|
angle_result_filter(mac_addr, azimuth, KF_DATA_TYPE_AZIMUTH);
|
}
|
if (elevation)
|
{
|
angle_result_filter(mac_addr, elevation, KF_DATA_TYPE_ELEVATION);
|
}
|
#else
|
// do not support multi-channel
|
if ((distance == NULL) || (azimuth == NULL))
|
{
|
return;
|
}
|
|
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
|
}
|
|
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];
|
|
if (uwb_app_config.session_param.ranging_round_usage == DS_TWR_DEFERRED || uwb_app_config.session_param.ranging_round_usage == DS_TWR)
|
{
|
if (1 == measurement_report_message_type1_process(ind->rx_data, ind->rx_len))
|
{
|
int64_t Tround1 = 0;
|
int64_t Tround2 = ranging_tround(DEV_ROLE_RESPONDER, 0);
|
int64_t Treply1 = ranging_treply(DEV_ROLE_RESPONDER, 0);
|
int64_t Treply2 = 0;
|
|
struct MEASUREMENT_REPORT_TYPE1_INFO *p_info = &uwb_fira_msg_info.mrm_type1;
|
|
for (uint8_t n = 0; n < p_info->reply_time_num; n++)
|
{
|
if (p_info->reply_time_list[n].address == uwbs_local_short_addr_get())
|
{
|
Treply2 = p_info->reply_time_list[n].reply_time;
|
break;
|
}
|
}
|
|
if (Treply2)
|
{
|
Tround1 = p_info->first_round_trip_time + p_info->reply_time_list[0].reply_time - Treply2;
|
}
|
|
// LOG_INFO(TRACE_MODULE_FIRA, "Tround1 %u Treply1 %u Tround2 %u Treply2 %u\r\n",
|
// (uint32_t)Tround1,(uint32_t)Treply1,(uint32_t)Tround2,(uint32_t)Treply2);
|
|
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);
|
range_result->distance = (uint16_t)(tof_f * 0.299702547 * VP_VAL - RANGING_CORR);
|
|
// LOG_INFO(TRACE_MODULE_FIRA, "Tround1 %lld Treply1 %lld Tround2 %lld Treply2 %lld\r\n", Tround1, Treply1, Tround2, Treply2);
|
ret = 1;
|
}
|
}
|
else
|
{
|
range_result->status = STATUS_FAILED;
|
}
|
}
|
else if (uwb_app_config.session_param.ranging_round_usage == SS_TWR_DEFERRED)
|
{
|
#if FIRA_TEST_EN
|
volatile int64_t Tround = 0;
|
volatile int64_t Treply = ranging_treply(DEV_ROLE_RESPONDER, 0);
|
// Measurement Report Message Type 2
|
// 34-35 --> Message Control
|
// 36-37 --> Round Index
|
// 40... --> Round-trip Time List
|
uint8_t responder_num = (ind->rx_data[34] >> 2);
|
for (uint8_t n = 0; n < responder_num; n++)
|
{
|
uint16_t addr = READ_SHORT(&ind->rx_data[38 + 6 * n]);
|
if (addr == uwbs_local_short_addr_get())
|
{
|
Tround = (int64_t)READ_WORD(&ind->rx_data[40 + 6 * n]);
|
break;
|
}
|
}
|
#else
|
int64_t Tround = 0;
|
int64_t Treply = ranging_treply(DEV_ROLE_RESPONDER, 0);
|
|
// 34 --> Message Control
|
// 35-36 --> Round Index
|
// 37... --> Round-trip Time List
|
uint8_t responder_num = (ind->rx_data[34] >> 2);
|
for (uint8_t n = 0; n < responder_num; n++)
|
{
|
uint16_t addr = READ_SHORT(&ind->rx_data[37 + 6 * n]);
|
if (addr == uwbs_local_short_addr_get())
|
{
|
Tround = READ_WORD(&ind->rx_data[39 + 6 * n]);
|
break;
|
}
|
}
|
#endif
|
// LOG_INFO(TRACE_MODULE_FIRA, "Tround: %lld, Treply: %lld\r\n", Tround, Treply);
|
|
if ((Tround) && (Treply))
|
{
|
// corrected by frequency offset
|
// Treply = (int64_t)((double)Treply * (1 - (double)ranging_env.freq_offset_filter / ch_center_freq_map[uwb_app_config.ppdu_params.ch_num]));
|
Tround = (int64_t)((double)Tround * (1 + (double)ranging_env.freq_offset_filter / ch_center_freq_map[uwb_app_config.ppdu_params.ch_num]));
|
|
int64_t tof_i = (Tround - Treply) / 2;
|
|
// 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);
|
range_result->distance = (uint16_t)(tof_f * 0.299702547 * VP_VAL - RANGING_CORR);
|
|
ret = 1;
|
}
|
else
|
{
|
range_result->status = STATUS_FAILED;
|
}
|
}
|
else if (uwb_app_config.session_param.ranging_round_usage == SS_TWR)
|
{
|
if (1 == measurement_report_message_type2_process(ind->rx_data, ind->rx_len))
|
{
|
struct MEASUREMENT_REPORT_TYPE2_INFO *p_info = &uwb_fira_msg_info.mrm_type2;
|
|
volatile int64_t Tround = ranging_tround(DEV_ROLE_INITIATOR, ind->slot_idx - 1);
|
volatile int64_t Treply = p_info->flag_reply_time == 1 ? p_info->reply_time : 0;
|
|
if ((Tround) && (Treply))
|
{
|
int64_t tof_i = (Tround - Treply) / 2;
|
|
// 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);
|
range_result->distance = (uint16_t)(tof_f * 0.299702547 * VP_VAL - RANGING_CORR);
|
|
ret = 1;
|
}
|
else
|
{
|
range_result->status = STATUS_FAILED;
|
}
|
}
|
else
|
{
|
range_result->status = STATUS_FAILED;
|
}
|
}
|
|
return ret;
|
}
|
|
int ranging_result_report_handler(const struct UWB_PKT_RX_DONE_IND_T *ind)
|
{
|
int ret = 0;
|
uint8_t slot_idx = (ranging_env.result_flag >> 8) & 0xFF;
|
uint8_t status = ranging_env.result_flag & 0xFF;
|
|
// 1. update
|
if (status & 0x1)
|
{
|
uint32_t tof = 0;
|
int16_t azimuth = 0;
|
int16_t elevation = 0;
|
uint8_t azimuth_fom = 0;
|
uint8_t elevation_fom = 0;
|
|
// process rx-result packet
|
uint8_t flag = ind->rx_data[34];
|
if (flag & 0x01)
|
{
|
tof = READ_WORD(&ind->rx_data[35]);
|
}
|
if (flag & 0x02)
|
{
|
azimuth = (int16_t)READ_SHORT(&ind->rx_data[39]);
|
}
|
if (flag & 0x04)
|
{
|
elevation = (int16_t)READ_SHORT(&ind->rx_data[41]);
|
if (flag & 0x08)
|
{
|
azimuth_fom = ind->rx_data[43];
|
elevation_fom = ind->rx_data[44];
|
}
|
}
|
else
|
{
|
if (flag & 0x08)
|
{
|
azimuth_fom = ind->rx_data[41];
|
elevation_fom = ind->rx_data[42];
|
}
|
}
|
|
uint8_t initiator_tx_slot_count = 0;
|
if (uwb_app_config.session_param.ranging_round_usage == DS_TWR_DEFERRED)
|
{
|
initiator_tx_slot_count = 4; // rcm + poll + final + report
|
}
|
else if (uwb_app_config.session_param.ranging_round_usage == SS_TWR_DEFERRED)
|
{
|
initiator_tx_slot_count = 3; // rcm + poll + report
|
}
|
else if (uwb_app_config.session_param.ranging_round_usage == DS_TWR)
|
{
|
initiator_tx_slot_count = 2; // poll + final
|
}
|
|
uint8_t responder_idx = slot_idx - initiator_tx_slot_count - ranging_responder_num_get();
|
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];
|
|
range_result->mac_addr[0] = target_addr & 0xff;
|
range_result->mac_addr[1] = (target_addr >> 8) & 0xff;
|
|
// update distance
|
range_result->distance = (uint16_t)(tof_f * 0.299702547 * VP_VAL - RANGING_CORR);
|
|
// update destination angles
|
range_result->aoa_dst_azimuth = uwbapi_angle_ota_format_to_q7(azimuth, 180);
|
range_result->aoa_dst_elevation = uwbapi_angle_ota_format_to_q7(elevation, 90);
|
range_result->aoa_dst_azimuth_fom = azimuth_fom;
|
range_result->aoa_dst_elevation_fom = elevation_fom;
|
|
if (range_result->status == STATUS_RANGING_RX_RESPONSE_OK)
|
{
|
// check this flag to send out measurement report
|
range_result->status = STATUS_OK;
|
ranging_env.range_data.measurements_num++;
|
if (uwb_app_config.session_param.aoa_result_req == 0)
|
{
|
range_result->aoa_azimuth = 0;
|
range_result->aoa_elevation = 0;
|
}
|
|
#if FILTER_EN
|
if (uwb_app_config.filter_en)
|
{
|
// multi-channel filter process
|
#if PDOA_3D_EN
|
ranging_result_filter(range_result->mac_addr, NULL, &range_result->aoa_azimuth, &range_result->aoa_elevation);
|
#endif
|
}
|
#endif
|
|
if (uwb_app_config.session_param.aoa_result_req)
|
{
|
board_ranging_result_correct(&range_result->distance, &range_result->aoa_azimuth, &range_result->aoa_elevation);
|
}
|
}
|
}
|
|
// 2. complete
|
if ((status & 0x2) && (ranging_env.range_data.measurements_num))
|
{
|
ret = 1;
|
}
|
|
// LOG_INFO(TRACE_MODULE_FIRA, "result report %x %d\r\n", ranging_env.result_flag, ret);
|
|
return ret;
|
}
|
|
void session_timer_callback(void *dev, uint32_t time)
|
{
|
// LOG_INFO(TRACE_MODULE_FIRA, "PHY timer slot %d\r\n", ranging_env.slot_idx);
|
// board_led_on(BOARD_LED_2);
|
|
if (uwb_app_config.session_param.ranging_round_usage == DS_TWR_DEFERRED || uwb_app_config.session_param.ranging_round_usage == SS_TWR_DEFERRED)
|
{
|
ds_twr_phy_timer_callback(dev, time);
|
}
|
else if (uwb_app_config.session_param.ranging_round_usage == DS_TWR || uwb_app_config.session_param.ranging_round_usage == SS_TWR)
|
{
|
ds_twr_non_deferred_phy_timer_callback(dev, time);
|
}
|
|
// board_led_off(BOARD_LED_2);
|
}
|
|
enum RANGING_STAGE_T session_fsm(const struct MAC_HW_REPORT_T *ind)
|
{
|
// LOG_INFO(TRACE_MODULE_FIRA, "FSM %02x slot %d\r\n", uwb_app_config.ranging_stage, ranging_env.slot_idx);
|
// board_led_on(BOARD_LED_2);
|
enum RANGING_STAGE_T stage = RANGING_IDLE;
|
|
if (uwb_app_config.session_param.ranging_round_usage == DS_TWR_DEFERRED || uwb_app_config.session_param.ranging_round_usage == SS_TWR_DEFERRED)
|
{
|
stage = ds_twr_fsm(ind);
|
}
|
else if (uwb_app_config.session_param.ranging_round_usage == DS_TWR || uwb_app_config.session_param.ranging_round_usage == SS_TWR)
|
{
|
stage = ds_twr_non_deferred_fsm(ind);
|
}
|
|
// board_led_off(BOARD_LED_2);
|
return stage;
|
}
|
|
static void ranging_tx_process(struct MAC_HW_REPORT_T *tx_report)
|
{
|
session_fsm(tx_report);
|
}
|
|
static void ranging_rx_process(struct MAC_HW_REPORT_T *rx_report)
|
{
|
session_fsm(rx_report);
|
}
|
