/*
|
* 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_uwb.h"
|
#include "mk_clock.h"
|
#include "uwb_api.h"
|
#include "aoa.h"
|
#include "lib_aoa.h"
|
#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
|
|
struct AOA_ENV_T aoa_env;
|
|
static struct AOA_CB_T aoa_cb;
|
|
static struct UWB_OP_T op = {
|
.session_configure = aoa_configure,
|
.session_start = aoa_start,
|
.session_stop = NULL,
|
.session_local_addr_set = NULL,
|
.session_peer_addr_set = NULL,
|
.session_responder_addr_add = NULL,
|
.session_responder_list_clr = NULL,
|
.session_set_ccc_ursk = NULL,
|
};
|
|
static void aoa_tx_process(struct MAC_HW_REPORT_T *tx_report);
|
static void aoa_rx_process(struct MAC_HW_REPORT_T *rx_report);
|
void app_session_init(void);
|
|
//------------------------------------------------------------------------------
|
int aoa_init(uint8_t handle_id)
|
{
|
/* store handler ID */
|
aoa_cb.handle_id = handle_id;
|
|
/* init rx queue */
|
WSF_QUEUE_INIT(&aoa_cb.msg_queue);
|
|
LOG_INFO(TRACE_MODULE_APP, "AoA lib version: %s\r\n", MK8000_get_aoalib_version());
|
return 0;
|
}
|
|
int aoa_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(aoa_tx_process, aoa_rx_process);
|
|
uwbs_handler_init(&op);
|
}
|
|
void aoa_configure(void)
|
{
|
aoa_env.stage = AOA_IDLE;
|
aoa_env.ranging_period = MS_TO_PHY_TIMER_COUNT(uwb_app_config.session_param.ranging_interval);
|
aoa_env.lost_cnt = 0;
|
|
uwbs_configure(PHY_TX | PHY_RX, uwb_app_config.session_param.tx_power_level);
|
|
if (uwb_app_config.session_param.device_role == DEV_ROLE_INITIATOR)
|
{
|
phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_NEVER_SWITCH, 0, 0);
|
}
|
else
|
{
|
phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
|
}
|
|
#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);
|
#endif
|
|
#if FILTER_EN
|
if (uwb_app_config.filter_en)
|
{
|
loc_post_filter_config(uwb_app_config.session_param.ranging_interval, 0, 1);
|
}
|
#endif
|
}
|
|
static void aoa_timer_callback(void *dev, uint32_t time)
|
{
|
aoa_env.anchor_point += aoa_env.ranging_period;
|
aoa_env.sequence_num++;
|
if (uwb_app_config.session_param.device_role == DEV_ROLE_INITIATOR)
|
{
|
// Power on radio
|
power_on_radio(1, 0);
|
|
// SP3 payload len = 0
|
mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, aoa_env.anchor_point, 0, NULL, 0);
|
mac_start();
|
|
LOG_INFO(TRACE_MODULE_APP, "AoA Initiator SEQ NUM %u\r\n", aoa_env.sequence_num);
|
}
|
else
|
{
|
// Change Rx main antenna
|
uint8_t main_ant = 3;
|
phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 1, main_ant);
|
aoa_param_update(main_ant);
|
// Power on radio
|
power_on_radio(0, 1);
|
sts_lsp_store();
|
if (aoa_env.stage == AOA_SYNC)
|
{
|
mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, aoa_env.ranging_period);
|
}
|
else
|
{
|
mac_rx(EVT_MODE_MAC_ASAP_PHY_FIX, aoa_env.anchor_point - UWB_RX_OPEN_IN_ADVANCE, UWB_RX_WINDOW);
|
}
|
mac_start();
|
LOG_INFO(TRACE_MODULE_APP, "AoA Responder SEQ NUM %u\r\n", aoa_env.sequence_num);
|
}
|
}
|
|
void aoa_start(void)
|
{
|
aoa_env.anchor_point = phy_timer_count_get();
|
if (uwb_app_config.session_param.device_role == DEV_ROLE_INITIATOR)
|
{
|
aoa_env.stage = AOA_POLL;
|
}
|
else
|
{
|
aoa_env.stage = AOA_SYNC;
|
}
|
phy_timer_target_set(aoa_env.anchor_point + aoa_env.ranging_period - UWB_EVT_PREFETCH_TIME, aoa_timer_callback);
|
aoa_env.sequence_num = 0;
|
}
|
|
void aoa_process(const struct MAC_HW_REPORT_T *ind)
|
{
|
if (uwb_app_config.session_param.device_role == DEV_ROLE_INITIATOR)
|
{
|
phy_timer_target_set(aoa_env.anchor_point + aoa_env.ranging_period - UWB_EVT_PREFETCH_TIME, aoa_timer_callback);
|
}
|
else
|
{
|
if (ind->err_code == UWB_RX_OK)
|
{
|
sts_lsp_store_stop();
|
// update anchor point
|
aoa_env.anchor_point = ind->timestamp - phy_shr_duration();
|
aoa_env.stage = AOA_POLL;
|
aoa_env.lost_cnt = 0;
|
|
if (aoa_done_flag)
|
{
|
aoa_done_flag = 0;
|
aoa_done(ind);
|
}
|
}
|
else
|
{
|
if (aoa_env.stage == AOA_SYNC)
|
{
|
sts_lsp_store();
|
mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, aoa_env.ranging_period);
|
mac_start();
|
return;
|
}
|
else
|
{
|
aoa_env.lost_cnt++;
|
if (aoa_env.lost_cnt > 3)
|
{
|
aoa_env.stage = AOA_SYNC;
|
}
|
}
|
}
|
phy_timer_target_set(aoa_env.anchor_point + aoa_env.ranging_period - UWB_EVT_PREFETCH_TIME, aoa_timer_callback);
|
}
|
|
power_off_radio();
|
}
|
|
void aoa_done(const struct MAC_HW_REPORT_T *rx)
|
{
|
struct AOA_DONE_IND_T *ind;
|
|
if ((ind = WsfMsgAlloc(sizeof(struct AOA_DONE_IND_T) + rx->pkt_len)) != NULL)
|
{
|
ind->hdr.event = AOA_DONE_MSG;
|
ind->status = rx->err_code;
|
ind->rssi = rx->rssi;
|
ind->snr = rx->snr;
|
|
if ((rx->pkt_len) && (rx->pkt_data != NULL))
|
{
|
memcpy(ind->rx_data, rx->pkt_data, rx->pkt_len);
|
}
|
ind->rx_len = rx->pkt_len;
|
|
// Send the message
|
WsfMsgSend(aoa_cb.handle_id, ind);
|
}
|
}
|
|
static void aoa_tx_process(struct MAC_HW_REPORT_T *tx_report)
|
{
|
aoa_process(tx_report);
|
}
|
|
static void aoa_rx_process(struct MAC_HW_REPORT_T *rx_report)
|
{
|
aoa_process(rx_report);
|
}
|
