From ae42ba04bfca83ddca9df9d41061a365d4f57618 Mon Sep 17 00:00:00 2001
From: WXK <287788329@qq.com>
Date: 星期五, 26 一月 2024 17:08:05 +0800
Subject: [PATCH] 加入GPS模块。加入低功耗串口
---
Src/application/dw_ancapp.c | 461 +++++++++++++--------------------------------------------
1 files changed, 104 insertions(+), 357 deletions(-)
diff --git a/Src/application/dw_ancapp.c b/Src/application/dw_ancapp.c
index f48f1d7..3b0a160 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;
@@ -178,6 +180,7 @@
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;
@@ -193,47 +196,22 @@
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。
+ 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。
}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。
+ 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);//接收超时时间
+ //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);
- 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;
// send_buffer[MAINBASE_INDEX]=flag_syncbase;
send_buffer[MESSAGE_TYPE_IDX]=MBX_RESPONSE;
@@ -241,36 +219,7 @@
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_forcetrxoff();
dwt_writetxdata(38, send_buffer, 0);//写入发送数据
dwt_writetxfctrl(38, 0);//设定发送长度
result = dwt_starttx(DWT_START_TX_DELAYED);
@@ -281,245 +230,31 @@
// 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 )))///不断查询芯片状态直到接收成功或者出现错误
+ 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;
+
+// current_time=HAL_LPTIM_ReadCounter(&hlptim1);
+// if(current_time>end_time+((32768/1000)*3)||current_time<end_time+15000)
+// {
+//// printf("debug2 timec:%d timee:%d\r\n",current_time,end_time);
+// break;}
if(lp_time-uwb_timeout>2)
break;
};
-// }else{
-// result = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//延迟发送，等待接收
-
-
-// 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))
-// {
-// dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
+// }
// 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)
{
@@ -527,48 +262,45 @@
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;}
+ 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);
- dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR| SYS_STATUS_TXFRS |SYS_STATUS_RXFCG);
-// g_start_sync_flag=0;
-
- dwt_setrxtimeout(0);//设定接收超时时间，0位没有超时时间
- dwt_rxenable(0);//打开接收
-
-// 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]);
+ 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&¤t_time<end_time+15000)
+ break;
+ };
-// 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);//清除标志位
-
+ if (status_reg & SYS_STATUS_RXFCG && status_reg!=0xffffffff)//成功接收
+ {
+ uint16_t tag_recv_interval;
+ float temp_tagpos;
+
+ 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);
+ 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:
@@ -587,7 +319,8 @@
break;
case MBX_POLL:
if(group_id==rx_buffer[GROUP_ID_IDX])
- {
+ {
+// printf("timec:%d timee:%d\r\n",current_time,end_time);
dwt_readdiagnostics(&d1);
LOS(&d1);
if(firstpath_power<-100)
@@ -607,26 +340,48 @@
for(u8 i=0;i<rec_nearbase_num;i++)
{
if(rec_ancidlist[i] == dev_id)
- {
-// PushAnchorDataArray(tag_id_recv,rec_ancdistlist[i],-(int8_t)firstpath_power);
-
- // PushAnchorDataArray(tag_id_recv,rec_ancdistlist[i],battary);
- }
+ {
+ 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] = 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
+//// #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)
@@ -660,6 +415,7 @@
break;
}
}
+
if(i==rec_nearbase_num)
{
seize_anchor=1; //抢占anchor
@@ -671,22 +427,13 @@
}
}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
-// }
-// }
+// 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_entersleep();
- }
\ No newline at end of file
+}
+
--
Gitblit v1.9.3