ChinaUWBProject.git

			@@ -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.
			@@ -48,7 +48,7 @@
			#include "ranging_custom.h"
			#include "lib_aoa.h"
			#include "libc_rom.h"
			#if KF_EN
			#if FILTER_EN
			#include "lib_kf.h"
			#endif
			#if PDOA_3D_EN
			@@ -58,13 +58,13 @@
			#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];
			#endif

			#if KF_EN && FILTER_EN
			#if 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];
			@@ -161,12 +161,11 @@

			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
			// Reduce inter frame spacing (IFS) for non encrypted packets
			mac_ifs_set(0, 0);

			#if (ANT_PATTERN == ANT_PATTERN_SQUARE)
			struct AOA_ANGLE_SPAN_T aoa_span;
			angle_span_t aoa_span;
			#if AOA_3D_EN
			aoa_span.Ndim = 2;
			aoa_span.el_low = 0;
			@@ -188,28 +187,22 @@
			#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));
			sts_param_config(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_AUX_ANT_IQ_RSSI_PDOA_AOA_FOM, STS_BUF_NUM, STS_BUF_SIZE);
			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);
			sts_param_config(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_AUX_ANT_IQ_RSSI, STS_BUF_NUM, STS_BUF_SIZE);
			#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_3d_param_config(ANT_PATTERN, ANT_LAYOUT, PDOA_3D_AMBIGUITY_LEVEL_NONE, 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
			uint32_t update_period_ms = uwb_app_config.session_param.ranging_interval;
			loc_post_kf_config(update_period_ms, kf_mac_addr_cache, kf_channel_cache, kf_mat_value_cache, KF_SUPPORT_NUM, KF_TIMEOUT_MS);
			}
			#endif

			@@ -228,6 +221,7 @@
			if (dev_role == DEV_ROLE_INITIATOR)
			{
			uwb_app_config.ranging_stage = RANGING_POLL;
			ranging_env.synced = 1;
			}
			else
			{
			@@ -239,14 +233,14 @@
			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);
			mk_timer_list_start_timer(&ranging_cb.daemon_timer, uwb_app_config.session_param.ranging_interval * 10, MK_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);
			mk_timer_list_stop_timer(&ranging_cb.daemon_timer);
			}

			void ranging_restart(void)
			@@ -259,7 +253,7 @@

			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);
			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)
			@@ -279,7 +273,7 @@
			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;
			ranging_env.main_ant_id = uwb_app_config.ppdu_params->rx_main_ant;
			return 1;
			}
			return 0;
			@@ -360,19 +354,6 @@
			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)
			{
			@@ -392,7 +373,7 @@
			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);
			tround -= ranging_ant_delays_get(uwb_app_config.ppdu_params->rx_main_ant);

			return tround;
			}
			@@ -416,7 +397,7 @@
			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);
			treply += ranging_ant_delays_get(uwb_app_config.ppdu_params->rx_main_ant);

			return treply;
			}
			@@ -497,7 +478,7 @@
			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];
			struct TWR_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;

			@@ -551,7 +532,7 @@

			static void ranging_timer_callback(void *dev, uint32_t time)
			{
			// board_led_on(BOARD_LED_1);
			// board_led_on(BOARD_LED_2);
			if (ranging_env.enable)
			{
			enum RANGING_STAGE_T stage = uwb_app_config.ranging_stage;
			@@ -578,15 +559,11 @@

			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);
			uwb_tx((uint8_t *)&ranging_user_pkt, user_poll_data_len + MSG_HEADER_LEN + MSG_POLL_USER_DATA_IDX, TX_MODE_CSMACA, target_time);
			}
			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);
			uwb_tx((uint8_t *)&ranging_user_pkt, user_poll_data_len + MSG_HEADER_LEN + MSG_POLL_USER_DATA_IDX, TX_MODE_DEFER, target_time);
			}

			power_mode_request(POWER_UNIT_APP, POWER_MODE_SLEEP);
			@@ -597,11 +574,9 @@
			{
			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);
			uwb_rx(RX_MODE_DEFER, target_time - UWB_RX_OPEN_IN_ADVANCE, UWB_RX_WINDOW);
			}
			break;

			@@ -614,8 +589,7 @@

			// 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);
			uwb_tx((uint8_t *)&ranging_user_pkt, MSG_HEADER_LEN + MSG_FINAL_USER_DATA_IDX, TX_MODE_DEFER, target_time);
			}
			break;

			@@ -624,11 +598,9 @@
			{
			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);
			uwb_rx(RX_MODE_DEFER, target_time - UWB_RX_OPEN_IN_ADVANCE, UWB_RX_WINDOW);
			}
			break;
			#endif
			@@ -650,21 +622,19 @@
			{
			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);
			phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 1, ranging_env.main_ant_id);
			sts_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);
			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);
			uwb_rx(RX_MODE_IMMEDIATE, 0, ranging_env.ranging_period);
			power_mode_request(POWER_UNIT_APP, POWER_MODE_SLEEP);
			ranging_env.responder_final_flag = 0;
			}
			@@ -674,23 +644,21 @@
			{
			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);
			phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 1, ranging_env.main_ant_id);
			sts_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);
			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));
			uwb_rx(RX_MODE_DEFER, target_time - UWB_RX_OPEN_IN_ADVANCE,
			UWB_RX_WINDOW + 2 * 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;
			}
			@@ -705,15 +673,11 @@

			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);
			uwb_tx((uint8_t *)&ranging_user_pkt, user_response_data_len + MSG_HEADER_LEN + MSG_RESPONSE_USER_DATA_IDX, TX_MODE_CSMACA, target_time);
			}
			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);
			uwb_tx((uint8_t *)&ranging_user_pkt, user_response_data_len + MSG_HEADER_LEN + MSG_RESPONSE_USER_DATA_IDX, TX_MODE_DEFER, target_time);
			}
			}
			break;
			@@ -721,22 +685,20 @@
			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);
			phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 1, ranging_env.main_ant_id);
			sts_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);
			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);
			uwb_rx(RX_MODE_DEFER, target_time - UWB_RX_OPEN_IN_ADVANCE, UWB_RX_WINDOW);
			}
			break;

			@@ -745,10 +707,8 @@
			{
			// 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);
			uwb_tx((uint8_t *)&ranging_user_pkt, user_response_data_len + MSG_HEADER_LEN + MSG_RESULT_INFO_LEN, TX_MODE_DEFER, target_time);
			}
			break;
			#endif
			@@ -758,15 +718,13 @@
			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);
			// board_led_off(BOARD_LED_2);
			}

			enum RANGING_STAGE_T ranging_fsm(const struct MAC_HW_REPORT_T *ind)
			@@ -777,6 +735,8 @@

			// board_led_on(BOARD_LED_2);
			power_off_radio();
			sts_lsp_store_stop();

			if (ranging_env.enable)
			{
			ranging_env.count++;
			@@ -844,12 +804,6 @@
			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
			}
			}

			@@ -871,7 +825,6 @@
			uwb_app_config.ranging_stage = RANGING_POLL;
			}
			}
			sts_lsp_store_stop();
			}
			break;

			@@ -956,9 +909,8 @@

			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);
			phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_NEVER_SWITCH, 0, 0);
			}
			sts_lsp_store_stop();
			}
			break;

			@@ -1000,9 +952,8 @@

			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);
			phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_NEVER_SWITCH, 0, 0);
			}
			sts_lsp_store_stop();
			}
			break;

			@@ -1075,9 +1026,8 @@

			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);
			phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params->sts_pkt_cfg, STS_NEVER_SWITCH, 0, 0);
			}
			sts_lsp_store_stop();
			}
			break;

			@@ -1112,11 +1062,20 @@
			}
			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();

			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);
			sts_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
			}

			uwb_rx(RX_MODE_IMMEDIATE, 0, ranging_env.ranging_period);
			ranging_env.synced = 0;
			}
			else
			@@ -1176,8 +1135,9 @@
			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;
			// ind->rssi = rx->rssi;
			ind->rssi = correct_rssi(rx->rssi);
			ind->snr = correct_snr(rx->snr);

			if (rx->err_code == UWB_RX_OK)
			{
			@@ -1198,8 +1158,9 @@
			if (ranging_debug_csi_en_get())
			{
			uint8_t frame_idx = (ind->ranging_stage == RANGING_FINAL ? 1 : 0);

			debug_csi.antenna = ranging_env.main_ant_id;
			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);
			@@ -1211,9 +1172,8 @@
			{
			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);
			debug_csi.frame[frame_idx].channel_power = REG_READ(0x40002064);
			debug_csi.frame[frame_idx].noise_power = REG_READ(0x40002078);
			}
			}
			#endif