From e3cc01b77b6f240140d2536e365e559b42d1505c Mon Sep 17 00:00:00 2001
From: WXK <287788329@qq.com>
Date: 星期二, 22 七月 2025 15:27:30 +0800
Subject: [PATCH] V1.0 开发完成 5S一次，实际休眠时间要诚5 上位机设置除以5就行 休眠18ua 功耗
---
keil/uwb_tag.c | 951 ++++++++++++++++++++++++++++++++++++++++++++++------------
1 files changed, 747 insertions(+), 204 deletions(-)
diff --git a/keil/uwb_tag.c b/keil/uwb_tag.c
index 3f1ed2c..b32f674 100644
--- a/keil/uwb_tag.c
+++ b/keil/uwb_tag.c
@@ -8,7 +8,8 @@
#include "dw_tag.h"
#include "board.h"
#include "mk_calib.h"
-
+#include <serial_at_cmd_app.h>
+#include <global_param.h>
extern int TagRange(void);
extern Operation_step UWB_work_state;
@@ -25,7 +26,7 @@
#define POLL_DELAY 100U //yuan100U
/* Receive response timeout */
-#define RESP_RX_TIMEOUT_US 2000U //Yuan500 10mssuccess 300jixian
+#define RESP_RX_TIMEOUT_US 10000 //Yuan500 10mssuccess 300jixian
/* Field index in frame */
#define MSG_SEQ_NUM_IDX 2
@@ -36,6 +37,18 @@
#define DELAY_BETWEEN_TWO_FRAME_UUS 600 //yuan1400
#define HALF_SECOND_TIME 62400000
+#define STARTPOLL DISCPOLL//#define SWITCHBASE_DIST
+#define SWITCHBASE_ZHUANDIAN
+#define SWITCHBASE_DIST
+enum enumtagstate
+{
+
+ DISCPOLL,
+ REGPOLL,
+ GETNEARMSG,
+ NEARPOLL,
+ SINGLEPOLL,
+} tag_state=STARTPOLL;
static uint16_t CmpTagInList(uint16_t tagid);
uint16_t AddNewTagIntoList(uint16_t tagid);
void AnchorListUpdate(void);
@@ -97,9 +110,9 @@
* - byte 14 - 17: response message reception timestamp.
* - byte 18 - 21: final message transmission timestamp.
*/
-static uint8_t tx_poll_msg[] = {0x41, 0x88, 0, 0x4D, 0x4B, 0x53, 0x45, 0x4D, 0x49, 0x02};
-static uint8_t rx_resp_msg[] = {0x41, 0x88, 0, 0x4D, 0x4B, 0x4D, 0x49, 0x53, 0x45, 0x03, 0x07};
-static uint8_t tx_final_msg[] = {0x41, 0x88, 0, 0x4D, 0x4B, 0x53, 0x45, 0x4D, 0x49, 0x04, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+//static uint8_t tx_poll_msg[] = {0x41, 0x88, 0, 0x4D, 0x4B, 0x53, 0x45, 0x4D, 0x49, 0x02};
+//static uint8_t rx_resp_msg[] = {0x41, 0x88, 0, 0x4D, 0x4B, 0x4D, 0x49, 0x53, 0x45, 0x03, 0x07};
+//static uint8_t tx_final_msg[] = {0x41, 0x88, 0, 0x4D, 0x4B, 0x53, 0x45, 0x4D, 0x49, 0x04, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
/* Count value of phy counter when transmitting and receiving frames */
static uint32_t poll_tx_en_start_u32;
@@ -131,7 +144,7 @@
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 nearbaseid_list[MAX_NEARBASE_NUM],mainbase_id,true_nearbase_idlist[MAX_NEARBASE_NUM];
uint16_t u16_nearbase_distlist[MAX_NEARBASE_NUM];
uint32_t temp_tag_count1=0;
uint32_t temp_tag_count2=0;
@@ -184,7 +197,15 @@
return tx_timestamp;
}
-
+static void final_msg_set_ts(uint8_t *ts_field, uint64_t ts)
+{
+ int i;
+ for (i = 0; i < FINAL_MSG_TS_LEN; i++)
+ {
+ ts_field[i] = (uint8_t) ts;
+ ts >>= 8;
+ }
+}
/**
* @brief Correct RX timestamp of the ranging frame.
*
@@ -265,32 +286,49 @@
temp_flag=1;
}
}
-
-
-
-//注意：因为新注册的标签并不知道基站已经收到了，所以要在下一包立刻将新的标签ID发出，否则标签会一直抢占基站的poll包。
-//方法是将新注册的标签添加到下次发送的标签列表头部，其他标签依次向后挪动。
-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;
+uint16_t time111=5000;
+void SetLPTimer(uint16_t time)
+{
+ sleep_timer_stop();
+ if(time==0)
+ {
+ time111=5000;
+ sleep_timer_start(__MS_TO_32K_CNT(time111));
+ }
+ else
+ {
+ time111=time111+time;
+ if(time111>5100)
+ {
+ time111=5000;
+ }
+ sleep_timer_start(__MS_TO_32K_CNT(time111));//测试
+ }
}
+
+////注意：因为新注册的标签并不知道基站已经收到了，所以要在下一包立刻将新的标签ID发出，否则标签会一直抢占基站的poll包。
+////方法是将新注册的标签添加到下次发送的标签列表头部，其他标签依次向后挪动。
+//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;
+//}
uint8_t FindNearBasePos(uint16_t baseid)//寻找当前列表中的基站返回索引
{
uint8_t i;
@@ -303,55 +341,55 @@
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;//更新当前基站数
- }
-}
+//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;//更新当前基站数
+// }
+//}
-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];
- }
-}
+//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];
+// }
+//}
static uint16_t CmpTagInList(uint16_t tagid)//找这个新一包的基站在不在现存的taglist中在的话对应位置更新存活时间，不在就增加（确保有效数量小于最大设置数）
{
uint16_t temp;
@@ -368,140 +406,645 @@
return temp;
}
-int TagRange(void)
+static uint8_t tx_poll_msg[20] = {0};
+static uint8_t tx_sync_msg[14] = {0};
+static uint8_t tx_final_msg[60] = {0};
+static uint8_t tx_resp_msg[22] = {0};
+static uint8_t tx_near_msg[180] = {0};
+uint32_t rec_tagpos_binary;
+uint16_t nearbaseid_list2[MAX_NEARBASE_NUM];
+uint8_t module_power,imu_enable,motor_enable;
+extern uint8_t bat_percent;
+extern uint16_t tag_frequency;
+uint16_t bigslot_num;
+uint16_t tagslotpos;
+uint8_t trygetnearmsg_times,try_reg_times;
+uint8_t tag_succ_times=0;
+int32_t salvebase_mindist;
+uint8_t recbase_num=0;
+#define MAX_NEARBASE_ANCNUM 11
+uint16_t rec_maxrangelen,rec_anc_signalpower[MAX_NEARBASE_ANCNUM];
+uint8_t flag_finalsend,flag_getresponse,flag_rxon;
+uint8_t nearbase_num=0,last_nearbase_num,next_nearbase_num,last_slotnum, para_update,para_len,stationary_flag=0;
+int32_t temp_dist;
+void GetNearMsg(void)
{
- // The following peripherals will be initialized in the uwb_open function
- // phy/mac/aes/lsp/phy timers initialized
- uwb_open();
-
- // Set calibration parameters
- uwb_calibration_params_set(config.phy_cfg.ch_num);
-
- // set advanced parameters
- struct PHY_ADV_CONFIG_T adv_config = {
- .thres_fap_detect = 40,
- .nth_scale_factor = 4,
- .ranging_performance_mode = 0,
- .skip_weakest_port_en = 0,
- };
- phy_adv_params_configure(&adv_config);
-
- // uwb configure
- uwb_configure(config.phy_work_mode, board_param.tx_power_fcc[CALIB_CH(config.phy_cfg.ch_num)], &config.phy_cfg);
-
- ranging_frame_type_set(config.phy_cfg.sts_pkt_cfg);
-
- // Register rx interrupt callback function
+ uint32_t start_poll,frame_len;
+ uint8_t nearmsg_i=0;
+// NextSlotDelayMs(0);
+ phy_timer_open(1, IRQ_PRIORITY_HIGH);
mac_register_process_handler(tx_int_callback, rx_int_callback);
+ for(nearmsg_i=0; nearmsg_i<MAX_NEARBASE_NUM; nearmsg_i++)
+ {
+ nearbase_distlist[nearmsg_i] = 0x1ffff;
+ }
+//mainbase_id = 0x1;
+ memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);
+ memcpy(&tx_near_msg[TAG_ID_IDX],&g_com_map[DEV_ID],2);
+ tx_near_msg[MESSAGE_TYPE_IDX] = POS_MSG;
+//发送必须要延时发送才可以用于测距否则立即发送会获取时间戳不对，需要计算程序运行时间，避免设置过去时间
+ poll_tx_en_start_u32 = phy_timer_count_get()+US_TO_PHY_TIMER_COUNT(POLL_DELAY);
+ tempflag=uwb_tx(tx_near_msg,12,1,poll_tx_en_start_u32);//立即发送
+ 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();
+ //设置发送多少s后接收按target独立时间起点算+ US_TO_PHY_TIMER_COUNT(POLL_TX_TO_RESP_RX_DLY_US为发送后要多久开启接收时间
+ resp_rx_en_start_u32 = poll_tx_en_start_u32+ US_TO_PHY_TIMER_COUNT(POLL_TX_TO_RESP_RX_DLY_US);
+ temp_resp=resp_rx_en_start_u32;
+ tempflag=uwb_rx(0,0, RESP_RX_TIMEOUT_US);
+ while(mac_is_busy())//等待接收完成
+ {
+ IdleTask();
+ }
+ if(receive_flag==1)//成功接收数据
+ {
+ receive_flag=0;
+ if (rx_buf[MESSAGE_TYPE_IDX] == POS_MSG&&!memcmp(&rx_buf[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据
+ {
+ nearbase_num=rx_buf[NEARBASENUM_INDEX];
+ memcpy(&rec_tagpos_binary,&rx_buf[NEARMSG_EMPTYSLOTPOS_INDEX],4);
+ // tagslotpos=GetRandomSlotPos(rec_tagpos_binary);
+ //tagslotpos=rx_buffer[TAGSLOTPOS];
+ memcpy(nearbaseid_list,&rx_buf[NEARBASEID_INDEX],nearbase_num*2);
+ memcpy(nearbaseid_list2,&rx_buf[NEARBASEID_INDEX],nearbase_num*2);
+ //slottime=ceil((nearbase_num+2)*0.3)+1;
+ //tyncpoll_time=tagslotpos*slottime;
+ //tyncpoll_time=(g_com_map[DEV_ID]%max_slotpos)*slottime;
+ tag_state=NEARPOLL;
+ }
+ }
+ else {
+// tyncpoll_time = (tagslotpos--%max_slotpos)*slottime;
+ }
+}
+void Registor_Poll(void)
+{
+ static uint8_t regpoll_count=0;
+ mainbase_lost_count = 0;
+ phy_timer_open(1, IRQ_PRIORITY_HIGH);
+ mac_register_process_handler(tx_int_callback, rx_int_callback);
+// tag_frequency = REGISTER_FREQUENCY;
+// bigslot_num = TOTAL_SLOTNUM/tag_frequency;
+// regpoll_count++;
+// if(regpoll_count%2)
+// {
+// if(tagslotpos--<2)
+// tagslotpos=TOTAL_SLOTNUM;
+// }
+// //SetNextPollTime(tagslotpos);
+// NextSlotDelayMs(-2);
+// tx_near_msg[REGP_TAGSTATE_INDEX] = !GET_USERKEY<<2|imu_enable<<1|motor_enable;
+ tx_near_msg[REGP_TAGSTATE_INDEX] = 0;
+ tx_near_msg[MESSAGE_TYPE_IDX] = REG_POLL2;
+ tx_near_msg[REGP_FREQUENCY_INDEX] = 1000/g_com_map[COM_INTERVAL];
+ tx_near_msg[REGP_POWER_INDEX] = module_power;
+ memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);
+ memcpy(&tx_near_msg[REGP_VERSION_INDEX],&g_com_map[VERSION],2);
+ memcpy(&tx_near_msg[REGP_IMUTHRES_INDEX],&g_com_map[IMU_THRES],2);
+ memcpy(&tx_near_msg[REGP_NOMOVESLEEPTIME_INDEX],&g_com_map[NOMOVESLEEP_TIME],2);
+// tx_near_msg[REGP_BATTARY_INDEX] = bat_percent;
+
+ poll_tx_en_start_u32 = phy_timer_count_get()+US_TO_PHY_TIMER_COUNT(POLL_DELAY);
+ tempflag=uwb_tx(tx_near_msg,23,1,poll_tx_en_start_u32);//立即发送
+ 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());//等待发送完成
+
+ resp_rx_en_start_u32 = poll_tx_en_start_u32+ US_TO_PHY_TIMER_COUNT(POLL_TX_TO_RESP_RX_DLY_US);
+ temp_resp=resp_rx_en_start_u32;
+ tempflag=uwb_rx(0,0, RESP_RX_TIMEOUT_US);
+ while(mac_is_busy())//等待接收完成
+ {
+ IdleTask();
+ }
+ if(receive_flag==1)//成功接收数据
+ {
+ receive_flag=0;
+ if (rx_buf[MESSAGE_TYPE_IDX] == REG_RESPONSE&&!memcmp(&rx_buf[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据
+ {
+ uint16_t rec_nearbaseid,rec_nearbasepos;
+ memcpy(&rec_nearbaseid,&rx_buf[ANCHOR_ID_IDX],2);
+ tag_frequency = rx_buf[REGR_TAGFREQ_INDEX];
+ bigslot_num = TOTAL_SLOTNUM/tag_frequency;
+ tagslotpos = rx_buf[REGR_TAGSLOTPOS_INDEX];
+ //////////////////////////////////时间同步
+// memcpy(&sync_timer,&rx_buf[ANCTIMEMS],2);
+// memcpy(&tmp_time,&rx_buf[ANCTIMEUS],2);
+// current_count=HAL_LPTIM_ReadCounter(&hlptim1);
+//// memcpy(&tagslotpos,&rx_buffer[TAGSLOTPOS],2);
+// tmp_time=tmp_time+450;
+// if(tmp_time>999)
+// {
+// tmp_time-=999;
+// sync_timer++;
+// if(sync_timer>=1010)
+// {
+// sync_timer=0;
+// }
+// }
+// ancsync_time=((sync_timer)*1000+tmp_time);
+// SetLPTimer(ancsync_time,0);
+ mainbase_id=rec_nearbaseid;
+ nearbase_num=rx_buf[NEARBASENUM_INDEX];
+ memcpy(nearbaseid_list,&rx_buf[NEARBASEID_INDEX],nearbase_num*2);
+ memcpy(nearbaseid_list2,&rx_buf[NEARBASEID_INDEX],nearbase_num*2);
+ if(tagslotpos!=255)
+ {
+ trygetnearmsg_times = 0;
+ tag_state = GETNEARMSG;
+ GetNearMsg();
+ }
+ }
+ }
+}
+uint8_t ggggg;
+void DiscPoll2(void)
+{
+ uint32_t temp1,temp2,dw_systime,mindist_slavebaseid,mindist;
+ static uint8_t notenoughdist_count=0;
+ uint32_t final_tx_time;
+ uint32_t start_poll;
+ uint8_t i,j,getsync_flag=0,timeout,get_newdist;
-//#if LOW_POWER_EN
-// // Initialize low power mode
-// power_init();
-// // 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()+US_TO_PHY_TIMER_COUNT(POLL_DELAY);//发送必须要延时发送才可以用于测距否则立即发送会获取时间戳不对，需要计算程序运行时间，避免设置过去时间
- tempflag=uwb_tx(uwb_sendbuffer,13+4*nearbase_num,1,poll_tx_en_start_u32);//立即发送
-
- //gpio_pin_set(IO_PIN_5);//测试
- //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_tag_count1=phy_timer_count_get();
- while(mac_is_busy());//等待发送完成
- temp_tag_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);
-
- receive_flag =0;
+ tag_succ_times = 0;
+ salvebase_mindist=999999;
+ get_newdist = 0;
+ if(next_nearbase_num>=MAX_NEARBASE_NUM)
+ {
+ next_nearbase_num = MAX_NEARBASE_NUM-1;
+ }
+ nearbase_num = 10;
+ recbase_num=0;
+ phy_timer_open(1, IRQ_PRIORITY_HIGH);
+ mac_register_process_handler(tx_int_callback, rx_int_callback);
+ tx_near_msg[BATTARY_IDX] = bat_percent;
+ tx_near_msg[BUTTON_IDX] = 0/*userkey_state|stationary_flag<<1|gotosleep_flag<<2|motor_flag<<5*/;
+ tx_near_msg[SEQUENCE_IDX] = frame_seq_nb;
+ tx_near_msg[SEQUENCEH_IDX] = (frame_seq_nb++)>>8;
+ tx_near_msg[NEARBASENUM_INDEX] = nearbase_num;
+ memcpy(&tx_near_msg[NEARBASEID_INDEX],&nearbaseid_list,nearbase_num*2);
+ memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*2],&nearbase_distlist,nearbase_num*4+4);
+ memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*6+4],&rec_anc_signalpower,nearbase_num*2+2);
+// if(intheight!=0)
+// intheight+=g_com_map[HEIGHTOFFEST_INDEX];
+// memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*8+6],&intheight,2);
+ tx_near_msg[MESSAGE_TYPE_IDX] = DISCOVERPOLL2;
+ tx_near_msg[NEARP_TAGFREQ_INDEX] = tag_frequency;
+ tx_near_msg[NEARP_TAGSLOTPOS_INDEX] = tagslotpos;
+ memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);
+
+ poll_tx_en_start_u32 = phy_timer_count_get()+US_TO_PHY_TIMER_COUNT(POLL_DELAY);//发送必须要延时发送才可以用于测距否则立即发送会获取时间戳不对，需要计算程序运行时间，避免设置过去时间
+ tempflag=uwb_tx(tx_near_msg,29+8*nearbase_num,1,poll_tx_en_start_u32);//立即发送
+
+ poll_tx_ts_i64 = ranging_tx_time_correct(poll_tx_en_start_u32 + phy_shr_duration());//修正时间戳
+ temp_tag_count1=phy_timer_count_get();
+ while(mac_is_busy());//等待发送完成
+
+ tx_near_msg[TAGCONFIGSUCCESS_INDEX] =0;
+ para_update = 0;
+ flag_finalsend=0;
+ flag_rxon=1;
+ flag_getresponse=0;
+
+ temp_tag_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);
+ receive_flag =0;
+ 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(30000);//多一个多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();
+ mainbase_dist=100000;
+ mainbase_lost_count++;
+ while(current_count<end_receive_count||current_count>end_receive_count+HALF_SECOND_TIME)//循环接受包体,若为124.8K则是+62400000
+ {
+
+ current_count=phy_timer_count_get();//不断刷新当前计数器值
+ temp_tag_count2=phy_timer_count_get();
+ while(mac_is_busy());//等待接收完成
+ temp_tag_count3=phy_timer_count_get();
+ if(receive_flag==1)//成功接收数据
+ {
+ receive_flag=0;
+
+ //接收成功则判断是否为同一组
+ if (rx_buf[MESSAGE_TYPE_IDX] == POS_RESPONSE&&!memcmp(&rx_buf[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据
+ {
+ uint16_t rec_nearbaseid,rec_nearbasepos;
+ recbase_num++;
+ memcpy(&rec_nearbaseid,&rx_buf[ANCHOR_ID_IDX],2);
+ rec_nearbasepos = recbase_num;
+ nearbaseid_list[recbase_num] = 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);//这个是改过的参数
+ exsistbase_list[rec_nearbasepos]=KEEP_TIMES;
+ temp_dist = nearbase_distlist[rec_nearbasepos];
+ tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
+ }else
+ {
+ if(recbase_num!=nearbase_num+1)
+ {
+ tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
+ }
+ }
+ //gpio_pin_clr(IO_PIN_5);//测试
+
+ }
+ else if(receive_flag==2)
+ {//接收出错
+ receive_flag=0;
+ ggggg++;
+ tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
+ }
+ //gpio_pin_clr(IO_PIN_5);//测试
+ }
+ delay_us(1);
+ uwb_rx_force_off(1);
+ CalculateDists(poll_tx_ts_i64); //计算距离并更新距离表
+// AnchorListUpdate();//更新存活基站列表
+// Rank_ANchor_list_by_dis();//冒泡排序重新按距离重新排列基站列表最小距离存货基站放到最前面
+ for(i=0; i<11; i++)
+ {
+ if(mindist>nearbase_distlist[i])
+ {
+ mindist = nearbase_distlist[i];
+ mindist_slavebaseid = nearbaseid_list[i];
+
+ }
+ }
+ if(mindist!=0x1ffff)
+ {
+ mainbase_id = mindist_slavebaseid;
+ try_reg_times = 0;
+ tag_state = REGPOLL;
+ }
+}
+
+uint8_t qiehuan_flag;
+uint8_t motor_state,rec_remotepara_state,rec_remotepara[80];
+uint8_t motor_flag;
+extern uint8_t userkey_state;
+int32_t base_mindist;
+uint8_t changemainbase_count=0,gotosleep_flag,singlepoll_i;
+uint8_t outrange_times;
+uint8_t pack_length = 0,pack_index = 0,pack_msgtype = 0;
+float motor_keeptime;
+int32_t mainbase_dist,nearbase_switchdistlist[MAX_NEARBASE_NUM],nearbase_distlist[MAX_NEARBASE_NUM],true_nearbase_distlist[MAX_NEARBASE_NUM],true_exsistbase_list[MAX_NEARBASE_NUM],ancsync_time,nextpoll_delaytime,offsettimeus;
+void NearPoll(void)
+{
+
+ uint32_t temp1,temp2,dw_systime,mindist_slavebaseid;
+ static uint8_t notenoughdist_count=0;
+ uint32_t final_tx_time;
+ uint32_t start_poll;
+ uint8_t i,j,getsync_flag=0,timeout,get_newdist;
+ tag_succ_times = 0;
+ salvebase_mindist=999999;
+ get_newdist = 0;
+ phy_timer_open(1, IRQ_PRIORITY_HIGH);
+ mac_register_process_handler(tx_int_callback, rx_int_callback);
+ if(next_nearbase_num>=MAX_NEARBASE_NUM)
+ {
+ next_nearbase_num = MAX_NEARBASE_NUM-1;
+ }
+ if(nearbase_num>10)
+ {
+ nearbase_num = 10;
+ }
+ recbase_num=0;
+// motor_state=0;
+ if(motor_state!=0&&motor_state!=3)
+ {
+ motor_flag = 1;
+ } else {
+ motor_flag = 0;
+ }
+
+ tx_near_msg[BATTARY_IDX] = bat_percent;
+ tx_near_msg[BUTTON_IDX] = userkey_state|stationary_flag<<1|gotosleep_flag<<2|motor_flag<<5;
+ tx_near_msg[SEQUENCE_IDX] = frame_seq_nb;
+ tx_near_msg[SEQUENCEH_IDX] = (frame_seq_nb++)>>8;
+ tx_near_msg[NEARBASENUM_INDEX] = nearbase_num;
+ memcpy(&tx_near_msg[NEARBASEID_INDEX],&nearbaseid_list,nearbase_num*2);
+ memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*2],&nearbase_distlist,nearbase_num*4+4);
+ memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*6+4],&rec_anc_signalpower,nearbase_num*2+2);
+
+
+// if(intheight!=0)
+// intheight+=g_com_map[HEIGHTOFFEST_INDEX];
+// memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*8+6],&intheight,2);
+ tx_near_msg[MESSAGE_TYPE_IDX] = POS_POLL;
+
+ tx_near_msg[NEARP_TAGFREQ_INDEX] = tag_frequency;
+ tx_near_msg[NEARP_TAGSLOTPOS_INDEX] = tagslotpos;
+
+ memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);
+
+ poll_tx_en_start_u32 = phy_timer_count_get()+US_TO_PHY_TIMER_COUNT(POLL_DELAY);//发送必须要延时发送才可以用于测距否则立即发送会获取时间戳不对，需要计算程序运行时间，避免设置过去时间
+ tempflag=uwb_tx(tx_near_msg,29+8*nearbase_num,1,poll_tx_en_start_u32);//立即发送
+
+ poll_tx_ts_i64 = ranging_tx_time_correct(poll_tx_en_start_u32 + phy_shr_duration());//修正时间戳
+ temp_tag_count1=phy_timer_count_get();
+ while(mac_is_busy());//等待发送完成
+
+ tx_near_msg[TAGCONFIGSUCCESS_INDEX] =0;
+ para_update = 0;
+ flag_finalsend=0;
+ flag_rxon=1;
+ flag_getresponse=0;
+ recbase_num=0;
+
+ temp_tag_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);
+ receive_flag =0;
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(RESP_RX_TIMEOUT_US);//多一个多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_tag_count2=phy_timer_count_get();
- while(mac_is_busy());//等待接收完成
- temp_tag_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,(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长度
-// uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
-// while(mac_is_busy());
- //break;//成功接收就退出
- }
- tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
- //gpio_pin_clr(IO_PIN_5);//测试
-
- }else if(receive_flag==2){//接收出错
- receive_flag=0;
- tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
- temp_tag_count3=phy_timer_count_get();
-// while(mac_is_busy());
- temp_tag_count1=phy_timer_count_get();
- }
- //gpio_pin_clr(IO_PIN_5);//测试
- }
- delay_us(1);
- uwb_rx_force_off(1);
- CalculateDists(poll_tx_ts_i64); //计算距离并更新距离表
- AnchorListUpdate();//更新存活基站列表
- Rank_ANchor_list_by_dis();//冒泡排序重新按距离重新排列基站列表最小距离存货基站放到最前面
- LOG_INFO(TRACE_MODULE_APP,"基站ID：%X,距离: %d\r\n",rec_nearbaseid,nearbase_distlist[0]);
+ current_count=phy_timer_count_get();
+ mainbase_dist=100000;
+ mainbase_lost_count++;
+
+ while(current_count<end_receive_count||current_count>end_receive_count+HALF_SECOND_TIME)//循环接受包体,若为124.8K则是+62400000
+ {
+
+ current_count=phy_timer_count_get();//不断刷新当前计数器值
+ temp_tag_count2=phy_timer_count_get();
+ while(mac_is_busy());//等待接收完成
+ temp_tag_count3=phy_timer_count_get();
+ if(receive_flag==1)//成功接收数据
+ {
+ receive_flag=0;
+
+ //接收成功则判断是否为同一组
+ if (rx_buf[MESSAGE_TYPE_IDX] == POS_RESPONSE&&!memcmp(&rx_buf[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据
+ {
+ uint16_t rec_nearbaseid,rec_nearbasepos;
+ recbase_num++;
+ memcpy(&rec_nearbaseid,&rx_buf[ANCHOR_ID_IDX],2);
+ if(rec_nearbaseid==mainbase_id)
+ {
+ 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);
+ //////////////////////////////////时间同步
+// memcpy(&sync_timer,&rx_buffer[ANCTIMEMS],2);
+// memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],2);
+// current_count=HAL_LPTIM_ReadCounter(&hlptim1);
+
+ rec_remotepara_state=rx_buf[MOTORSTATE_INDEX]>>4;
+ if(rec_remotepara_state&&!memcmp(&rx_buf[TAG_ID_IDX],&dev_id,2))
+ {
+ para_update = 1;
+ tx_near_msg[TAGCONFIGSUCCESS_INDEX] = 1;
+ para_len = rx_length-22;
+ memcpy(rec_remotepara,&rx_buf[REMOTEPARA_INDEX],para_len);
+ }
+// userkey_state = !GET_USERKEY;
+ temp_dist = nearbase_distlist[0];
+ if(temp_dist!=nearbase_distlist[0])
+ {
+ get_newdist++;
+ }
+ mainbase_dist=temp_dist;
+ nearbase_distlist[0] = temp_dist;
+ base_mindist = temp_dist;
+ if((rx_buf[MOTORSTATE_INDEX]&0xf)!=3)
+ motor_state=rx_buf[MOTORSTATE_INDEX]&0xf;
+ if(rx_length==38)
+ {
+ memcpy(&rec_maxrangelen,&rx_buf[MAXRANGE_DISTANCE],2);
+ memcpy(&rec_anc_signalpower[0],&rx_buf[ANC_SIGNALPOWER],2);
+ if(mainbase_dist>rec_maxrangelen&&mainbase_dist>0&&rec_maxrangelen!=0)
+ {
+ if(outrange_times++>OUTRANGE_RESTARTTIMES)
+ {
+ tag_state = STARTPOLL;
+ }
+ } else {
+ outrange_times = 0;
+ }
+ }
+ mainbase_lost_count=0;
+ flag_finalsend=1;
+ flag_getresponse=1;
+// //时间同步
+// tmp_time=tmp_time+450;
+// if(tmp_time>999)
+// {
+// tmp_time-=999;
+// sync_timer++;
+// if(sync_timer>=1000)
+// {
+// sync_timer=0;
+// }
+// }
+// ancsync_time=((sync_timer)*1000+tmp_time);
+// SetLPTimer(ancsync_time,nearbase_num);
+ break;
+ }
+ else
+ {
+ rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
+ SetANCTimestap(rec_nearbasepos+1,&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);
+
+ exsistbase_list[rec_nearbasepos+1]=KEEP_TIMES;
+ temp_dist = nearbase_distlist[rec_nearbasepos+1];
+ if(rx_length==38)
+ memcpy(&rec_anc_signalpower[rec_nearbasepos+1],&rx_buf[ANC_SIGNALPOWER],2);
+ memcpy(&nearbase_switchdistlist[rec_nearbasepos],&rx_buf[NR_NEARSWITCH_DISTANCE],2);
+ if(temp_dist!=nearbase_distlist[rec_nearbasepos+1])
+ {
+ get_newdist++;
+ }
+ nearbase_distlist[rec_nearbasepos+1]=temp_dist; // nearbase_distlist[1]对应 rec_nearbaseid[0]的距离
+ final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos)*4], (uint32_t)resp_rx_ts_i64);
+ if(temp_dist<salvebase_mindist&&nearbase_switchdistlist[rec_nearbasepos]!=0&&(nearbase_switchdistlist[rec_nearbasepos]==1||temp_dist<nearbase_switchdistlist[rec_nearbasepos]))
+ {
+ salvebase_mindist = temp_dist;
+ mindist_slavebaseid = rec_nearbaseid;
+ }
+ tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
+ }
+ }else
+ {
+ if(recbase_num!=nearbase_num+1)
+ {
+ tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
+ }
+ }
+ }
+ else if(receive_flag==2)
+ {
+ if(recbase_num!=nearbase_num+1)
+ {
+ tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
+ }
+ }
+ }
+ if(flag_finalsend!=1)
+ {
+ flag_finalsend = 2;
+ }
+ delay_us(1);
+ uwb_rx_force_off(1);
+ if(mainbase_lost_count==0)
+ {
+ SetLPTimer(0);
+#ifdef SWITCHBASE_DIST
+ if(salvebase_mindist<mainbase_dist- THRESHOLD_CHANGE_MAINBASE_DIST )
+ {
+ changemainbase_count++;
+ if(changemainbase_count>2)
+ {
+ changemainbase_count = 0;
+ mainbase_id = mindist_slavebaseid;
+ tag_state = GETNEARMSG;
+ trygetnearmsg_times = 0;
+ GetNearMsg();
+ qiehuan_flag=1;
+ }
+ } else {
+ changemainbase_count = 0;
+ }
+#endif
+ } else if(mainbase_lost_count>10)
+ {
+ mainbase_lost_count = 0;
+ tag_state = DISCPOLL;
+ DiscPoll2();
+ SetLPTimer(16);
+ }
+ if(mainbase_lost_count!=0)
+ {
+// if(mainbase_lost_count<=5)
+// { NextSlotDelayMs(0);
+// } else {
+// NextSlotDelayMs(0);
+// }
+ SetLPTimer(11);
+ }
+ if(qiehuan_flag)
+ {qiehuan_flag=0;}
+ else
+ {CalculateDists(poll_tx_ts_i64);}
+// AnchorListUpdate();//更新存活基站列表
+// Rank_ANchor_list_by_dis();//冒泡排序重新按距离重新排列基站列表最小距离存货基站放到最前面
+ if(para_update)
+ {
+
+ pack_msgtype = rec_remotepara[0];
+ pack_index = rec_remotepara[1];
+ pack_length = rec_remotepara[2];
+ if(pack_msgtype==2)
+ {
+ if( pack_index == MOTOR_ONTIME_INDEX)
+ {
+ if(motor_keeptime==0)
+ motor_keeptime = rec_remotepara[3];
+ } else if( pack_index == 2*COM_INTERVAL)
+ {
+ if(memcmp(&g_com_map[COM_INTERVAL],&rec_remotepara[3],2)!=0)
+ {
+ memcpy((uint8_t*)&g_com_map + pack_index, &rec_remotepara[3], pack_length);
+ save_com_map_to_flash();
+ tag_frequency = 1000/g_com_map[COM_INTERVAL];
+ bigslot_num = TOTAL_SLOTNUM/tag_frequency;
+ }
+ }
+ else {
+ if(pack_index<200)
+ {
+ memcpy((uint8_t*)&g_com_map + pack_index, &rec_remotepara[3], pack_length);
+ //返回一个error状态
+ //SendComMap(pack_datalen,pack_index);
+ save_com_map_to_flash();
+ delay_ms(100);
+ NVIC_SystemReset();
+ }
+ }
+ }
+ }
+ for(i=0; i<nearbase_num; i++)
+ {
+ if(nearbaseid_list[i]!=nearbaseid_list2[i])
+ {
+ tag_state = GETNEARMSG;
+ GetNearMsg();
+ trygetnearmsg_times = 0;
+// nearbaseid_list0[i]=1;
+ }
+ }
+ TagListUpdate();
}
-void UWB_Tag_Poll(void)
+void SwitchTagState(void)
{
- switch(UWB_work_state)
- {
- case LINK_SUCCESS:
- case SEARCH_DEV:
- case UN_BIND:
- {
- uwb_led_on();
- TagRange();
- uwb_led_off();
- }
- break;
- }
-// update_led_power_state();//更新灯状态
+ switch(tag_state)
+ {
+ case REGPOLL:
+// LED_LG_ON;
+ Registor_Poll();
+ if(try_reg_times++>5)
+ {
+ tag_state = STARTPOLL;
+ }
+ outrange_times = 0;
+ mainbase_lost_count = 0;
+// LED_LG_OFF;
+// GetPressAndHeight();
+ break;
+ case DISCPOLL:
+// LED_LG_ON;
+ DiscPoll2();
+// LED_LG_OFF;
+ break;
+ case GETNEARMSG:
+
+// LED_LG_ON;
+ GetNearMsg();
+// LED_LG_OFF;
+ outrange_times = 0;
+ mainbase_lost_count = 0;
+ if(trygetnearmsg_times++>5)
+ {
+ tag_state = STARTPOLL;
+ }
+ break;
+ case NEARPOLL:
+ NearPoll();
+ break;
+ case SINGLEPOLL:
+ // Poll();
+ break;
+ }
+}
+
+void Dw1000_App_Init(void)
+{
+//g_com_map[DEV_ID] = 0x0b;
+ //tag_state=DISCPOLL;
+ tx_poll_msg[MESSAGE_TYPE_IDX]=POLL;
+ tx_resp_msg[MESSAGE_TYPE_IDX]=RESPONSE;
+ tx_final_msg[MESSAGE_TYPE_IDX]=FINAL;
+ tx_sync_msg[MESSAGE_TYPE_IDX]=SYNC;
+
+ memcpy(&tx_poll_msg[GROUP_ID_IDX], &group_id, 1);
+ memcpy(&tx_final_msg[GROUP_ID_IDX], &group_id, 1);
+ memcpy(&tx_resp_msg[GROUP_ID_IDX], &group_id, 1);
+
+ memcpy(&tx_poll_msg[TAG_ID_IDX], &g_com_map[DEV_ID], 2);
+ memcpy(&tx_final_msg[TAG_ID_IDX], &g_com_map[DEV_ID], 2);
+ memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &g_com_map[DEV_ID], 2);
+ memcpy(&tx_sync_msg[ANCHOR_ID_IDX], &g_com_map[DEV_ID], 2);
+ memcpy(&tx_near_msg[ANCHOR_ID_IDX], &g_com_map[DEV_ID], 2);
+ memcpy(&tx_near_msg[TAG_ID_IDX], &g_com_map[DEV_ID], 2);
}
\ No newline at end of file
--
Gitblit v1.9.3