From cc432b761c884a0bd8e9d83db0a4e26109fc08b1 Mon Sep 17 00:00:00 2001
From: chen <15335560115@163.com>
Date: 星期五, 08 十一月 2024 15:35:38 +0800
Subject: [PATCH] 安邦手环GPS删除部分无用数据和修改4G波特率9600出厂测试固件
---
keil/include/components/app/src/ranging_simple.c | 1012 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 1,012 insertions(+), 0 deletions(-)
diff --git a/keil/include/components/app/src/ranging_simple.c b/keil/include/components/app/src/ranging_simple.c
new file mode 100644
index 0000000..cf3e3f2
--- /dev/null
+++ b/keil/include/components/app/src/ranging_simple.c
@@ -0,0 +1,1012 @@
+/*
+ * 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 "wsf_msg.h"
+
+#include "ranging_simple.h"
+#include "lib_aoa.h"
+#include "lib_ranging.h"
+#if KF_EN
+#include "lib_kf.h"
+#endif
+#include "board.h"
+
+#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 struct RANGING_USER_PKT_T ranging_user_pkt;
+static struct RANGING_COMMON_MSG_T ranging_common_msg;
+
+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_dynamic_update_responder_list = NULL,
+ .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 void 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 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);
+}
+
+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;
+ 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 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
+
+ ranging_user_pkt.session_id = (uint16_t)uwb_app_config.session_id;
+}
+
+void ranging_start(void)
+{
+ ranging_env.enable = 1;
+ ranging_env.ranging_period = MS_TO_PHY_TIMER_COUNT(uwb_app_config.session_param.ranging_interval);
+ ranging_env.range_data.mac_addr_mode = uwbs_mac_addr_mode_get();
+
+ uint16_t tmp_delay = uwb_app_config.session_param.ranging_anchor_resp_delay;
+ if (tmp_delay & 0x8000)
+ ranging_env.anchor_resp_dly = MS_TO_PHY_TIMER_COUNT(tmp_delay & ~0x8000);
+ else
+ ranging_env.anchor_resp_dly = US_TO_PHY_TIMER_COUNT(tmp_delay);
+
+ tmp_delay = uwb_app_config.session_param.ranging_tag_resp_delay;
+ if (tmp_delay & 0x8000)
+ ranging_env.tag_resp_dly = MS_TO_PHY_TIMER_COUNT(tmp_delay & ~0x8000);
+ else
+ ranging_env.tag_resp_dly = US_TO_PHY_TIMER_COUNT(tmp_delay);
+
+ ranging_env.base_point = phy_timer_count_get() + ranging_env.ranging_period;
+ ranging_env.anchor_point = ranging_env.base_point;
+
+ enum DEV_ROLE_T dev_role = uwb_app_config.session_param.device_role;
+ uwb_app_config.ranging_stage = (RANGING_SYNC);
+ phy_timer_target_set(ranging_env.anchor_point - 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;
+
+ // uint8_t locak_long_addr[8] = {0x39,0xcd,0x91,0x4d,};
+ // uint8_t locak_long_addr[8] = {0xa5,0xc6,0x91,0xcd,};
+ // uwbs_local_long_addr_set(locak_long_addr);
+
+ // WsfTimerStartMs(&ranging_cb.daemon_timer, TAG_BLINK_PERIOD_MS * 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++;
+ 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_num_get(void)
+{
+ return ranging_env.responder_num;
+}
+
+// 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;
+}
+
+// sync msg packet
+static uint8_t ranging_sync_pkt_construct(void *p)
+{
+ uint8_t tmp_idx = 0;
+ struct RANGING_SYNC_MSG_T *p_sync_msg = (struct RANGING_SYNC_MSG_T *)p;
+
+ p_sync_msg->frameCtrl[0] = 0xc5;
+ tmp_idx += 1;
+ p_sync_msg->seqNum = (uint8_t)ranging_user_pkt.seq_num;
+ tmp_idx += 1;
+ memcpy(p_sync_msg->tagID, uwbs_local_long_addr_get(), 8);
+ tmp_idx += 8;
+
+ return tmp_idx;
+}
+
+// cfg msg packet
+static uint8_t ranging_cfg_pkt_construct(void *p)
+{
+ uint8_t tmp_idx = 0;
+ struct RANGING_COMMON_MSG_T *p_cfg_msg = (struct RANGING_COMMON_MSG_T *)p;
+
+ p_cfg_msg->frameCtrl[tmp_idx++] = 0x41;
+ p_cfg_msg->frameCtrl[tmp_idx++] = 0xcc;
+ p_cfg_msg->seqNum = (uint8_t)ranging_user_pkt.seq_num;
+ tmp_idx += 1;
+ p_cfg_msg->panID[0] = 0xca;
+ p_cfg_msg->panID[1] = 0xde;
+ tmp_idx += 2;
+ memcpy(p_cfg_msg->destAddr, uwbs_peer_long_addr_get(), 8);
+ tmp_idx += 8;
+ memcpy(p_cfg_msg->sourceAddr, uwbs_local_long_addr_get(), 8);
+ tmp_idx += 8;
+ p_cfg_msg->fcode = 0x20;
+ tmp_idx += 1;
+#if 1
+ p_cfg_msg->user_data[0] = p_cfg_msg->destAddr[0];
+ tmp_idx += 1;
+ p_cfg_msg->user_data[1] = p_cfg_msg->sourceAddr[0];
+ tmp_idx += 1;
+ // ANCHOR_RESP_DLY, delayRx_us + poll_us, coded in us with bit 15 == 0, coded in ms when bit 15 == 1
+ uint16_t tmp_delay = uwb_app_config.session_param.ranging_anchor_resp_delay;
+ p_cfg_msg->user_data[2] = tmp_delay & 0xFF;
+ tmp_idx += 1;
+ p_cfg_msg->user_data[3] = (tmp_delay >> 8) & 0xFF;
+ tmp_idx += 1;
+ // TAG_RESP_DLY, coded in us with bit 15 == 0, coded in ms when bit 15 == 1
+ tmp_delay = uwb_app_config.session_param.ranging_tag_resp_delay;
+ p_cfg_msg->user_data[4] = tmp_delay & 0xFF;
+ tmp_idx += 1;
+ p_cfg_msg->user_data[5] = (tmp_delay >> 8) & 0xFF;
+ tmp_idx += 1;
+#else
+ tmp_idx += 6;
+#endif
+
+ return tmp_idx;
+}
+
+static uint8_t ranging_poll_pkt_construct(void *p)
+{
+ // poll msg packet
+ uint8_t tmp_idx = 0;
+ struct RANGING_COMMON_MSG_T *p_poll_msg = (struct RANGING_COMMON_MSG_T *)p;
+
+ p_poll_msg->frameCtrl[tmp_idx++] = 0x41;
+ p_poll_msg->frameCtrl[tmp_idx++] = 0xcc;
+ p_poll_msg->seqNum = (uint8_t)ranging_user_pkt.seq_num;
+ tmp_idx += 1;
+ p_poll_msg->panID[0] = 0xca;
+ p_poll_msg->panID[1] = 0xde;
+ tmp_idx += 2;
+ memcpy(p_poll_msg->destAddr, uwbs_peer_long_addr_get(), 8);
+ tmp_idx += 8;
+ memcpy(p_poll_msg->sourceAddr, uwbs_local_long_addr_get(), 8);
+ tmp_idx += 8;
+ p_poll_msg->fcode = 0x21;
+ tmp_idx += 1;
+ memset(p_poll_msg->user_data, 0, sizeof(p_poll_msg->user_data));
+ tmp_idx += 4;
+
+ return tmp_idx;
+}
+
+static uint8_t ranging_response_pkt_construct(void *p)
+{
+ // send response
+ // response msg packet
+ uint8_t tmp_idx = 0;
+ struct RANGING_COMMON_MSG_T *p_resp_msg = (struct RANGING_COMMON_MSG_T *)p;
+
+ p_resp_msg->frameCtrl[tmp_idx++] = 0x41;
+ p_resp_msg->frameCtrl[tmp_idx++] = 0xcc;
+ p_resp_msg->seqNum = (uint8_t)ranging_user_pkt.seq_num;
+ tmp_idx += 1;
+ p_resp_msg->panID[0] = 0xca;
+ p_resp_msg->panID[1] = 0xde;
+ tmp_idx += 2;
+ memcpy(p_resp_msg->destAddr, uwbs_peer_long_addr_get(), 8);
+ tmp_idx += 8;
+ memcpy(p_resp_msg->sourceAddr, uwbs_local_long_addr_get(), 8);
+ tmp_idx += 8;
+ p_resp_msg->fcode = 0x10;
+ tmp_idx += 1;
+ memset(p_resp_msg->user_data, 0, sizeof(p_resp_msg->user_data));
+ p_resp_msg->user_data[0] = 0x02; // 0x02 ranging continue
+ tmp_idx += 1;
+#if 0
+
+#else
+ tmp_idx += 7;
+#endif
+
+ return tmp_idx;
+}
+
+static uint8_t ranging_final_pkt_construct(void *p)
+{
+ // send response
+ // response msg packet
+ uint8_t tmp_idx = 0;
+ struct RANGING_COMMON_MSG_T *p_final_msg = (struct RANGING_COMMON_MSG_T *)p;
+
+ p_final_msg->frameCtrl[tmp_idx++] = 0x41;
+ p_final_msg->frameCtrl[tmp_idx++] = 0xcc;
+ p_final_msg->seqNum = (uint8_t)ranging_user_pkt.seq_num;
+ tmp_idx += 1;
+ p_final_msg->panID[0] = 0xca;
+ p_final_msg->panID[1] = 0xde;
+ tmp_idx += 2;
+ memcpy(p_final_msg->destAddr, uwbs_peer_long_addr_get(), 8);
+ tmp_idx += 8;
+ memcpy(p_final_msg->sourceAddr, uwbs_local_long_addr_get(), 8);
+ tmp_idx += 8;
+ p_final_msg->fcode = 0x29;
+ tmp_idx += 1;
+ memset(p_final_msg->user_data, 0, sizeof(p_final_msg->user_data));
+ // timestamp
+ //
+ p_final_msg->user_data[0] = (ranging_env.tx_poll_time >> 0) & 0xFF;
+ p_final_msg->user_data[1] = (ranging_env.tx_poll_time >> 8) & 0xFF;
+ p_final_msg->user_data[2] = (ranging_env.tx_poll_time >> 16) & 0xFF;
+ p_final_msg->user_data[3] = (ranging_env.tx_poll_time >> 24) & 0xFF;
+ p_final_msg->user_data[4] = (ranging_env.tx_poll_time >> 32) & 0xFF;
+ tmp_idx += 5;
+ //
+ p_final_msg->user_data[5] = (ranging_env.rx_response_time[0] >> 0) & 0xFF;
+ p_final_msg->user_data[6] = (ranging_env.rx_response_time[0] >> 8) & 0xFF;
+ p_final_msg->user_data[7] = (ranging_env.rx_response_time[0] >> 16) & 0xFF;
+ p_final_msg->user_data[8] = (ranging_env.rx_response_time[0] >> 24) & 0xFF;
+ p_final_msg->user_data[9] = (ranging_env.rx_response_time[0] >> 32) & 0xFF;
+ tmp_idx += 5;
+ //
+ p_final_msg->user_data[10] = (ranging_env.tx_final_time >> 0) & 0xFF;
+ p_final_msg->user_data[11] = (ranging_env.tx_final_time >> 8) & 0xFF;
+ p_final_msg->user_data[12] = (ranging_env.tx_final_time >> 16) & 0xFF;
+ p_final_msg->user_data[13] = (ranging_env.tx_final_time >> 24) & 0xFF;
+ p_final_msg->user_data[14] = (ranging_env.tx_final_time >> 32) & 0xFF;
+ tmp_idx += 5;
+
+ return tmp_idx;
+}
+
+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];
+ 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;
+ 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_SYNC:
+ // send
+ power_on_radio(1, 0);
+ mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, ranging_env.anchor_point, 0, (uint8_t *)&ranging_common_msg,
+ ranging_sync_pkt_construct((void *)&ranging_common_msg));
+ break;
+
+ case RANGING_CFG:
+ power_on_radio(0, 1);
+ mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, UWB_RX_WINDOW);
+ break;
+
+ case RANGING_POLL:
+ ranging_user_pkt.seq_num++;
+ ranging_env.tx_poll_time = ranging_tx_time(ranging_env.anchor_point + phy_shr_duration());
+ // send
+ power_on_radio(1, 0);
+ mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, ranging_env.anchor_point, 0, (uint8_t *)&ranging_common_msg,
+ ranging_poll_pkt_construct((void *)&ranging_common_msg));
+ break;
+
+ case RANGING_RESPONSE:
+ // receive response
+ power_on_radio(0, 1);
+ mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, UWB_RX_WINDOW);
+ break;
+
+ case RANGING_FINAL:
+ ranging_env.tx_final_time = ranging_tx_time(ranging_env.anchor_point + phy_shr_duration());
+ // send final
+ power_on_radio(1, 0);
+ mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, ranging_env.anchor_point, 0, (uint8_t *)&ranging_common_msg,
+ ranging_final_pkt_construct((void *)&ranging_common_msg));
+ break;
+
+ default:
+ ASSERT(0, "UNKNOWN STAGE\r\n");
+ break;
+ }
+ }
+ else
+ {
+ switch (stage)
+ {
+ case RANGING_IDLE:
+ ASSERT(0, "RANGING_IDLE\r\n");
+ break;
+
+ case RANGING_SYNC:
+ power_on_radio(0, 1);
+ mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, ranging_env.ranging_period);
+ break;
+
+ case RANGING_CFG:
+ ranging_user_pkt.seq_num++;
+ // send cfg
+ power_on_radio(1, 0);
+ mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, ranging_env.anchor_point, 0, (uint8_t *)&ranging_common_msg,
+ ranging_cfg_pkt_construct((void *)&ranging_common_msg));
+ break;
+
+ case RANGING_POLL:
+ ranging_user_pkt.seq_num++;
+ //
+ power_on_radio(0, 1);
+ mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, UWB_RX_WINDOW);
+ break;
+
+ case RANGING_RESPONSE:
+ ranging_env.tx_response_time = ranging_tx_time(ranging_env.anchor_point + phy_shr_duration());
+ //
+ power_on_radio(1, 0);
+ mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, ranging_env.anchor_point, 0, (uint8_t *)&ranging_common_msg,
+ ranging_response_pkt_construct((void *)&ranging_common_msg));
+ break;
+
+ case RANGING_FINAL:
+ power_on_radio(0, 1);
+ mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, UWB_RX_WINDOW);
+ break;
+
+ 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)
+{
+ enum RANGING_STAGE_T stage = uwb_app_config.ranging_stage;
+
+ // board_led_on(BOARD_LED_1);
+ power_off_radio();
+ if (ranging_env.enable)
+ {
+ ranging_env.count++;
+
+ if (uwb_app_config.session_param.device_role == DEV_ROLE_INITIATOR)
+ {
+ switch (stage)
+ {
+ case RANGING_IDLE:
+ break;
+
+ case RANGING_SYNC:
+ {
+ uwb_pkt_tx_done_ind(ind, stage);
+ // rx cfg set
+ ranging_env.base_point = ind->timestamp - phy_shr_duration();
+ ranging_env.anchor_point = ranging_env.base_point + ranging_env.anchor_resp_dly;
+ uwb_app_config.ranging_stage = (RANGING_CFG);
+ }
+ break;
+
+ case RANGING_CFG:
+ {
+ uwb_pkt_rx_done_ind(ind, stage);
+
+ struct RANGING_COMMON_MSG_T *p_common_msg = (struct RANGING_COMMON_MSG_T *)&ind->pkt_data[0];
+ // recv cfg msg ?
+ if ((ind->err_code == UWB_RX_OK) && p_common_msg->frameCtrl[0] == 0x41 && p_common_msg->frameCtrl[1] == 0xCC && p_common_msg->fcode == 0x20)
+ {
+ // if (memcmp(p_common_msg->destAddr, uwbs_local_long_addr_get(), 8) == 0)
+ if (memcmp(p_common_msg->destAddr, uwbs_local_long_addr_get(), 8) == 0 &&
+ memcmp(p_common_msg->sourceAddr, uwbs_peer_long_addr_get(), 8) == 0)
+ {
+ uwb_pkt_rx_done_ind(ind, stage);
+ uwbs_peer_long_addr_set(p_common_msg->sourceAddr);
+ //
+ uint16_t tmp_anchor_resp_time = (uint16_t)READ_SHORT((uint8_t *)&p_common_msg->user_data[2]);
+ uwb_app_config.session_param.ranging_anchor_resp_delay = (tmp_anchor_resp_time);
+ if (tmp_anchor_resp_time & 0x8000)
+ ranging_env.anchor_resp_dly = MS_TO_PHY_TIMER_COUNT(tmp_anchor_resp_time & ~0x8000);
+ else
+ ranging_env.anchor_resp_dly = US_TO_PHY_TIMER_COUNT(tmp_anchor_resp_time);
+ //
+ uint16_t tmp_tag_resp_time = (uint16_t)READ_SHORT((uint8_t *)&p_common_msg->user_data[4]);
+ uwb_app_config.session_param.ranging_tag_resp_delay = (tmp_tag_resp_time);
+ if (tmp_tag_resp_time & 0x8000)
+ ranging_env.tag_resp_dly = MS_TO_PHY_TIMER_COUNT(tmp_tag_resp_time & ~0x8000);
+ else
+ ranging_env.tag_resp_dly = US_TO_PHY_TIMER_COUNT(tmp_tag_resp_time);
+
+ uwb_app_config.ranging_stage = (RANGING_POLL);
+ }
+ }
+
+ if (uwb_app_config.ranging_stage == RANGING_POLL)
+ {
+ // tx poll set
+ ranging_env.base_point += ranging_env.ranging_period;
+ ranging_env.anchor_point = ranging_env.base_point;
+ }
+ else
+ {
+ // tx cfg again set
+ uwb_app_config.ranging_stage = (RANGING_SYNC);
+ ranging_env.base_point += ranging_env.ranging_period;
+ ranging_env.anchor_point = ranging_env.base_point;
+ }
+ }
+ break;
+
+ case RANGING_POLL:
+ {
+ // process rx-poll packet
+ uwb_pkt_tx_done_ind(ind, stage);
+ // rx resp set
+ ranging_env.base_point = ind->timestamp - phy_shr_duration();
+ ranging_env.anchor_point = ranging_env.base_point + ranging_env.anchor_resp_dly;
+ uwb_app_config.ranging_stage = (RANGING_RESPONSE);
+ }
+ break;
+
+ case RANGING_RESPONSE:
+ {
+ // process rx-response packet
+ uwb_pkt_rx_done_ind(ind, stage);
+
+ struct RANGING_COMMON_MSG_T *p_common_msg = (struct RANGING_COMMON_MSG_T *)&ind->pkt_data[0];
+ if ((ind->err_code == UWB_RX_OK) && p_common_msg->frameCtrl[0] == 0x41 && p_common_msg->frameCtrl[1] == 0xCC && p_common_msg->fcode == 0x10)
+ {
+ if (memcmp(p_common_msg->destAddr, uwbs_local_long_addr_get(), 8) == 0 &&
+ memcmp(p_common_msg->sourceAddr, uwbs_peer_long_addr_get(), 8) == 0)
+ {
+ if (p_common_msg->user_data[0] == 0x02)
+ {
+ uwb_app_config.ranging_stage = (RANGING_FINAL);
+ ranging_env.rx_response_time[0] = ranging_rx_time(ind);
+ }
+ else
+ ranging_env.enable = 0;
+ }
+ }
+
+ if (uwb_app_config.ranging_stage == RANGING_FINAL)
+ {
+ ranging_env.anchor_point += ranging_env.tag_resp_dly;
+ }
+ else
+ {
+ ranging_env.base_point += ranging_env.ranging_period;
+ ranging_env.anchor_point = ranging_env.base_point;
+ uwb_app_config.ranging_stage = (RANGING_POLL);
+ }
+ }
+ break;
+
+ case RANGING_FINAL:
+ {
+ uwb_pkt_tx_done_ind(ind, stage);
+ //
+ ranging_env.base_point += ranging_env.ranging_period;
+ ranging_env.anchor_point = ranging_env.base_point;
+ uwb_app_config.ranging_stage = (RANGING_POLL);
+ }
+ break;
+
+ default:
+ break;
+ }
+ }
+ else
+ {
+ switch (stage)
+ {
+ case RANGING_IDLE:
+ break;
+ case RANGING_SYNC:
+ {
+ struct RANGING_SYNC_MSG_T *p_sync_msg = (struct RANGING_SYNC_MSG_T *)&ind->pkt_data[0];
+ struct RANGING_COMMON_MSG_T *p_common_msg = (struct RANGING_COMMON_MSG_T *)&ind->pkt_data[0];
+
+ if ((ind->err_code == UWB_RX_OK) && p_sync_msg->frameCtrl[0] == 0xC5)
+ { // recv sync
+ if (memcmp(p_sync_msg->tagID, uwbs_peer_long_addr_get(), 8) == 0)
+ {
+ uwbs_peer_long_addr_set(p_sync_msg->tagID);
+ uwb_app_config.ranging_stage = (RANGING_CFG);
+ }
+ }
+ else if ((ind->err_code == UWB_RX_OK) && p_common_msg->frameCtrl[0] == 0x41 && p_common_msg->frameCtrl[1] == 0xCC &&
+ p_common_msg->fcode == 0x21)
+ { // recv poll
+ if (memcmp(p_common_msg->destAddr, uwbs_local_long_addr_get(), 8) == 0 &&
+ memcmp(p_common_msg->sourceAddr, uwbs_peer_long_addr_get(), 8) == 0)
+ {
+ ranging_env.rx_poll_time = ranging_rx_time(ind);
+ // uwb_app_config.ranging_stage = (RANGING_POLL);
+ uwb_app_config.ranging_stage = (RANGING_RESPONSE);
+ }
+ }
+
+ if (uwb_app_config.ranging_stage == RANGING_CFG)
+ {
+ uwb_pkt_rx_done_ind(ind, stage);
+ ranging_env.base_point = ind->timestamp - phy_shr_duration();
+ ranging_env.anchor_point = ranging_env.base_point + ranging_env.anchor_resp_dly;
+ }
+ else if (uwb_app_config.ranging_stage == RANGING_RESPONSE)
+ {
+ uwb_pkt_rx_done_ind(ind, RANGING_POLL);
+ ranging_env.base_point = ind->timestamp - phy_shr_duration();
+ ranging_env.anchor_point = ranging_env.base_point + ranging_env.anchor_resp_dly;
+ }
+ else
+ {
+ uwb_pkt_rx_done_ind(ind, stage);
+ // receive sync again
+ power_on_radio(0, 1);
+ mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, ranging_env.ranging_period);
+ mac_start();
+
+ return stage;
+ }
+ }
+ break;
+
+ case RANGING_CFG:
+ {
+ uwb_pkt_tx_done_ind(ind, stage);
+ uwb_app_config.ranging_stage = (RANGING_SYNC);
+ ranging_env.base_point += ranging_env.ranging_period;
+ ranging_env.anchor_point = ranging_env.base_point;
+ }
+ break;
+
+ case RANGING_POLL:
+ {
+ uwb_pkt_rx_done_ind(ind, stage);
+ ranging_env.responder_final_flag = 0;
+
+ struct RANGING_COMMON_MSG_T *p_common_msg = (struct RANGING_COMMON_MSG_T *)&ind->pkt_data[0];
+ if ((ind->err_code == UWB_RX_OK) && p_common_msg->frameCtrl[0] == 0x41 && p_common_msg->frameCtrl[1] == 0xCC && p_common_msg->fcode == 0x21)
+ {
+ if (memcmp(p_common_msg->destAddr, uwbs_local_long_addr_get(), 8) == 0 &&
+ memcmp(p_common_msg->sourceAddr, uwbs_peer_long_addr_get(), 8) == 0)
+ {
+ ranging_env.rx_poll_time = ranging_rx_time(ind);
+ uwb_app_config.ranging_stage = (RANGING_RESPONSE);
+ }
+ }
+
+ if (uwb_app_config.ranging_stage == RANGING_RESPONSE)
+ {
+ ranging_env.base_point = ind->timestamp - phy_shr_duration();
+ ranging_env.anchor_point = ranging_env.base_point + ranging_env.anchor_resp_dly;
+ }
+ else
+ {
+ // receive poll
+ power_on_radio(0, 1);
+ mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, UWB_RX_WINDOW);
+ mac_start();
+
+ return stage;
+ }
+ }
+ break;
+
+ case RANGING_RESPONSE:
+ {
+ uwb_pkt_tx_done_ind(ind, stage);
+ ranging_env.anchor_point += ranging_env.tag_resp_dly;
+ uwb_app_config.ranging_stage = (RANGING_FINAL);
+ }
+ break;
+
+ case RANGING_FINAL:
+ {
+ // process rx-final packet
+ uwb_pkt_rx_done_ind(ind, stage);
+
+ struct RANGING_COMMON_MSG_T *p_common_msg = (struct RANGING_COMMON_MSG_T *)&ind->pkt_data[0];
+ if ((ind->err_code == UWB_RX_OK) && p_common_msg->frameCtrl[0] == 0x41 && p_common_msg->frameCtrl[1] == 0xCC && p_common_msg->fcode == 0x29)
+ {
+ if (memcmp(p_common_msg->destAddr, uwbs_local_long_addr_get(), 8) == 0 &&
+ memcmp(p_common_msg->sourceAddr, uwbs_peer_long_addr_get(), 8) == 0)
+ {
+ ranging_env.rx_final_time = ranging_rx_time(ind);
+ ranging_env.responder_final_flag = 1;
+ }
+ }
+
+ uwb_app_config.ranging_stage = (RANGING_POLL);
+ ranging_env.base_point += ranging_env.ranging_period;
+ ranging_env.anchor_point = ranging_env.base_point;
+ }
+ break;
+
+ 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(ranging_env.anchor_point - 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_1);
+
+ 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 void 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;
+
+ ind->rx_len = rx->pkt_len;
+
+ if ((rx->pkt_len) && (rx->pkt_data != NULL))
+ {
+ memcpy(ind->rx_data, rx->pkt_data, rx->pkt_len);
+ }
+
+ // Send the message
+ WsfMsgSend(ranging_cb.handle_id, ind);
+ }
+ else
+ {
+ LOG_WARNING(TRACE_MODULE_UWB, "memory is not enough for UWB_PKT_RX_DONE_IND_T %d\r\n", rx->pkt_len);
+ }
+}
+#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);
+}
--
Gitblit v1.9.3