From 5b8bde9ae08b29e4ce6d04149fe0cb78e0c5d68c Mon Sep 17 00:00:00 2001
From: WXK <287788329@qq.com>
Date: 星期日, 24 三月 2024 13:48:14 +0800
Subject: [PATCH] 1.12，加入lora'升级标签。
---
Src/application/dw_ancapp.c | 719 +++++++++++++++++++----------------------------------------
1 files changed, 234 insertions(+), 485 deletions(-)
diff --git a/Src/application/dw_ancapp.c b/Src/application/dw_ancapp.c
index f48f1d7..da3d62e 100644
--- a/Src/application/dw_ancapp.c
+++ b/Src/application/dw_ancapp.c
@@ -42,6 +42,7 @@
#define RESP_MSG_ANC_DISTOFFSET 34
#define MBX_ANC_NUM_IN_SYS 50
#define QUIT_SLOT_TIME 5
+uint8_t lora_yingda_flag;
static uint64_t resp_tx_ts;
static uint64_t final_rx_ts;
static uint32_t status_reg,resp_tx_time,tagpos_binary;
@@ -51,7 +52,8 @@
static uint8_t flag_syncbase,sync_seq,synclost_timer,temp_nearbaspos;
static uint16_t sync_timer;
static uint8_t send_buffer[150];
-static uint8_t frame_seq_nb2,battary,button,ancidlist_num;
+static uint8_t battary,button,ancidlist_num;
+uint8_t frame_seq_nb2;
uint8_t rec_nearbase_num;
static uint16_t ancidlist_rec[TAG_NUM_IN_SYS],ancidlist_send[TAG_NUM_IN_SYS],rec_ancidlist[TAG_NUM_IN_SYS],rec_ancdistlist[TAG_NUM_IN_SYS];
static int16_t rec_antdelay;
@@ -81,18 +83,18 @@
extern uint16_t g_Resttimer;
static int32_t filter_dist,g_Tagdist[TAG_NUM_IN_SYS];
double LOS(dwt_rxdiag_t *dia) {
- F1 = dia->firstPathAmp1;
- F2 = dia->firstPathAmp2;
- F3 = dia->firstPathAmp3;
- N = dia->rxPreamCount;
- C = dia->maxGrowthCIR;
-
+ F1 = dia->firstPathAmp1;
+ F2 = dia->firstPathAmp2;
+ F3 = dia->firstPathAmp3;
+ N = dia->rxPreamCount;
+ C = dia->maxGrowthCIR;
+
firstpath_power=10* log10((F1*F1+F2*F2+F3*F3)/(N*N))-A;
rx_power=10*log10(C*B/(N*N))-A;
- // min_power = - 10 * log10((F1 *F1 + F2 * F2 + F3 * F3) / (C *B));
+// min_power = - 10 * log10((F1 *F1 + F2 * F2 + F3 * F3) / (C *B));
return min_power;
- }
+}
static uint64_t get_tx_timestamp_u64(void)
{
uint8_t ts_tab[5];
@@ -142,26 +144,26 @@
}
void TagListUpdate(void)
{
- uint16_t i,j=0,temp[TAG_NUM_IN_SYS];
- for(i=0;i<taglist_num;i++)
- {
- if(tagofflinetime[i]++<QUIT_SLOT_TIME)
- {
- tagid_list[j]=tagid_list[i];
- tagofflinetime[j++]=tagofflinetime[i];
- }
- }
- taglist_num=j;
+ uint16_t i,j=0,temp[TAG_NUM_IN_SYS];
+ for(i=0; i<taglist_num; i++)
+ {
+ if(tagofflinetime[i]++<QUIT_SLOT_TIME)
+ {
+ tagid_list[j]=tagid_list[i];
+ tagofflinetime[j++]=tagofflinetime[i];
+ }
+ }
+ taglist_num=j;
}
uint16_t CmpTagInList(uint16_t tagid)
-{uint16_t i;
- for(i=0;i<taglist_num;i++)
- {
- if(memcmp(&tagid,&tagid_list[i],2)==0)
- break;
- }
+{ uint16_t i;
+ for(i=0; i<taglist_num; i++)
+ {
+ if(memcmp(&tagid,&tagid_list[i],2)==0)
+ break;
+ }
tagofflinetime[i] = 0;
- return i;
+ return i;
}
static void resp_msg_set_ts(uint8_t *ts_field, const uint64_t ts)
{
@@ -178,102 +180,49 @@
u16 basesystimer,uwb_losttimer;
uint8_t remotesend_state,remotesend_index;
uint16_t random_time;
+u32 value;
uint8_t Anchor_RecNearPoll(uint8_t ancrec_nearbasepos) //0 mainbase 1 first near_base
{
uint8_t motorstate;
// tmp_time=TIM3->CNT;
- temp_nearbaspos = ancrec_nearbasepos;
- memcpy(&send_buffer[ANCHOR_ID_IDX],&dev_id,2);
- memcpy(&send_buffer[ANCTIMEMS],&basesystimer,2);
+ temp_nearbaspos = ancrec_nearbasepos;
+ memcpy(&send_buffer[ANCHOR_ID_IDX],&dev_id,2);
+ memcpy(&send_buffer[ANCTIMEMS],&basesystimer,2);
// memcpy(&send_buffer[ANCTIMEUS],&tmp_time,2);
- send_buffer[TAGFREQ_IDX] = tag_frequency;
- memcpy(&send_buffer[TAG_ID_IDX],&tag_id_recv,2);
- poll_rx_ts = get_rx_timestamp_u64();//获得Poll包接收时间T2
+ send_buffer[TAGFREQ_IDX] = tag_frequency;
+ memcpy(&send_buffer[TAG_ID_IDX],&tag_id_recv,2);
+ poll_rx_ts = get_rx_timestamp_u64();//获得Poll包接收时间T2
if(rec_nearbase_num == ancrec_nearbasepos)//抢占模式
{
random_time = (GetLPTime()%100);
- resp_tx_time = (poll_rx_ts+((random_time+rec_nearbase_num*20 + POLL_RX_TO_RESP_TX_DLY_UUS+ancrec_nearbasepos*DELAY_BETWEEN_TWO_FRAME_UUS) * UUS_TO_DWT_TIME)) >> 8;//计算Response发送时间T3。
-
- }else{
- resp_tx_time = (poll_rx_ts + ((rec_nearbase_num*20+POLL_RX_TO_RESP_TX_DLY_UUS+ancrec_nearbasepos*DELAY_BETWEEN_TWO_FRAME_UUS) * UUS_TO_DWT_TIME)) >> 8;//计算Response发送时间T3。
- }
- dwt_setdelayedtrxtime(resp_tx_time);//设置Response发送时间T3
- dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS+(rec_nearbase_num+1-ancrec_nearbasepos)*DELAY_BETWEEN_TWO_FRAME_UUS);//设置发送完成后开启接收延迟时间
- dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//接收超时时间
- resp_tx_ts = (((uint64_t)(resp_tx_time & 0xFFFFFFFEUL)) << 8) ;
+ resp_tx_time = (poll_rx_ts+((/*value+*/random_time+rec_nearbase_num*20 + POLL_RX_TO_RESP_TX_DLY_UUS+ancrec_nearbasepos*DELAY_BETWEEN_TWO_FRAME_UUS) * UUS_TO_DWT_TIME)) >> 8;//计算Response发送时间T3。
- /* Write all timestamps in the final message. See NOTE 8 below. */
- resp_msg_set_ts(&send_buffer[RESP_MSG_POLL_RX_TS_IDX], poll_rx_ts);
- resp_msg_set_ts(&send_buffer[RESP_MSG_RESP_TX_TS_IDX], resp_tx_ts);
- memcpy(&send_buffer[RESP_MSG_ANC_DISTOFFSET],&g_com_map[DIST_OFFSET],2);
- if(new_tagid)
- {
- tagdist_list[taglist_pos]=0x1ffff;
- memcpy(&send_buffer[DIST_IDX], &tagdist_list[taglist_pos], 4);
- }else{
- memcpy(&send_buffer[DIST_IDX], &tagdist_list[taglist_pos], 4);
- }
- motorstate =0;
- if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE1]||tagdist_list[taglist_pos]<0)
- {
- motorstate =0;
- }
- else if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE2])
- {
- if(flag_tag_distsmooth[taglist_pos])
- {motorstate =2;
- }else{
- motorstate =0;
- }
- }else if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE3])
- {
- if(flag_tag_distsmooth[taglist_pos])
- {motorstate =1;
- }else{
- motorstate =0;
- }
- }
- send_buffer[GROUP_ID_IDX] = group_id;
+ } else {
+ resp_tx_time = (poll_rx_ts + ((/*value+ */rec_nearbase_num*20+POLL_RX_TO_RESP_TX_DLY_UUS+ancrec_nearbasepos*DELAY_BETWEEN_TWO_FRAME_UUS) * UUS_TO_DWT_TIME)) >> 8;//计算Response发送时间T3。
+ }
+ dwt_setdelayedtrxtime(resp_tx_time);//设置Response发送时间T3
+ //dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS+(rec_nearbase_num+1-ancrec_nearbasepos)*DELAY_BETWEEN_TWO_FRAME_UUS);//设置发送完成后开启接收延迟时间
+ //dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//接收超时时间
+ resp_tx_ts = (((uint64_t)(resp_tx_time & 0xFFFFFFFEUL)) << 8) ;
+
+ /* Write all timestamps in the final message. See NOTE 8 below. */
+ resp_msg_set_ts(&send_buffer[RESP_MSG_POLL_RX_TS_IDX], poll_rx_ts);
+ resp_msg_set_ts(&send_buffer[RESP_MSG_RESP_TX_TS_IDX], resp_tx_ts);
+ memcpy(&send_buffer[RESP_MSG_ANC_DISTOFFSET],&g_com_map[DIST_OFFSET],2);
+
+
+ send_buffer[GROUP_ID_IDX] = group_id;
// send_buffer[MAINBASE_INDEX]=flag_syncbase;
- send_buffer[MESSAGE_TYPE_IDX]=MBX_RESPONSE;
- send_buffer[MOTORSTATE_INDEX]=motorstate;//(remotesend_state<<4)|motorstate;
- send_buffer[MOTORSTATE_INDEX]&=0x0f;
- remotesend_state = 0;
-
-// for(uint8_t i=0;i<PWTAG_MAXGROUPNUM;i++)
-// {
-// if(pwtag[i].remain_time!=0)
-// {
-// if(pwtag[i].id==tag_id_recv)
-// {
-// uint16_t calCRC;
-// uint8_t remotetag_para[10],remotetag_paralen;
-// remotesend_state=1;
-// remotesend_index = i;
-// if(pwtag[i].index==200)
-// {remotesend_state = 2;}
-// send_buffer[MOTORSTATE_INDEX]=(remotesend_state<<4)|motorstate;
-// remotetag_paralen = 2+3+2;
-// remotetag_para[0] = 2;
-// remotetag_para[1] = pwtag[i].index;
-// remotetag_para[2] = 2;
-// memcpy(&remotetag_para[3],&pwtag[i].value,2);
-// calCRC = CRC_Compute(remotetag_para,5);
-// memcpy(&remotetag_para[5],&calCRC,2);
-// memcpy(&send_buffer[REMOTEPARA_INDEX],remotetag_para,remotetag_paralen);
-// dwt_writetxdata(24+remotetag_paralen, send_buffer, 0);//写入发送数据
-// dwt_writetxfctrl(24+remotetag_paralen, 0);//设定发送长度
-//
-// //finalsend = 1;
-// break;
-// }
-// }
-// }
-
- dwt_writetxdata(38, send_buffer, 0);//写入发送数据
- dwt_writetxfctrl(38, 0);//设定发送长度
- result = dwt_starttx(DWT_START_TX_DELAYED);
+ send_buffer[MESSAGE_TYPE_IDX]=MBX_RESPONSE;
+ send_buffer[MOTORSTATE_INDEX]=motorstate;//(remotesend_state<<4)|motorstate;
+ send_buffer[MOTORSTATE_INDEX]&=0x0f;
+ remotesend_state = 0;
+
+ dwt_forcetrxoff();
+ dwt_writetxdata(38, send_buffer, 0);//写入发送数据
+ dwt_writetxfctrl(38, 0);//设定发送长度
+ result = dwt_starttx(DWT_START_TX_DELAYED);
// if(remotesend_state==0)
// {
// dwt_writetxdata(38, send_buffer, 0);//写入发送数据
@@ -281,296 +230,81 @@
// result = dwt_starttx(DWT_START_TX_DELAYED);
//// uwb_timeout = 0;
// if(result == 0)
- uwb_timeout=lp_time;
- while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_TXFRS )))///不断查询芯片状态直到接收成功或者出现错误
- {
-// current_time=HAL_LPTIM_ReadCounter(&hlptim1);
-// if(current_time>end_time||current_time<end_time+15000)
-// break;
- if(lp_time-uwb_timeout>2)
- break;
- };
-// }else{
-// result = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//延迟发送，等待接收
+// {
+ uwb_timeout=lp_time;
+ while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & SYS_STATUS_TXFRS ))///不断查询芯片状态直到接收成功或者出现错误
+ {
-
-// if(result==0)
-// {
-// while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))&&!g_start_sync_flag)///不断查询芯片状态直到接收成功或者出现错误
-// { };
-// }else{
-// if(g_com_map[MODBUS_ID1]==1)
-// {
-// #ifdef DBGMSG_OUTPUT
-// printf("发送超时");
-// #endif
-// }
-// result++;
-// }
-// if(!(status_reg& SYS_STATUS_RXFCG))
+// current_time=HAL_LPTIM_ReadCounter(&hlptim1);
+// if(current_time>end_time+((32768/1000)*3)||current_time<end_time+15000)
// {
-// dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
+//// printf("debug2 timec:%d timee:%d\r\n",current_time,end_time);
+// break;}
+ if(lp_time-uwb_timeout>2)
+ break;
+ };
+// }
// dwt_rxenable(0);
-// while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))&&!g_start_sync_flag)///不断查询芯片状态直到接收成功或者出现错误
-// { };
-// }
-
-// if (status_reg & SYS_STATUS_RXFCG)//接收成功
-// {
-//
-// dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//清楚标志位
-// frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;//数据长度
-// dwt_readrxdata(rx_buffer, frame_len, 0);//读取接收数据
-// if(rx_buffer[MESSAGE_TYPE_IDX] == MBX_PRAAREPLY)
-// {
-// u16 calCRC,recCRC,rec_commap[SENDANCPARA_LEN];
-// calCRC = CRC_Compute(&rx_buffer[MESSAGE_PARAREPLY_IDX],SENDANCPARA_LEN);
-// memcpy(&recCRC,&rx_buffer[MESSAGE_PARAREPLY_IDX+SENDANCPARA_LEN],2);
-// if(memcmp(&recCRC,&calCRC,2)==0)
-// {
-// uint8_t buffer_len,datalen,tempindex;
-// char send_buffer[200];
-// memcpy(rec_commap,&rx_buffer[MESSAGE_PARAREPLY_IDX],SENDANCPARA_LEN);
-// pwtag[remotesend_index].remain_time = 0;
-// buffer_len = sprintf(send_buffer,"$replypara_anchor,");
-// datalen = sprintf(&send_buffer[buffer_len],"%x,",pwtag[remotesend_index].id);
-// buffer_len += datalen;
-// for(uint16_t i=0;i<SENDANCPARA_LEN/2;i++)
-// {
-// datalen = sprintf(&send_buffer[buffer_len],"%d,",rec_commap[i]);
-// buffer_len += datalen;
-// }
-// send_buffer[buffer_len++] = 'e';
-// send_buffer[buffer_len++] = 'n';
-// send_buffer[buffer_len++] = 'd';
-// send_buffer[buffer_len++] = 0x0d;
-// send_buffer[buffer_len++] = 0x0a;
-// UDPClient_Uploadhex((uint8_t*)send_buffer,buffer_len);
-//
-// }
-// }
-// if(seize_anchor&&memcmp(&rx_buffer[ANCHOR_ID_IDX],&dev_id,2)) //抢占anchor 失败
-// {
-// if(g_com_map[MODBUS_ID1]==1)
-// {
-// #ifdef DBGMSG_OUTPUT
-// printf("抢占失败");
-// #endif
-// }
-// return 1;
-// }
-// if (rx_buffer[MESSAGE_TYPE_IDX] == MBX_FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,2)&&group_id==rx_buffer[GROUP_ID_IDX]) //判断是否为Final包
-// {
-//
-// resp_tx_ts = get_tx_timestamp_u64();//获得response发送时间T3
-// final_rx_ts = get_rx_timestamp_u64();//获得final接收时间T6
-// final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);//从接收数据中读取T1，T4，T5
-// final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_NEARBASE_IDX+ancrec_nearbasepos*4], &resp_rx_ts);
-// final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);
-
-// // testtime[0] = time32_incr*1000+TIM3->CNT;
-// poll_rx_ts_32 = (uint32_t)poll_rx_ts;//使用32位数据计算
-// resp_tx_ts_32 = (uint32_t)resp_tx_ts;
-// final_rx_ts_32 = (uint32_t)final_rx_ts;
-// Ra = (double)(resp_rx_ts - poll_tx_ts);//Tround1 = T4 - T1
-// Rb = (double)(final_rx_ts_32 - resp_tx_ts_32);//Tround2 = T6 - T3
-// Da = (double)(final_tx_ts - resp_rx_ts);//Treply2 = T5 - T4
-// Db = (double)(resp_tx_ts_32 - poll_rx_ts_32);//Treply1 = T3 - T2
-// tof_dtu = (int64_t)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));//计算公式
-// tof = tof_dtu * DWT_TIME_UNITS;
-// distance = tof * SPEED_OF_LIGHT;//距离=光速*飞行时间
-//#ifdef DW3000
-// dist_no_bias = distance;
-//#else
-// dist_no_bias = distance - dwt_getrangebias(config.chan, (float)distance, config.prf); //距离减去矫正系数
-//#endif
-// dist_cm = dist_no_bias * 1000; //dis 为单位为cm的距离
-//// dwt_readdiagnostics(&d1);
-//// LOS(&d1);
-// /*--------------------------以下为非测距逻辑------------------------*/
-// //dist_cm=33000;
-// // LED0_BLINK; //每成功一次通讯则闪烁一次
-//// UWBLED_BLINK;
-// uwb_losttimer = 0;
-// dis_after_filter=dist_cm;
-// hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10;
-
-// if(recpoll_len==rec_nearbase_num*4+15)
-// {
-// hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10-rec_antdelay*10;
-// }
-//
-// g_flag_Taggetdist[taglist_pos]=0;
-//
-// if(hex_dist>-100000&&hex_dist<2000000)
-// {
-// if(abs(hex_dist-his_dist[taglist_pos])<10000)
-// {
-// flag_tag_distsmooth[taglist_pos] =1;
-// }else{
-// flag_tag_distsmooth[taglist_pos] =0;
-// }
-// if(abs(hex_dist-his_dist[taglist_pos])<10000||misdist_num[taglist_pos]>3)
-// {
-// uint16_t checksum;
-// g_Resttimer=0;
-// // IWDG_Feed();
-// #ifdef TDFILTER
-// NewTrackingDiffUpdate(taglist_pos, (float)hex_dist);
-// filter_dist = pos_predict[taglist_pos]/10;
-// filter_speed = vel_predict[taglist_pos]/10;
-// newmeasure = 1;
-// #else
-// filter_dist=hex_dist/10;
-// #endif
-// misdist_num[taglist_pos]=0;
-// tagdist_list[taglist_pos] = filter_dist;
-// his_dist[taglist_pos]=hex_dist;
-// if(hex_dist>0)
-// {
-// g_Tagdist[taglist_pos]=hex_dist/10;
-// }else{
-// g_Tagdist[taglist_pos]= 0x2ffff;
-// }
-// #ifdef USART_SINGLE_OUTPUT
-// usart_send[0]=0x55;
-// usart_send[1]=0xAA;
-// usart_send[2] = 1;//正常模式
-// usart_send[3] = 17;//数据段长度
-// usart_send[4] = frame_seq_nb2;//数据段长度
-// memcpy(&usart_send[5],&tag_id_recv,2);
-// memcpy(&usart_send[7],&dev_id,2);
-// if(tagdist_list[taglist_pos]<=0)
-// tagdist_list[taglist_pos]=0x2ffff;
-// memcpy(&usart_send[9],&tagdist_list[taglist_pos],4);
-// usart_send[13] = battary;
-// usart_send[14] = button;
-// usart_send[15] = firstpath_power;
-// usart_send[16] = rx_power;
-// checksum = Checksum_u16(&usart_send[2],17);
-// memcpy(&usart_send[19],&checksum,2);
-// //HexToAsciiSendUDP(usart_send,21);
-// #ifdef DBGMSG_OUTPUT
-// if(g_com_map[MODBUS_ID1]==1)
-// {
-// printf("测距成功%d",g_Tagdist[taglist_pos]);
-// }
-// #endif
-// // PushAnchorDataArray(tag_id_recv,tagdist_list[taglist_pos],battary);
-// if(uwbled == LEDOFF )
-// {uwbled = BLUE;
-// }else{
-// uwbled = LEDOFF;
-// }
-// if(DBG_GetMode() == DBG_MODE_CFG||DBG_GetMode() == DBG_MODE_CHARGE)
-// {
-// UART_HandleTypeDef *pstUartHandle = HIDO_NULL;
-// Uart_GetHandle(UART_ID_DBG, (HIDO_VOID **)&pstUartHandle);
-// // HAL_UART_Transmit(pstUartHandle, usart_send, 21, 1000);
-// }
-// #else
-// memcpy(&usart_send_anc[4+6*anc_report_num],&tag_id_recv,2);
-// memcpy(&usart_send_anc[6+6*anc_report_num],&tagdist_list[taglist_pos],4);
-// anc_report_num++;
-// #endif
-// }else{
-// if(g_com_map[MODBUS_ID1]==1)
-// {
-// #ifdef DBGMSG_OUTPUT
-// printf("测距错误");
-// #endif
-// }
-//
-// // printf("%d",hex_dist);
-// misdist_num[taglist_pos]++;
-// }
-// }
-// if(remotesend_state == 1)
-// {
-// uint8_t buffer_len,datalen,tempindex;
-// char send_buffer[100];
-// pwtag[remotesend_index].remain_time = 0;
-// if(pwtag[remotesend_index].index==ANCHOR_UPDATE)
-// {
-// if(pwtag[remotesend_index].value == 2)
-// {
-// StartAnchorUpdateProcess(pwtag[remotesend_index].id);
-// }
-// }
-// buffer_len = sprintf(send_buffer,"$confirmwrite_anchor,");
-// datalen = sprintf(&send_buffer[buffer_len],"%x,1",pwtag[remotesend_index].id);
-// buffer_len += datalen;
-// send_buffer[buffer_len++] = 0x0d;
-// send_buffer[buffer_len++] = 0x0a;
-// UDPClient_Uploadhex((uint8_t*)send_buffer,buffer_len);
-// }
-// }
-// }else{
-// if(g_com_map[MODBUS_ID1]==1)
-// {
-// #ifdef DBGMSG_OUTPUT
-// printf("final状态错误:%x",status_reg);
-// #endif
-// }
-//
-// //printf("%x/n",status_reg);
-// dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
-// }
-// }
}
+
uint16_t target_tagid[12] = {0x1316,0x1394,0x1265,0x1318,0x1306,0x1304,0x1376,0x1312,0x1277,0x1388,0x1140,0x1392};
+uint16_t Lora_tx_ancid[50];
+uint16_t Lora_tx_ancdist[50];
+uint8_t Lora_tx_anc_electricity[50];
+uint8_t Lora_tx_ancnum=0;
+uint8_t break_flag ;
+uint16_t RX_TIMEOUTs_UWB;
void Anchor_App(void)
{
-
- u8 send_len,i;
- u16 tempid;
- uint32_t rec_syncid;
- uint16_t checksum;
-
+
+ u8 send_len,i;
+ u16 tempid;
+ uint32_t rec_syncid;
+ uint16_t checksum;
+ uint8_t kk;
+
+ dwt_setrxtimeout(0);//设定接收超时时间，0位没有超时时间
+ dwt_rxenable(0);//打开接收
+
+
+
start_time=HAL_LPTIM_ReadCounter(&hlptim1);
- end_time=start_time+((32768/1000)*20);
- if(end_time>=30000)
- {end_time-=30000;}
- current_time=HAL_LPTIM_ReadCounter(&hlptim1);
+ end_time=start_time+((32768/1000)*2);
+ if(end_time>=32768)
+ {
+ end_time-=32768;
+ }
+ current_time=HAL_LPTIM_ReadCounter(&hlptim1);
while(current_time<end_time||current_time>end_time+15000)
+ {
+ current_time=HAL_LPTIM_ReadCounter(&hlptim1);
+ while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到接收成功或者出现错误
{
current_time=HAL_LPTIM_ReadCounter(&hlptim1);
- dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR| SYS_STATUS_TXFRS |SYS_STATUS_RXFCG);
-// g_start_sync_flag=0;
+ if(current_time>=end_time&¤t_time<end_time+15000)
+ break;
+ };
- dwt_setrxtimeout(0);//设定接收超时时间，0位没有超时时间
- dwt_rxenable(0);//打开接收
+ if (status_reg & SYS_STATUS_RXFCG && status_reg!=0xffffffff)//成功接收
+ {
+ uint16_t tag_recv_interval;
+ float temp_tagpos;
-// GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_SET);
- uwb_timeout=lp_time;
- while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到接收成功或者出现错误
- {
-// current_time=HAL_LPTIM_ReadCounter(&hlptim1);
-// if(current_time>end_time||current_time<end_time+15000)
-// break;
-// IdleTask();
- if(lp_time-uwb_timeout>2)
- break;
- };
-//GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_RESET);
- // delay_us(g_com_map[NEARBASE_ID2]);
-
-// status_reg = dwt_read32bitreg(SYS_STATUS_ID);
-// dwt_setinterrupt( DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO | DWT_INT_RXPTO), 0);
- if (status_reg & SYS_STATUS_RXFCG)//成功接收
- { uint16_t tag_recv_interval;
- float temp_tagpos;
- dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//清除标志位
-
- frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//获得接收数据长度
- recpoll_len = frame_len;
- dwt_readrxdata(rx_buffer, frame_len, 0);//读取接收数据
- memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
- //将收到的tag_id分别写入各次通讯的包中，为多标签通讯服务，防止一次通讯中接收到不同ID标签的数据
- memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
- switch(rx_buffer[MESSAGE_TYPE_IDX])
- {uint8_t target_tagi;
+ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
+ frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//获得接收数据长度
+ recpoll_len = frame_len;
+ dwt_readrxdata(rx_buffer, frame_len, 0);//读取接收数据
+ dwt_setrxtimeout(0);//DELAY_BETWEEN_TWO_FRAME_UUS*(nearbase_num+1-recbase_num)+10);//设定接收超时时间，0位没有超时时间
+ dwt_rxenable(0);
+ memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
+ //将收到的tag_id分别写入各次通讯的包中，为多标签通讯服务，防止一次通讯中接收到不同ID标签的数据
+ memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
+
+// HAL_GPIO_WritePin(GPIOA, GPIO_PIN_15, GPIO_PIN_SET);
+// printf("ID %x\r\n",tag_id_recv);
+ switch(rx_buffer[MESSAGE_TYPE_IDX])
+ { uint8_t target_tagi;
case MBX_REG:
// dwt_readdiagnostics(&d1);
// LOS(&d1);
@@ -585,108 +319,123 @@
// memcpy(&usart_send[22],&checksum,2);
// HexToAsciiSendUDP(usart_send,24);
break;
- case MBX_POLL:
- if(group_id==rx_buffer[GROUP_ID_IDX])
- {
- dwt_readdiagnostics(&d1);
- LOS(&d1);
- if(firstpath_power<-100)
+ case MBX_POLL:
+ if(group_id==rx_buffer[GROUP_ID_IDX])
{
- break;
- }
- frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
- battary = rx_buffer[BATTARY_IDX];
- button = rx_buffer[BUTTON_IDX];
- rec_nearbase_num=rx_buffer[FZ_NEARBASENUM_INDEX];
- memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
- memcpy(&rec_ancidlist,&rx_buffer[FZ_NEARBASEID_INDEX],2*rec_nearbase_num);
- memcpy(&rec_ancdistlist,&rx_buffer[FZ_NEARBASEID_INDEX+2*rec_nearbase_num],2*rec_nearbase_num);
- memcpy(&rec_antdelay,&rx_buffer[FZ_NEARBASEID_INDEX+rec_nearbase_num*4],2);
- uwb_losttimer = 0;
-// wltag_state=RANGE;
- for(u8 i=0;i<rec_nearbase_num;i++)
- {
- if(rec_ancidlist[i] == dev_id)
+// printf("timec:%d timee:%d\r\n",current_time,end_time);
+ dwt_readdiagnostics(&d1);
+ LOS(&d1);
+ if(firstpath_power<-100)
{
-// PushAnchorDataArray(tag_id_recv,rec_ancdistlist[i],-(int8_t)firstpath_power);
-
- // PushAnchorDataArray(tag_id_recv,rec_ancdistlist[i],battary);
+ break;
+ }
+ frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
+ battary = rx_buffer[BATTARY_IDX];
+ button = rx_buffer[BUTTON_IDX];
+ rec_nearbase_num=rx_buffer[FZ_NEARBASENUM_INDEX];
+ memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
+ memcpy(&rec_ancidlist,&rx_buffer[FZ_NEARBASEID_INDEX],2*rec_nearbase_num);
+ memcpy(&rec_ancdistlist,&rx_buffer[FZ_NEARBASEID_INDEX+2*rec_nearbase_num],2*rec_nearbase_num);
+ memcpy(&rec_antdelay,&rx_buffer[FZ_NEARBASEID_INDEX+rec_nearbase_num*4],2);
+ uwb_losttimer = 0;
+// wltag_state=RANGE;
+ for(u8 i=0; i<rec_nearbase_num; i++)
+ {
+ if(rec_ancidlist[i] == dev_id)
+ {
+ for(kk=0; kk<Lora_tx_ancnum; kk++)
+ {
+ if(Lora_tx_ancid[kk]==tag_id_recv)
+ {
+// Lora_tx_ancnum--;
+ Lora_tx_ancdist[kk]=rec_ancdistlist[i];
+ Lora_tx_anc_electricity[kk]=battary;
+ break_flag=1;
+ break;
+ }
+ }
+ if(break_flag)
+ {
+ break_flag=0;
+ break;
+ }
+ Lora_tx_ancid[Lora_tx_ancnum]=tag_id_recv;
+ Lora_tx_ancdist[Lora_tx_ancnum]=rec_ancdistlist[i];
+ Lora_tx_anc_electricity[Lora_tx_ancnum]=battary;
+// printf("接收到uwb测距:%d",Lora_tx_ancdist[0]);
+ Lora_tx_ancnum++;
+ RX_TIMEOUTs_UWB++;
+ }
+ }
+//// #ifdef USART_ALL_OUTPUT
+// usart_send[2] = 0x12;//正常模式
+// usart_send[3] = 8+rec_nearbase_num*4;//数据段长度
+// memcpy(&usart_send[4],&tag_id_recv,2);
+// usart_send[6] = frame_seq_nb2;//包序
+// power_and_key=battary<<1;//把按键跟电量2合1，发出去
+// power_and_key=power_and_key+button;
+// memcpy(usart_send+7,&power_and_key,1);
+// usart_send[8] = lora_yingda_flag;//lora等待应答位
+// usart_send[9] = rec_nearbase_num;//lora等待应答位
+//// usart_send[9] = lora_yingda_flag;
+// memcpy(&usart_send[10],&rec_ancidlist,2*rec_nearbase_num);
+// memcpy(&usart_send[10+rec_nearbase_num*2],&rec_ancdistlist,2*rec_nearbase_num);
+// checksum = Checksum_u16(&usart_send[2],8+4*rec_nearbase_num);
+// memcpy(&usart_send[10+4*rec_nearbase_num],&checksum,2);
+//// UART_PushFrame(usart_send,12+rec_nearbase_num*4);
+//// #endif
+
+ taglist_pos=CmpTagInList(tag_id_recv);
+ if(taglist_num>=TAG_NUM_IN_SYS)
+ break;
+ if(taglist_pos==taglist_num)
+ {
+ taglist_pos=taglist_num;
+ tagid_list[taglist_num++]=tag_id_recv;
+
+ new_tagid=1;
+ } else {
+ new_tagid=0;
+ }
+ temp_tagpos=round((float)(sync_timer%g_com_map[COM_INTERVAL])/slottime);
+ tagpos_rec[(uint8_t)temp_tagpos]=1;
+
+
+ if(rec_nearbase_num>ancidlist_num)
+ {
+ ancidlist_num=rec_nearbase_num;
+ memcpy(ancidlist_rec,&rx_buffer[FZ_NEARBASEID_INDEX],rec_nearbase_num*2);
+ }
+
+ for(i=0; i<rec_nearbase_num; i++)
+ {
+ memcpy(&tempid,&rx_buffer[FZ_NEARBASEID_INDEX+i*2],2);
+ if(tempid==dev_id)
+ {
+ seize_anchor=0; //非抢占。已存在列表中
+ Anchor_RecNearPoll(i);
+ break;
+ }
+ }
+
+ if(i==rec_nearbase_num)
+ {
+ seize_anchor=1; //抢占anchor
+ Anchor_RecNearPoll(i);
}
}
-// #ifdef USART_ALL_OUTPUT
- usart_send[2] = 0x0c;//正常模式
- usart_send[3] = 8+rec_nearbase_num*4;//数据段长度
- usart_send[4] = frame_seq_nb2;//数据段长度
- usart_send[5] = battary;
- usart_send[6] = button;
- usart_send[7] = rec_nearbase_num;
- memcpy(&usart_send[8],&tag_id_recv,2);
- memcpy(&usart_send[10],&rec_ancidlist,2*rec_nearbase_num);
- memcpy(&usart_send[10+rec_nearbase_num*2],&rec_ancdistlist,2*rec_nearbase_num);
- checksum = Checksum_u16(&usart_send[2],8+4*rec_nearbase_num);
- memcpy(&usart_send[10+4*rec_nearbase_num],&checksum,2);
-// UART_PushFrame(usart_send,12+rec_nearbase_num*4);
-// #endif
- taglist_pos=CmpTagInList(tag_id_recv);
- if(taglist_num>=TAG_NUM_IN_SYS)
- break;
- if(taglist_pos==taglist_num)
- {
- taglist_pos=taglist_num;
- tagid_list[taglist_num++]=tag_id_recv;
-
- new_tagid=1;
- }else{
- new_tagid=0;
- }
- temp_tagpos=round((float)(sync_timer%g_com_map[COM_INTERVAL])/slottime);
- tagpos_rec[(uint8_t)temp_tagpos]=1;
-
-
- if(rec_nearbase_num>ancidlist_num)
- {
- ancidlist_num=rec_nearbase_num;
- memcpy(ancidlist_rec,&rx_buffer[FZ_NEARBASEID_INDEX],rec_nearbase_num*2);
- }
-
- for(i=0;i<rec_nearbase_num;i++)
- {
- memcpy(&tempid,&rx_buffer[FZ_NEARBASEID_INDEX+i*2],2);
- if(tempid==dev_id)
- {
- seize_anchor=0; //非抢占。已存在列表中
- Anchor_RecNearPoll(i);
- break;
- }
- }
- if(i==rec_nearbase_num)
- {
- seize_anchor=1; //抢占anchor
- Anchor_RecNearPoll(i);
- }
- }
-
- break;
- }
-
- }else{
-// if(g_com_map[MODBUS_ID1]==1)
-// {
-// if(g_com_map[MODBUS_ID1]==1)
-// {
-// #ifdef DBGMSG_OUTPUT
-// if(g_com_map[MODBUS_ID1]==1)
-// {
-// printf("POLL状态错误:%x",status_reg);
-// }
-// #endif
-// }
-// }
- dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
- }
-
+ break;
+ }
+
+ } else {
+// printf("debug4 timec:%d timee:%d\r\n",current_time,end_time);
+ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
+ dwt_rxenable(0);
+ }
}
- dwt_forcetrxoff();
+ dwt_forcetrxoff();
+ dwt_entersleep();
- }
\ No newline at end of file
+}
+
--
Gitblit v1.9.3