From 2ee355706fb52cd610000dfa89ad244656428d72 Mon Sep 17 00:00:00 2001
From: WXK <287788329@qq.com>
Date: 星期四, 14 八月 2025 18:23:46 +0800
Subject: [PATCH] 开发完成 先提交 适配纽扣电池
---
keil/uwb_tag.c | 1201 ++++++++++++++++++++++++++++++++++++++++++++++----------
1 files changed, 980 insertions(+), 221 deletions(-)
diff --git a/keil/uwb_tag.c b/keil/uwb_tag.c
index 3f1ed2c..50cb2ee 100644
--- a/keil/uwb_tag.c
+++ b/keil/uwb_tag.c
@@ -8,6 +8,10 @@
#include "dw_tag.h"
#include "board.h"
#include "mk_calib.h"
+#include <serial_at_cmd_app.h>
+#include <global_param.h>
+#include <global_param.h>
+#include "mk_flash.h"
extern int TagRange(void);
extern Operation_step UWB_work_state;
@@ -25,7 +29,7 @@
#define POLL_DELAY 100U //yuan100U
/* Receive response timeout */
-#define RESP_RX_TIMEOUT_US 2000U //Yuan500 10mssuccess 300jixian
+#define RESP_RX_TIMEOUT_US 1000 //Yuan500 10mssuccess 300jixian
/* Field index in frame */
#define MSG_SEQ_NUM_IDX 2
@@ -36,6 +40,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);
@@ -63,23 +79,6 @@
.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 */
};
-///* 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 */
-//};
-/* Buffer to store received frame */
static uint8_t rx_buf[150];
static uint8_t uwb_sendbuffer[150];
/* Frames used in the ranging process
@@ -97,9 +96,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 +130,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 +183,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.
*
@@ -266,31 +273,48 @@
}
}
-
-
-//注意：因为新注册的标签并不知道基站已经收到了，所以要在下一包立刻将新的标签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;
+#ifdef _UWB_1HZ
+uint16_t time111=1000;
+void SetLPTimer(uint16_t time)
+{
+ sleep_timer_stop();
+ if(time==0)
+ {
+ time111=1000;
+ sleep_timer_start(__MS_TO_32K_CNT(time111));
+ }
+ else
+ {
+ time111=time111+time;
+ if(time111>1100)
+ {
+ time111=1000;
+ }
+ sleep_timer_start(__MS_TO_32K_CNT(time111));//测试
+ }
}
+#else
+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));//测试
+ }
+}
+#endif
+
uint8_t FindNearBasePos(uint16_t baseid)//寻找当前列表中的基站返回索引
{
uint8_t i;
@@ -303,55 +327,6 @@
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 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 +343,924 @@
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;
+uint8_t xiaobiaoqian_flag;
+uint8_t readgcom_flag;
+uint16_t wg_report_id;
+int rand_temp;
+static uint16_t source_id;
+uint8_t rec_index,rec_secdelay;
+uint16_t rec_value;
+uint8_t readgcom_flag;
+uint16_t rec_wenjian_daxiao;
+uint16_t wangguan_up_id;
+uint8_t shengji_flag,time=5;
+uint8_t LoraUp_flag;
+extern uint16_t final_bag_num;
+uint16_t checksum1;
+void Uwb_Update(void)
+{
+
+ checksum1=Checksum_u16(rx_buf,rx_length-2);
+ if(!memcmp(&checksum1,&rx_buf[rx_length-2],2))
+ if(!memcmp(&dev_id,&rx_buf[4],2))
+ {
+ memcpy(&source_id,&rx_buf[2],2);
+// if(wg_state==WG_Lost)
+// {
+// wg_state = WG_Connected;
+ wg_report_id = source_id;
+// }
+// if(!memcmp(&wg_report_id,&rx_buf[SOURCE_ID_IDX],2))
+// {
+// wg_lost_count = 0;
+// loraled=GREEN;
+ switch(rx_buf[9])
+ {
+ case WGRSP_RWTAG_READ:
+ readgcom_flag = 1;
+// LoraSendComMap(WGRSP_RWTAG_READ);
+ break;
+ case WGRSP_RWTAG_WRITE:
+ rec_index = rx_buf[PWTAG_WRITE_IDX_IDX];
+ if(rec_index<=DEV_ID)
+ {break;}
+ memcpy(&rec_value,&rx_buf[PWTAG_WRITE_VALUE_IDX],2);
+ if(g_com_map[rec_index/2]==rec_value)
+ {break;}
+ g_com_map[rec_index/2] = rec_value;
+ save_com_map_to_flash();
+ delay_us(60000);
+ NVIC_SystemReset();
+ break;
+ case WGRSP_RWTAG_UPDATE:
+ rec_index = rx_buf[PWTAG_WRITE_IDX_IDX];
+ switch(rec_index)
+ {
+ case 0xaa: //升级下发
+ memcpy(&rec_wenjian_daxiao,&rx_buf[11],2);//文件大小
+ if(rec_wenjian_daxiao>APP_SIZE/100||rec_wenjian_daxiao==0)
+ {}//文件过大，超出范围
+ else
+ {
+ final_bag_num=rec_wenjian_daxiao;
+ memcpy(&wangguan_up_id,&rx_buf[SOURCE_ID_IDX],2);//网关ID 占用2个字节
+ shengji_flag=1;
+ LoraUp_flag=1;
+ LOG_INFO(TRACE_MODULE_APP, "进入升级模式,需要接收的文件包数%d\r\n",rec_wenjian_daxiao);
+ }
+ break;
+ }
+ break;
+ }
+// }
+ }
+}
+uint8_t readgcom_flag1;
+uint32_t current_count11,current_count22,current_count33,current_count44,current_count55;
+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;
+ xiaobiaoqian_flag=0;
+ tag_succ_times = 0;
+ salvebase_mindist=999999;
+ get_newdist = 0;
+ if(readgcom_flag)
+ {
+ readgcom_flag=0;
+ readgcom_flag1=1;
+ phy_timer_open(1, IRQ_PRIORITY_HIGH);
+ mac_register_process_handler(tx_int_callback, rx_int_callback);
+ uint8_t data_length = 0x50;
+ uint16_t checksum = 0;
+ tx_near_msg[0] = 0x39;
+ tx_near_msg[1] = data_length+11;
+ memcpy(&tx_near_msg[2],&g_com_map[DEV_ID],2);
+ memcpy(&tx_near_msg[4],&wg_report_id,2);
+ tx_near_msg[6] = 2;
+ tx_near_msg[7] = 2;
+ tx_near_msg[8] = data_length;
+ memcpy(&tx_near_msg[9], &g_com_map[1], data_length);
+ checksum = Checksum_u16(tx_near_msg,11+data_length);
+ memcpy(&tx_near_msg[11+data_length],&checksum,2);
+// Radio.Send(uwb_sendbuffer,data_length+13);
+ // Delay_Ms(100);
+ poll_tx_en_start_u32 = phy_timer_count_get()+US_TO_PHY_TIMER_COUNT(POLL_DELAY);//发送必须要延时发送才可以用于测距否则立即发送会获取时间戳不对，需要计算程序运行时间，避免设置过去时间
+ tempflag=uwb_tx(tx_near_msg,data_length+13,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());//等待发送完成
+ }
+ else
+ {
+ 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;
+ memcpy(&tx_near_msg[7],&g_com_map[VERSION],2);
+ memcpy(&tx_near_msg[5],&g_com_map[DEV_ID],2);
+ 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);//立即发送
-//#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;
+ 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 = temp_tag_count2+ US_TO_PHY_TIMER_COUNT(600);//设置发送多少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, 3000);
+ receive_flag =0;
+ start_receive_count=phy_timer_count_get();
+ poll_timeout=US_TO_PHY_TIMER_COUNT(600);//多一个多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];
+ if(rx_buf[0]==0xff)
+ {xiaobiaoqian_flag=1;}
+ current_count11=phy_timer_count_get();
+// tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
+ }
+ else if(rx_buf[MSG_TYPE_IDX]==LORA_MSGTYPE_WGRESPTAG)
+ {
+ Uwb_Update();
+ }
+ else
+ {
+ current_count22=phy_timer_count_get();
+ 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超时
+ current_count33=phy_timer_count_get();
+ }
+ //gpio_pin_clr(IO_PIN_5);//测试
+// }
+ delay_us(1);
+ uwb_rx_force_off(1);
+ current_count44=phy_timer_count_get();
+// if(readgcom_flag1==0)
+// {
+ CalculateDists(poll_tx_ts_i64); //计算距离并更新距离表
+// }
+// readgcom_flag1=0;
+// 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&&xiaobiaoqian_flag==0)
+ {
+ 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 = temp_tag_count2+ US_TO_PHY_TIMER_COUNT(600);//设置发送多少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();//更新灯状态
-}
\ No newline at end of file
+ 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);
+}
+
+extern uint8_t send_lora_data[250];
+uint16_t muqiandeshengjibao;
+static uint16_t current_count1,target_count1,end_count1,start_count1;
+uint8_t huifushengjibaoerror_num;
+extern uint16_t final_bag_num;
+uint16_t testflag;
+uint32_t Zhongjian_data[60];
+uint8_t shengji_flag;
+uint16_t wangguan_up_id;
+uint8_t huifushengjibao_flag;
+uint8_t LoraUp_flag;
+uint16_t rec_wenjian_daxiao;
+uint8_t lora_up_rec_flag;
+uint16_t flash_shuju;
+static uint32_t status_reg;
+static uint32_t frame_len;
+void LoraUp_Poll(void)
+{ uint16_t result ;
+ uint16_t crc16;
+ uint8_t jindu;
+ if(shengji_flag)
+ {
+ uwb_rx_force_off(1);
+ Uwb_init();
+ phy_timer_open(1, IRQ_PRIORITY_HIGH);
+ mac_register_process_handler(tx_int_callback, rx_int_callback);
+
+ send_lora_data[MSG_TYPE_IDX]=LORA_MSGTYPE_UPDATE_CONFIRM;
+ memcpy(&send_lora_data[SOURCE_ID_IDX],&dev_id,2);//网关ID 占用2个字节
+ memcpy(&send_lora_data[DEST_ID_IDX],&wangguan_up_id,2);//标签或者基站的设备ID 2个字节
+// send_lora_data[MUQIAN_BAG]=0xFF;
+ crc16=Checksum_u16(send_lora_data,6);
+ memcpy(&send_lora_data[6],&crc16,2);
+ huifushengjibao_flag=0;
+ LoraUp_flag=1;
+
+ poll_tx_en_start_u32 = phy_timer_count_get()+US_TO_PHY_TIMER_COUNT(POLL_DELAY);//发送必须要延时发送才可以用于测距否则立即发送会获取时间戳不对，需要计算程序运行时间，避免设置过去时间
+ tempflag=uwb_tx(send_lora_data,8+2,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());//等待发送完成
+
+ __disable_irq();
+ result=flash_open(FLASH_ID0,NULL);
+ flash_read(FLASH_ID0, APP2_ADRESS, (uint8_t*)&flash_shuju, 2);
+ if(flash_shuju!=0xffff/*&&flash_shuju!=0x0000*/)
+ {
+ while(1)
+ {
+ flash_erase(FLASH_ID0,APP2_ADRESS,APP_SIZE);
+ delay_ms(500);
+ flash_read(FLASH_ID0, APP2_ADRESS, (uint8_t*)&flash_shuju, 2);
+ if(flash_shuju==0xffff/*&&flash_shuju!=0x0000*/)
+ {break;}
+ }
+ }
+ __enable_irq();
+ while(1)
+ {
+ uwb_rx_force_off(1);
+ send_lora_data[MSG_TYPE_IDX]=LORA_MSGTYPE_UPDATEFILE_REQUEST;
+ memcpy(&send_lora_data[SOURCE_ID_IDX],&dev_id,2);//网关ID 占用2个字节
+ memcpy(&send_lora_data[DEST_ID_IDX],&wangguan_up_id,2);//标签或者基站的设备ID 2个字节
+// send_lora_data[MUQIAN_BAG]=muqiandeshengjibao;
+ memcpy(&send_lora_data[MUQIAN_BAG],&muqiandeshengjibao,2);
+ crc16=Checksum_u16(send_lora_data,8);
+ memcpy(&send_lora_data[8],&crc16,2);
+ lora_up_rec_flag=0;
+// LOG_INFO(TRACE_MODULE_APP, "正在请求第%d包\r\n",muqiandeshengjibao);
+ jindu=(muqiandeshengjibao*100)/final_bag_num;
+ LOG_INFO(TRACE_MODULE_APP, "升级进度%d\r\n",jindu);
+// wdt_ping(WDT_ID0);//喂狗
+ poll_tx_en_start_u32 = phy_timer_count_get()+US_TO_PHY_TIMER_COUNT(POLL_DELAY);//发送必须要延时发送才可以用于测距否则立即发送会获取时间戳不对，需要计算程序运行时间，避免设置过去时间
+ tempflag=uwb_tx(send_lora_data,10+2,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();
+ 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(0,0, 50000);
+
+ while(mac_is_busy())
+ {}
+ if(receive_flag==1)//成功接收数据
+ { uint16_t result12;
+
+ __disable_irq();
+ crc16=Cal_CRC16(rx_buf,DATA_IDX+ONE_BAG_DAXIAO);
+
+ if(!memcmp(&muqiandeshengjibao,&rx_buf[MUQIAN_BAG],2)&&!memcmp(&crc16,&rx_buf[DATA_IDX+ONE_BAG_DAXIAO],2)&&!memcmp(&dev_id,&rx_buf[DEST_ID_IDX],2)&& !memcmp(&wangguan_up_id,&rx_buf[SOURCE_ID_IDX],2))
+ {
+ result=flash_open(FLASH_ID0,NULL);
+ memcpy(Zhongjian_data,&rx_buf[DATA_IDX],ONE_BAG_DAXIAO);
+ result12=flash_write_nbytes(FLASH_ID0, APP2_ADRESS+ONE_BAG_DAXIAO*muqiandeshengjibao, (uint8_t*)Zhongjian_data, ONE_BAG_DAXIAO);
+ while(!result12==0)
+ {
+ result12=flash_write_nbytes(FLASH_ID0, APP2_ADRESS+ONE_BAG_DAXIAO*muqiandeshengjibao, (uint8_t*)Zhongjian_data, ONE_BAG_DAXIAO);
+ delay_ms(10);
+ }
+ if(result12==0)
+ {
+// if(muqiandeshengjibao==650)
+// {
+// delay_ms(10);
+// }
+ muqiandeshengjibao++;
+ huifushengjibaoerror_num=0;
+ delay_ms(10);
+ }
+ }
+ __enable_irq();
+ }
+ else
+ {
+ delay_ms(500);
+ huifushengjibaoerror_num++;
+ if(huifushengjibaoerror_num>=20)
+ {
+ huifushengjibaoerror_num=0;
+ muqiandeshengjibao=0;
+ delay_ms(500);
+ NVIC_SystemReset();
+ break;
+ }
+ uwb_rx_force_off(1);
+ }
+ if(muqiandeshengjibao==final_bag_num)
+ {
+ send_lora_data[MSG_TYPE_IDX]=0x99;
+ memcpy(&send_lora_data[SOURCE_ID_IDX],&dev_id,2);//网关ID 占用2个字节
+ memcpy(&send_lora_data[DEST_ID_IDX],&wangguan_up_id,2);//标签或者基站的设备ID 2个字节
+ crc16=Checksum_u16(send_lora_data,6);
+ memcpy(&send_lora_data[6],&crc16,2);
+ huifushengjibao_flag=0;
+ LoraUp_flag=1;
+ LOG_INFO(TRACE_MODULE_APP, "正在发送最后一包\r\n");
+ poll_tx_en_start_u32 = phy_timer_count_get()+US_TO_PHY_TIMER_COUNT(POLL_DELAY);//发送必须要延时发送才可以用于测距否则立即发送会获取时间戳不对，需要计算程序运行时间，避免设置过去时间
+ tempflag=uwb_tx(send_lora_data,8+2,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());//等待发送完成
+
+ uint16_t tmp11 ,result11;
+ tmp11=0XDDDD;
+ result=flash_open(FLASH_ID0,NULL);
+ result11=flash_erase(FLASH_ID0,APP_1OR2_ADRESS,0x1000);
+ if(result11==0)
+ flash_write_nbytes(FLASH_ID0, APP_1OR2_ADRESS, (uint8_t*)&tmp11, 2);
+ __disable_irq();
+ delay_ms(500);
+ NVIC_SystemReset();
+ }
+ }
+ uwb_rx_force_off(1);
+ }
+}
+
--
Gitblit v1.9.3