From f9199bf56082288137ac7620f6dc1020417a1bd1 Mon Sep 17 00:00:00 2001
From: WXK <287788329@qq.com>
Date: 星期三, 09 十月 2024 14:08:15 +0800
Subject: [PATCH] 一对一免布线方案纯uwb调通，先提交，接下来lora uwb联调
---
keil/simple_ss_twr_dw_init.c | 439 ++++++++++++++++++++++++++++++++++--------------------
1 files changed, 279 insertions(+), 160 deletions(-)
diff --git a/keil/simple_ss_twr_dw_init.c b/keil/simple_ss_twr_dw_init.c
index 945d295..39f0a9a 100644
--- a/keil/simple_ss_twr_dw_init.c
+++ b/keil/simple_ss_twr_dw_init.c
@@ -7,27 +7,54 @@
#include "lib_ranging.h"
#include "dw_app.h"
#include "board.h"
+#include "mk_calib.h"
#if defined(MK_SS_TWR_DW_INIT)
+//#define UWB_100K_MODE
-extern int simple_main(void);
+
+#ifndef UWB_100K_MODE
/* Ranging period: 1s */
#define RANGING_PERIOD_MS (1000)
/* This is the delay from the end of the poll frame transmission to the enable of the receiver */
-#define POLL_TX_TO_RESP_RX_DLY_US 700U
+#define POLL_TX_TO_RESP_RX_DLY_US 750U //yuan700
#define RESP_RX_TO_FINAL_TX_DLY_US 550U
+/* Poll delay: 1s */
+#define POLL_DELAY 100U //yuan100U
+
/* Receive response timeout */
-#define RESP_RX_TIMEOUT_US 500U
+#define RESP_RX_TIMEOUT_US 2000U //Yuan500 10mssuccess 300jixian
+
+#else
+/* Ranging period: 1s */
+#define RANGING_PERIOD_MS (1000)
+
+/* This is the delay from the end of the poll frame transmission to the enable of the receiver */
+#define POLL_TX_TO_RESP_RX_DLY_US 750U //yuan700
+
+#define RESP_RX_TO_FINAL_TX_DLY_US 550U
+
+/* Poll delay: 1s */
+#define POLL_DELAY 100U //yuan100U
+
+/* Receive response timeout */
+#define RESP_RX_TIMEOUT_US 5000U //Yuan500 10mssuccess 300jixian
+
+#endif
/* Field index in frame */
#define MSG_SEQ_NUM_IDX 2
#define FINAL_MSG_POLL_TX_TS_IDX 10
#define FINAL_MSG_RESP_RX_TS_IDX 14
#define FINAL_MSG_FINAL_TX_TS_IDX 18
+#define DELAY_DEFAULT 1000
+#define DELAY_BETWEEN_TWO_FRAME_UUS 400
+#define HALF_SECOND_TIME 62400000
+
uint16_t CmpTagInList(uint16_t tagid);
uint16_t AddNewTagIntoList(uint16_t tagid);
void AnchorListUpdate(void);
@@ -39,8 +66,9 @@
};
extern uint32_t dev_id;
extern uint8_t group_id;
+#ifndef UWB_100K_MODE
/* Default communication configuration. */
-static struct mk_uwb_configure config = {
+static struct mk_uwb_configure config = {//yuan
.phy_work_mode = (uint8_t)(PHY_TX | PHY_RX),
.phy_cfg.ch_num = 5, /* Channel number. */
.phy_cfg.code_index = 9, /* TRX preamble code */
@@ -53,11 +81,28 @@
.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_ANTENNA_3, /* UWB RX antenna port */
+ .phy_cfg.rx_ant_id = UWB_RX_ANT_3, /* UWB RX antenna port */
};
-
+#else
+/* Default communication configuration. */
+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 = 3, /* TX preamble code. */
+ .phy_cfg.mean_prf = MEAN_PRF_16M, /* Data rate 6.8M */
+ .phy_cfg.data_bit_rate = DATA_BR_110K, /* data rate 6.8M. */
+ .phy_cfg.sync_sym = PREAM_LEN_1024, /* Preamble duration, length of preamble 128 */
+ .phy_cfg.sfd_sym = NSFD_64, /* Identifier for SFD sequence */
+ .phy_cfg.ranging_bit = 1, /* ranging bit set. */
+ .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 */
+};
+#endif
/* Buffer to store received frame */
-static uint8_t rx_buf[128];
+static uint8_t rx_buf[150];
static uint8_t uwb_sendbuffer[150];
/* Frames used in the ranging process
* Poll message:
@@ -92,13 +137,33 @@
static uint8_t frame_seq_nb = 0;
static volatile uint16_t rx_state;
+static volatile uint16_t rx_state1;
static volatile uint16_t rx_length;
//DW
-uint16_t rec_nearbaseid,temp_index;
+uint16_t rec_nearbaseid;
uint8_t temp_tag_num;
int32_t test2;
static uint8_t receive_flag=0;
static uint8_t rec_tag_index,tag_num_tosend;
+
+uint32_t start_receive_count,end_receive_count,poll_timeout,current_count,temp_resp;
+uint16_t taglist_total_num,taglist_current_index; //当前列表总数量
+uint8_t taglist_keeptime[MAX_TAG_LIST_NUM]; //每个标签存活时间
+uint16_t taglist_id[MAX_TAG_LIST_NUM],taglist_dist[MAX_TAG_LIST_NUM]; //标签距离和标签的ID
+
+int32_t mainbase_dist,nearbase_distlist[MAX_NEARBASE_NUM],true_nearbase_distlist[MAX_NEARBASE_NUM],true_exsistbase_list[MAX_NEARBASE_NUM],ancsync_time,nextpoll_delaytime,offsettimeus;
+uint8_t anclost_times=0 , mainbase_lost_count=0,exsistbase_list[MAX_NEARBASE_NUM],get_newbase=0,nearbase_num,last_nearbase_num,next_nearbase_num;
+uint16_t nearbaseid_list[MAX_NEARBASE_NUM],mainbase_id,true_nearbase_idlist[MAX_NEARBASE_NUM],rec_nearbaseid,rec_nearbasepos;
+uint16_t u16_nearbase_distlist[MAX_NEARBASE_NUM];
+uint32_t temp_count1=0;
+uint32_t temp_count2=0;
+uint32_t temp_count3=0;
+uint32_t temp_count4=0;
+uint32_t tempflag=0;
+int32_t freq_offset,freq_offset_filter;
+int temp_flag,poll_tx_num,resp_rx_num,recv_error_num;
+extern double distance;
+uint32_t count1,count2;
enum SIMPLE_FSM_T
{
SIMPLE_IDLE = 0,
@@ -108,7 +173,19 @@
};
void uwb_poll_buffer_construct(void)
{
-
+ for(uint8_t i=0;i<nearbase_num;i++) //标签距离32位转成16位
+{
+ u16_nearbase_distlist[i] = nearbase_distlist[i];
+}
+ uwb_sendbuffer[GROUP_ID_IDX] = group_id;
+ memcpy(&uwb_sendbuffer[TAG_ID_IDX],&dev_id,2);
+ //uwb_sendbuffer[BATTARY_IDX] = bat_percent;
+ uwb_sendbuffer[SEQUENCE_IDX] = frame_seq_nb++;
+ uwb_sendbuffer[NEARBASENUM_INDEX] = nearbase_num;
+ memcpy(&uwb_sendbuffer[NEARBASEID_INDEX],&nearbaseid_list,nearbase_num*2);//基站id
+ memcpy(&uwb_sendbuffer[NEARBASEID_INDEX+nearbase_num*2],&u16_nearbase_distlist,nearbase_num*2);
+ uwb_sendbuffer[MESSAGE_TYPE_IDX] = MBX_POLL;
+ memcpy(&uwb_sendbuffer[ANCHOR_ID_IDX],&mainbase_id,2);
}
static enum SIMPLE_FSM_T state = SIMPLE_IDLE;
@@ -161,7 +238,16 @@
memcpy(rx_buf, rx_report->pkt_data, rx_length);
/* Calculate rx timestamp */
resp_rx_ts_i64 = ranging_rx_time_correct(rx_report);
- receive_flag=1;
+ temp_count1=phy_timer_count_get();
+ //获取发射端时钟偏差
+ resp_rx_num++;
+ freq_offset=phy_freq_offset_get();
+ freq_offset_filter=average_filter(freq_offset);//获取频偏
+ int32_t ppm = freq_offset_filter / (int32_t)(ch_center_freq_map[UWB_CH_NUM] * 1e-6);
+ calib_xtal38m4_load_cap_auto_tune(ppm);//利用电容调整晶振适配频偏应在完整的一包之后调整，需要关闭XTAL_AUTO_TUNE_EN 宏定义避免收包中途校准导致测距错误
+
+ receive_flag=1;
+
}
else
{
@@ -171,10 +257,15 @@
/* UWB_BD_ERR Preamble detection error */
/* UWB_TO_ERR Receive timeout */
/* UWB_STS_ERR STS error */
+
+ rx_state1=rx_report->err_code;
rx_length = 0;
- receive_flag=2;
+ receive_flag=2;
+ temp_count2=phy_timer_count_get();
+ recv_error_num++;
+
}
-
+
}
/* TX done process handler. */
@@ -186,118 +277,120 @@
/** UWB TX success */
if (tx_report->err_code == UWB_TX_OK)
{
+ poll_tx_num++;
+ temp_count4=phy_timer_count_get();//测试
+ if(temp_flag){
+ count2=phy_timer_count_get();
+ temp_flag=0;
+ return;
+ }
+ count1=temp_count1;
+ temp_flag=1;
}
}
-uint32_t start_receive_count,end_receive_count,poll_timeout,current_count;
-uint16_t taglist_total_num,taglist_current_index; //当前列表总数量
-uint8_t taglist_keeptime[MAX_TAG_LIST_NUM]; //每个标签存活时间
-uint16_t taglist_id[MAX_TAG_LIST_NUM],taglist_dist[MAX_TAG_LIST_NUM]; //标签距离和标签的ID
-uint8_t rec_tag_num,temp_tag_num,kp1;
-int32_t mainbase_dist,nearbase_distlist[MAX_NEARBASE_NUM],true_nearbase_distlist[MAX_NEARBASE_NUM],true_exsistbase_list[MAX_NEARBASE_NUM],ancsync_time,nextpoll_delaytime,offsettimeus;
-uint8_t anclost_times=0 , mainbase_lost_count=0,exsistbase_list[MAX_NEARBASE_NUM],get_newbase=0,nearbase_num,last_nearbase_num,next_nearbase_num;
-uint16_t nearbaseid_list[MAX_NEARBASE_NUM],mainbase_id,true_nearbase_idlist[MAX_NEARBASE_NUM],rec_nearbaseid,rec_nearbasepos;
//注意：因为新注册的标签并不知道基站已经收到了，所以要在下一包立刻将新的标签ID发出，否则标签会一直抢占基站的poll包。
//方法是将新注册的标签添加到下次发送的标签列表头部，其他标签依次向后挪动。
-uint16_t AddNewTagIntoList(uint16_t tagid)
-{
- uint16_t newindex_cal = taglist_current_index+25;//当前索引+25，为下次索引要发出的值的最大值，如果总数小于这个值，即可放到最后面。
- if(newindex_cal>=taglist_total_num) //如果当前索引是最后一包，那么就加到总索引的后面，如果不是就加到下一次起始索引位置。
- {
+uint16_t AddNewTagIntoList(uint16_t tagid)
+{
+ uint16_t newindex_cal = taglist_current_index+25;//当前索引+25，为下次索引要发出的值的最大值，如果总数小于这个值，即可放到最后面。
+ if(newindex_cal>=taglist_total_num) //如果当前索引是最后一包，那么就加到总索引的后面，如果不是就加到下一次起始索引位置。
+ {
newindex_cal = taglist_total_num;
-
- } else {
- for(uint16_t i=taglist_total_num; i>newindex_cal; i--) //从后往前挪地方??就是插入
- {
- taglist_id[i]=taglist_id[i-1];
- taglist_dist[i]=taglist_dist[i-1];
- taglist_keeptime[i]=taglist_keeptime[i-1];
- }
- }
- taglist_total_num++;
- taglist_id[newindex_cal] = tagid;
- taglist_dist[newindex_cal] = 0;
- taglist_keeptime[newindex_cal] = TAG_KEEPTIMES;
- return newindex_cal;
+
+ }else{
+ for(uint16_t i=taglist_total_num;i>newindex_cal;i--) //从后往前挪地方??就是插入
+ {
+ taglist_id[i]=taglist_id[i-1];
+ taglist_dist[i]=taglist_dist[i-1];
+ taglist_keeptime[i]=taglist_keeptime[i-1];
+ }
+ }
+ taglist_total_num++;
+ taglist_id[newindex_cal] = tagid;
+ taglist_dist[newindex_cal] = 0;
+ taglist_keeptime[newindex_cal] = TAG_KEEPTIMES;
+ return newindex_cal;
}
uint8_t FindNearBasePos(uint16_t baseid)//寻找当前列表中的基站返回索引
{
- uint8_t i;
- for(i=0; i<nearbase_num; i++)
- {
- if(baseid==nearbaseid_list[i])
- return i;
- }
- if(i==nearbase_num)
- return nearbase_num;
+ uint8_t i;
+ for(i=0;i<nearbase_num;i++)
+ {
+ if(baseid==nearbaseid_list[i])
+ return i;
+ }
+ if(i==nearbase_num)
+ return nearbase_num;
}
void AnchorListUpdate(void)
{
- next_nearbase_num = 0;
- int j=0;
- for(int i=0; i<nearbase_num; i++)
- {
- if(exsistbase_list[i]>0)
- {
- exsistbase_list[i]--;
- next_nearbase_num++;
- true_exsistbase_list[j]=exsistbase_list[i];
- true_nearbase_idlist[j]=nearbaseid_list[i];
- true_nearbase_distlist[j++]=nearbase_distlist[i];
-
- }
- nearbase_num = next_nearbase_num;//更新现存基站数
- last_nearbase_num = next_nearbase_num;//更新当前基站数
- }
+ next_nearbase_num = 0;
+ int j=0;
+ for(int i=0;i<nearbase_num;i++)
+ {
+ if(exsistbase_list[i]>0)
+ {
+ exsistbase_list[i]--;
+ next_nearbase_num++;
+ true_exsistbase_list[j]=exsistbase_list[i];
+ true_nearbase_idlist[j]=nearbaseid_list[i];
+ true_nearbase_distlist[j++]=nearbase_distlist[i];
+
+ }
+ nearbase_num = next_nearbase_num;//更新现存基站数
+ last_nearbase_num = next_nearbase_num;//更新当前基站数
+ }
}
void Rank_ANchor_list_by_dis(void)//按照目前的基站距离进行冒泡排序用临时数组方式将最近的放到最前面，最远的放到最后面
{
- for(int i=0; i<last_nearbase_num-1; i++)
- {
- for(int j=0; j<last_nearbase_num-1; j++)
- {
- if(true_nearbase_distlist[j]>true_nearbase_distlist[j+1])
- {
- uint32_t temp_dist,temp_id,temp_exsis;
- temp_dist=true_nearbase_distlist[j];
- temp_id = true_nearbase_idlist[j];
- temp_exsis=true_exsistbase_list[j];
- true_nearbase_distlist[j]=true_nearbase_distlist[j+1];
- true_nearbase_idlist[j]=true_nearbase_idlist[j+1];
- true_exsistbase_list[j]=true_exsistbase_list[j+1];
-
- true_nearbase_distlist[j+1]=temp_dist;
- true_nearbase_idlist[j+1]=temp_id;
- true_exsistbase_list[j+1]=temp_exsis;
- }
- }
- }
- for (int i=0; i<last_nearbase_num; i++)
- {
- nearbaseid_list[i]=true_nearbase_idlist[i];
- nearbase_distlist[i]=true_nearbase_distlist[i];
- exsistbase_list[i] = true_exsistbase_list[i];
- }
+for(int i=0;i<last_nearbase_num-1;i++)
+ {
+ for(int j=0;j<last_nearbase_num-1;j++)
+ {
+ if(true_nearbase_distlist[j]>true_nearbase_distlist[j+1])
+ {
+ uint32_t temp_dist,temp_id,temp_exsis;
+ temp_dist=true_nearbase_distlist[j];
+ temp_id = true_nearbase_idlist[j];
+ temp_exsis=true_exsistbase_list[j];
+ true_nearbase_distlist[j]=true_nearbase_distlist[j+1];
+ true_nearbase_idlist[j]=true_nearbase_idlist[j+1];
+ true_exsistbase_list[j]=true_exsistbase_list[j+1];
+
+ true_nearbase_distlist[j+1]=temp_dist;
+ true_nearbase_idlist[j+1]=temp_id;
+ true_exsistbase_list[j+1]=temp_exsis;
+ }
+ }
+ }
+ for (int i=0;i<last_nearbase_num;i++)
+ {
+ nearbaseid_list[i]=true_nearbase_idlist[i];
+ nearbase_distlist[i]=true_nearbase_distlist[i];
+ exsistbase_list[i] = true_exsistbase_list[i];
+ }
}
uint16_t CmpTagInList(uint16_t tagid)//找这个新一包的基站在不在现存的taglist中在的话对应位置更新存活时间，不在就增加（确保有效数量小于最大设置数）
{
uint16_t temp;
- for(uint8_t i=0; i<taglist_total_num; i++)
- {
- if(memcmp(&tagid,&taglist_id[i],2)==0)
+ for(uint8_t i=0;i<taglist_total_num;i++)
+ {
+ if(memcmp(&tagid,&taglist_id[i],2)==0)
{
taglist_keeptime[i] = TAG_KEEPTIMES;
- return i;
+ return i;
}
- }
+ }
if(taglist_total_num<MAX_TAG_LIST_NUM)
- temp = AddNewTagIntoList(tagid);
- return temp;
+ temp = AddNewTagIntoList(tagid);
+ return temp;
}
+
int simple_main(void)
{
// The following peripherals will be initialized in the uwb_open function
@@ -308,11 +401,11 @@
uwb_calibration_params_set(config.phy_cfg.ch_num);
// set advanced parameters
- struct PHY_ADV_CONFIG_T adv_config = {
+ struct PHY_ADV_CONFIG_T adv_config = {
.thres_fap_detect = 40,
.nth_scale_factor = 4,
.ranging_performance_mode = 0,
- .dynamic_port_sel_en = 0,
+ .skip_weakest_port_en = 0,
};
phy_adv_params_configure(&adv_config);
@@ -330,72 +423,98 @@
// // Enable sleep timer
// sleep_timer_open(true, SLEEP_TIMER_MODE_ONESHOT, NULL);
//#endif
- uwb_poll_buffer_construct();
- temp_tag_num=0;//临时数量为0
- poll_tx_en_start_u32 = phy_timer_count_get();
- uwb_tx(uwb_sendbuffer,sizeof(uwb_sendbuffer),0,0);//立即发送
- poll_tx_ts_i64 = ranging_tx_time_correct(poll_tx_en_start_u32 + phy_start_time_gap());//修正时间戳
- while(mac_is_busy());//等待发送完成
-
- resp_rx_en_start_u32 = poll_tx_en_start_u32 + US_TO_PHY_TIMER_COUNT(POLL_TX_TO_RESP_RX_DLY_US);//设置发送多少s后接收按target独立时间起点算+ US_TO_PHY_TIMER_COUNT(POLL_TX_TO_RESP_RX_DLY_US为发送后要多久开启接收时间
- start_receive_count=phy_timer_count_get();
- poll_timeout=tag_num_tosend*50000+50000;//多一个多0.4ms默认0.4ms计算为0.125*4*100000
- end_receive_count=start_receive_count+poll_timeout;
- if(end_receive_count>=124800000)
- {
- end_receive_count-=124800000;
- }
- current_count=phy_timer_count_get();
- uwb_rx(1, resp_rx_en_start_u32, RESP_RX_TIMEOUT_US);//开启接受并设置接收超时
- while(current_count<end_receive_count||current_count>end_receive_count+62400000)//循环接受包体,若为124.8K则是+62400000
- {
- current_count=phy_timer_count_get();//不断刷新当前计数器值
- if(receive_flag==1)//成功接收数据
- {
- receive_flag=0;
- //获取发射端时钟偏差
- test2=phy_freq_offset_get();
- //接收成功则判断是否为同一组
- if (rx_buf[MESSAGE_TYPE_IDX] == MBX_RESPONSE&&!memcmp(&rx_buf[TAG_ID_IDX],&dev_id,2)&&rx_buf[GROUP_ID_IDX]==group_id) //判断接收到的数据是否是response数据
- {
- memcpy(&rec_nearbaseid,&rx_buf[ANCHOR_ID_IDX],2);
- if(last_nearbase_num==0)
- {
- nearbaseid_list[0]=rec_nearbaseid;
- nearbase_num=1;
- exsistbase_list[0]=KEEP_TIMES;
- SetANCTimestap(0,&rx_buf[RESP_MSG_POLL_RX_TS_IDX],&rx_buf[RESP_MSG_RESP_TX_TS_IDX],(uint32_t)resp_rx_ts_i64,&rx_buf[RESP_MSG_ANC_DISTOFFSET],test2);//这个是改过的参数
- }
+ uwb_poll_buffer_construct();
+ temp_tag_num=0;//临时数量为0
+ poll_tx_en_start_u32 = phy_timer_count_get()+US_TO_PHY_TIMER_COUNT(POLL_DELAY);//发送必须要延时发送才可以用于测距否则立即发送会获取时间戳不对，需要计算程序运行时间，避免设置过去时间
+ tempflag=uwb_tx(uwb_sendbuffer,13+4*nearbase_num,1,poll_tx_en_start_u32);//立即发送
+ #ifdef DEBUG_BOXING
+ gpio_pin_set(IO_PIN_5);//测试
+ #endif
+ //LOG_INFO(TRACE_MODULE_APP, "进入测距",poll_tx_num,resp_rx_num,distance);
+ poll_tx_ts_i64 = ranging_tx_time_correct(poll_tx_en_start_u32 + phy_shr_duration());//修正时间戳
+ temp_count1=phy_timer_count_get();
+ while(mac_is_busy());//等待发送完成
+ temp_count2=phy_timer_count_get();
+ resp_rx_en_start_u32 = poll_tx_en_start_u32+ US_TO_PHY_TIMER_COUNT(POLL_TX_TO_RESP_RX_DLY_US);//设置发送多少s后接收按target独立时间起点算+ US_TO_PHY_TIMER_COUNT(POLL_TX_TO_RESP_RX_DLY_US为发送后要多久开启接收时间
+ temp_resp=resp_rx_en_start_u32;
+ //tempflag=uwb_rx(1, resp_rx_en_start_u32, RESP_RX_TIMEOUT_US);//开启接受并设置接收超时
+ tempflag=uwb_rx(0,0, RESP_RX_TIMEOUT_US);
+
+
+ start_receive_count=phy_timer_count_get();
+ poll_timeout=nearbase_num*US_TO_PHY_TIMER_COUNT(DELAY_BETWEEN_TWO_FRAME_UUS)+US_TO_PHY_TIMER_COUNT(DELAY_DEFAULT);//多一个多0.4ms默认0.4ms计算为0.125*4*100000,默认开启1mss
+ end_receive_count=start_receive_count+poll_timeout;
+ if(end_receive_count>=UINT32_MAX)
+ {end_receive_count-=UINT32_MAX;}
+ current_count=phy_timer_count_get();
+ while(current_count<end_receive_count||current_count>end_receive_count+HALF_SECOND_TIME)//循环接受包体,若为124.8K则是+62400000
+ {
+
+ current_count=phy_timer_count_get();//不断刷新当前计数器值
+ temp_count2=phy_timer_count_get();
+ while(mac_is_busy());//等待接收完成
+
+ temp_count3=phy_timer_count_get();
+ if(receive_flag==1)//成功接收数据
+ {
+ receive_flag=0;
+
+ //接收成功则判断是否为同一组
+ if (rx_buf[MESSAGE_TYPE_IDX] == MBX_RESPONSE&&!memcmp(&rx_buf[TAG_ID_IDX],&dev_id,2)&&rx_buf[GROUP_ID_IDX]==group_id) //判断接收到的数据是否是response数据
+ {
+ memcpy(&rec_nearbaseid,&rx_buf[ANCHOR_ID_IDX],2);
+ if(last_nearbase_num==0)
+ {
+ nearbaseid_list[0]=rec_nearbaseid;
+ nearbase_num=1;
+ exsistbase_list[0]=KEEP_TIMES;
+ SetANCTimestap(0,&rx_buf[RESP_MSG_POLL_RX_TS_IDX],&rx_buf[RESP_MSG_RESP_TX_TS_IDX],(uint32_t)resp_rx_ts_i64,&rx_buf[RESP_MSG_ANC_DISTOFFSET],test2,(uint32_t)poll_tx_ts_i64);//这个是改过的参数
+ }
// if(rec_nearbaseid==nearbaseid_list[0])
// {
// exsistbase_list[0]=KEEP_TIMES;
// SetANCTimestap(0,&rx_buf[RESP_MSG_POLL_RX_TS_IDX],&rx_buf[RESP_MSG_RESP_TX_TS_IDX],(uint32_t)resp_rx_ts_i64,&rx_buf[RESP_MSG_ANC_DISTOFFSET],test2);//这个是改过的参数
// }
- else
- {
- rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
- SetANCTimestap(rec_nearbasepos,&rx_buf[RESP_MSG_POLL_RX_TS_IDX],&rx_buf[RESP_MSG_RESP_TX_TS_IDX],(uint32_t)resp_rx_ts_i64,&rx_buf[RESP_MSG_ANC_DISTOFFSET],test2);
- if(rec_nearbasepos>=last_nearbase_num) //发现新的基站
- {
- //get_newbase=1;
- nearbase_num++;
- nearbaseid_list[rec_nearbasepos] = rec_nearbaseid;
- }
- exsistbase_list[rec_nearbasepos]=KEEP_TIMES;//更新存活时间
- }
- memset(rx_buf, 0, sizeof(rx_buf));//清空接收到的用完的数组
- rx_length=0;//清0长度
- uwb_rx(0, 0, 0);//立即开启接受并设置0超时
- } else if(receive_flag==2) { //接收出错
- receive_flag=0;
- uwb_rx(0, 0, 0);//立即开启接受并设置0超时
- }
- }
- //关闭UWBapi
- uwb_close();
- CalculateDists(poll_tx_ts_i64); //计算距离
- AnchorListUpdate();//更新存活基站列表
- Rank_ANchor_list_by_dis();//冒泡排序重新按距离重新排列基站列表最小距离存货基站放到最前面
- }
+ else
+ {
+ rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
+ SetANCTimestap(rec_nearbasepos,&rx_buf[RESP_MSG_POLL_RX_TS_IDX],&rx_buf[RESP_MSG_RESP_TX_TS_IDX],(uint32_t)resp_rx_ts_i64,&rx_buf[RESP_MSG_ANC_DISTOFFSET],test2,(uint32_t)poll_tx_ts_i64);
+ if(rec_nearbasepos>=last_nearbase_num) //发现新的基站
+ {
+ //get_newbase=1;
+ nearbase_num++;
+ nearbaseid_list[rec_nearbasepos] = rec_nearbaseid;
+ }
+ exsistbase_list[rec_nearbasepos]=KEEP_TIMES;//更新存活时间
+ }
+
+ memset(rx_buf, 0, sizeof(rx_buf));//清空接收到的用完的数组
+ rx_length=0;//清0长度
+ break;
+// uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
+// while(mac_is_busy());
+ }
+ //gpio_pin_clr(IO_PIN_5);//测试
+// break;//成功接收就退出
+ }else if(receive_flag==2){//接收出错
+ receive_flag=0;
+ tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
+ temp_count3=phy_timer_count_get();
+// while(mac_is_busy());
+ temp_count1=phy_timer_count_get();
+ }
+
+ }
+ #ifdef DEBUG_BOXING
+ gpio_pin_clr(IO_PIN_5);//测试
+ #endif
+ //dwt_forecetrxoff();
+ CalculateDists(poll_tx_ts_i64); //计算距离并更新距离表
+ AnchorListUpdate();//更新存活基站列表
+ Rank_ANchor_list_by_dis();//冒泡排序重新按距离重新排列基站列表最小距离存货基站放到最前面
+ #ifndef DEBUG_BOXING
+ //LOG_INFO(TRACE_MODULE_APP, "poll_tx_num is %d,resp_rx_num is %d,,error_num is %d distance is %lf\r\n",poll_tx_num,resp_rx_num,recv_error_num,distance);
+ #endif
+
}
#endif
\ No newline at end of file
--
Gitblit v1.9.3