#include "dw_app.h"
|
#include "ADC.h"
|
enum enumtagstate
|
{
|
DISCPOLL,
|
GETNEARMSG,
|
NEARPOLL,
|
}tag_state=GETNEARMSG;
|
static dwt_config_t config = {
|
2, /* Channel number. */
|
DWT_PRF_64M, /* Pulse repetition frequency. */
|
DWT_PLEN_64, /* Preamble length. */
|
DWT_PAC8, /* Preamble acquisition chunk size. Used in RX only. */
|
9, /* TX preamble code. Used in TX only. */
|
9, /* RX preamble code. Used in RX only. */
|
1, /* Use non-standard SFD (Boolean) */
|
DWT_BR_6M8, /* Data rate. */
|
DWT_PHRMODE_STD, /* PHY header mode. */
|
(129 + 8 - 8) /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
|
};
|
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_nearpoll_msg[80] = {0};
|
static uint8_t tx_nearresp_msg[80] = {0};
|
static uint8_t tx_nearfinal_msg[80] = {0};
|
|
static uint8_t tx_near_msg[80] = {0};
|
|
uint32_t frame_seq_nb = 0, frame_seq_nb2 = 0;
|
static uint32_t status_reg = 0;
|
static uint8_t rx_buffer[100];
|
static uint64_t poll_rx_ts;
|
static uint64_t resp_tx_ts;
|
static uint64_t final_rx_ts;
|
static double tof;
|
int32_t anchor_dist_last_frm[TAG_NUM_IN_SYS],his_dist[TAG_NUM_IN_SYS];
|
uint32_t tag_id = 0;
|
uint32_t tag_id_recv = 0;
|
uint32_t anc_id_recv = 0;
|
uint8_t random_delay_tim = 0;
|
double distance, dist_no_bias, dist_cm;
|
uint32_t g_UWB_com_interval = 0;
|
float dis_after_filter; //µ±Ç°¾àÀëÖµ
|
LPFilter_Frac* p_Dis_Filter; //²â¾àÓõĵÍͨÂ˲¨Æ÷
|
int32_t g_Tagdist[TAG_NUM_IN_SYS];
|
uint8_t g_flag_Taggetdist[256];
|
|
static uint64_t get_tx_timestamp_u64(void)
|
{
|
uint8_t ts_tab[5];
|
uint64_t ts = 0;
|
int i;
|
dwt_readtxtimestamp(ts_tab);
|
for (i = 4; i >= 0; i--)
|
{
|
ts <<= 8;
|
ts |= ts_tab[i];
|
}
|
return ts;
|
}
|
|
static uint64_t get_rx_timestamp_u64(void)
|
{
|
uint8_t ts_tab[5];
|
uint64_t ts = 0;
|
int i;
|
dwt_readrxtimestamp(ts_tab);
|
for (i = 4; i >= 0; i--)
|
{
|
ts <<= 8;
|
ts |= ts_tab[i];
|
}
|
return ts;
|
}
|
|
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;
|
}
|
}
|
|
static void final_msg_get_ts(const uint8_t *ts_field, uint32_t *ts)
|
{
|
int i;
|
*ts = 0;
|
for (i = 0; i < FINAL_MSG_TS_LEN; i++)
|
{
|
*ts += ts_field[i] << (i * 8);
|
}
|
}
|
extern int32_t tagdist_list[TAG_NUM_IN_SYS];
|
void TagDistClear(void)
|
{
|
static uint16_t clear_judge_cnt;
|
uint16_t i;
|
for(i=0;i<255;i++)
|
{
|
g_flag_Taggetdist[i]++;
|
if(g_flag_Taggetdist[i]>=2)
|
{
|
tagdist_list[i]=0x1ffff;
|
}
|
}
|
}
|
void Dw1000_Init(void)
|
{
|
/* Reset and initialise DW1000.
|
* For initialisation, DW1000 clocks must be temporarily set to crystal speed. After initialisation SPI rate can be increased for optimum
|
* performance. */
|
Reset_DW1000();//ÖØÆôDW1000 /* Target specific drive of RSTn line into DW1000 low for a period. */
|
status_reg = dwt_read32bitreg(SYS_STATUS_ID);
|
dwt_initialise(DWT_LOADUCODE);//³õʼ»¯DW1000
|
status_reg = dwt_read32bitreg(SYS_STATUS_ID);
|
Spi_ChangePrescaler(SPIx_PRESCALER_FAST); //ÉèÖÃΪ¿ìËÙģʽ
|
|
/* Configure DW1000. See NOTE 6 below. */
|
dwt_configure(&config);//ÅäÖÃDW1000
|
|
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), 1);
|
|
/* Apply default antenna delay value. See NOTE 1 below. */
|
dwt_setrxantennadelay(RX_ANT_DLY); //ÉèÖýÓÊÕÌìÏßÑÓ³Ù
|
dwt_settxantennadelay(TX_ANT_DLY); //ÉèÖ÷¢ÉäÌìÏßÑÓ³Ù
|
|
/* Set expected response's delay and timeout. See NOTE 4 and 5 below.
|
* As this example only handles one incoming frame with always the same delay and timeout, those values can be set here once for all. */
|
//ÉèÖýÓÊÕ³¬Ê±Ê±¼ä
|
}
|
void Dw1000_App_Init(void)
|
{
|
//g_com_map[DEV_ID] = 0x0b;
|
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[TAG_ID_IDX], &dev_id, 2);
|
memcpy(&tx_final_msg[TAG_ID_IDX], &dev_id, 2);
|
memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &dev_id, 2);
|
memcpy(&tx_sync_msg[ANCHOR_ID_IDX], &dev_id, 2);
|
memcpy(&tx_nearresp_msg[ANCHOR_ID_IDX], &dev_id, 2);
|
memcpy(&tx_nearpoll_msg[TAG_ID_IDX], &dev_id, 2);
|
memcpy(&tx_nearfinal_msg[TAG_ID_IDX], &dev_id, 2);
|
}
|
uint16_t Checksum_u16(uint8_t* pdata, uint32_t len)
|
{
|
uint16_t sum = 0;
|
uint32_t i;
|
for(i=0; i<len; i++)
|
sum += pdata[i];
|
sum = ~sum;
|
return sum;
|
}
|
|
u16 tag_time_recv[TAG_NUM_IN_SYS];
|
u8 usart_send[150],usart_send_anc[100];
|
u8 battary,button,tag_frequency,tag_slotpos;
|
extern uint8_t g_pairstart;
|
void tag_sleep_configuraion(void)
|
{
|
dwt_configuresleep(0x940, 0x7);
|
dwt_entersleep();
|
}
|
|
extern uint8_t g_start_send_flag;
|
u8 g_start_sync_flag;
|
void SyncPoll(u8 sync_seq)
|
{u8 result;
|
// g_start_sync_flag=1; //ÖжÏģʽ£¬Í˳öÖն˺ó£¬ÐèÒªÖØÐÂÀ´¹ý
|
dwt_forcetrxoff(); //¹Ø±Õ½ÓÊÕ£¬ÒÔ·ÀÔÚRX ON ״̬
|
|
|
tx_sync_msg[SYNC_SEQ_IDX]=sync_seq;
|
memcpy(&tx_sync_msg[ANCHOR_ID_IDX],&dev_id,4);
|
dwt_writetxdata(sizeof(tx_sync_msg), tx_sync_msg, 0);//½«Poll°üÊý¾Ý´«¸øDW1000£¬½«ÔÚ¿ªÆô·¢ËÍʱ´«³öÈ¥
|
dwt_writetxfctrl(sizeof(tx_sync_msg), 0);//ÉèÖó¬¿í´ø·¢ËÍÊý¾Ý³¤¶È
|
dwt_starttx(DWT_START_TX_IMMEDIATE);
|
if(result==0)
|
{
|
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//²»¶Ï²éѯоƬ״ֱ̬µ½·¢ËÍÍê³É
|
{ };
|
}
|
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//Çå³þ±ê־λ
|
|
dwt_setrxtimeout(0);//É趨½ÓÊÕ³¬Ê±Ê±¼ä£¬0λûÓг¬Ê±Ê±¼ä
|
dwt_rxenable(0);//´ò¿ª½ÓÊÕ
|
}
|
uint16_t g_Resttimer;
|
uint8_t result;
|
u8 tag_succ_times=0;
|
int32_t hex_dist;
|
u16 checksum;
|
int8_t tag_delaytime;
|
extern uint16_t sync_timer;
|
u16 tmp_time;
|
int32_t temp_dist;
|
u16 tagslotpos;
|
|
u16 anclist_num=0,anclist_pos; //list ×ÜÊýÁ¿ºÍµ±Ç°Î»ÖÃ
|
u16 ancid_list[TAG_NUM_IN_SYS];
|
u8 nearbase_num=0,last_nearbase_num,next_nearbase_num;
|
u16 nearbaseid_list[MAX_NEARBASE_NUM],mainbase_id,true_nearbase_idlist[MAX_NEARBASE_NUM];
|
int32_t mainbase_dist,nearbase_distlist[MAX_NEARBASE_NUM],true_nearbase_distlist[MAX_NEARBASE_NUM],true_exsistbase_list[MAX_NEARBASE_NUM];
|
uint8_t trygetnearmsg_times;
|
u16 current_slotnum,total_slotnum,target_time,last_time;
|
u32 rec_tagpos_binary;
|
extern uint32_t tagpos_binary;
|
void SetNextPollTime(u16 time)
|
{
|
current_slotnum++;
|
if(current_slotnum==total_slotnum)
|
current_slotnum-=total_slotnum;
|
//time=5;
|
target_time=((current_slotnum*g_com_map[COM_INTERVAL])+time);
|
|
last_time=target_time;
|
if(target_time>=990&&target_time<992)
|
{
|
current_slotnum++;
|
target_time+=g_com_map[COM_INTERVAL];
|
tagslotpos=GetRandomSlotPos(rec_tagpos_binary|tagpos_binary);
|
tyncpoll_time = (tagslotpos--%max_slotpos)*slottime;
|
}
|
if(target_time>=1000)
|
{target_time-=1000;}
|
if(target_time<0)
|
{target_time+=1000;}
|
|
}
|
u8 FindNearBasePos(u16 baseid)
|
{
|
u8 i;
|
for(i=0;i<last_nearbase_num;i++)
|
{
|
if(baseid==nearbaseid_list[i])
|
return i;
|
}
|
return i;
|
}
|
u8 recbase_num=0;
|
#define CHANGE_BASE_THRESHOLD 5
|
//void NearAncSelect(void)
|
//{static u16 last_mainbase_id,change_base_count;
|
// int32_t nearbase_mindist=99999, nearbase_minpos;
|
// u8 i;
|
// for(i=0;i<recbase_num-1;i++)
|
// {
|
// if(nearbase_mindist>nearbase_distlist[i])
|
// {
|
// nearbase_mindist=nearbase_distlist[i];
|
// nearbase_minpos=i;
|
// }
|
// }
|
// if(nearbase_mindist<mainbase_dist-THRESHOLD_CHANGE_MAINBASE_DIST)
|
// {
|
// if(last_mainbase_id==nearbaseid_list[nearbase_minpos])
|
// {
|
// change_base_count++;
|
// if(change_base_count>CHANGE_BASE_THRESHOLD)
|
// {
|
// mainbase_id=last_mainbase_id;
|
// tag_state=GETNEARMSG;
|
// }
|
// }else{
|
// change_base_count=0;
|
// }
|
// last_mainbase_id=nearbaseid_list[nearbase_minpos];
|
// }else{
|
// change_base_count=0;
|
// }
|
//}
|
uint8_t GetRandomValue(void)
|
{
|
uint8_t random_value=0,temp_adc,i;
|
for(i=0;i<8;i++)
|
{
|
temp_adc=Get_ADC_Value();
|
random_value=random_value|((temp_adc&0x01)<<i);
|
}
|
return random_value;
|
}
|
u8 GetRandomSlotPos(uint32_t emptyslot)
|
{
|
u8 i,temp_value;
|
temp_value = GetRandomValue();
|
for(i=temp_value%max_slotpos;i<max_slotpos;i++)
|
{
|
if(((emptyslot>>i)&0x1)==0)
|
{
|
return i;
|
}
|
}
|
|
for(i=1;i<max_slotpos;i++)
|
{
|
if(((emptyslot>>i)&0x1)==0)
|
{
|
return i;
|
}
|
}
|
return max_slotpos-1;
|
}
|
dwt_rxdiag_t d1;
|
u8 test=0;
|
u8 anclost_times=0;
|
u8 exsistbase_list[MAX_NEARBASE_NUM],report_num,get_newbase=0;
|
u16 temp_sync_timer1,temp_sync_timer2;
|
double firstpath_power, rx_power;
|
double f1, f2, r1, r2;
|
uint16_t F1,F2,F3,N,C;
|
double B = 131072;
|
double A = 121.74;
|
double min_power;
|
double LOS(dwt_rxdiag_t *dia) {
|
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));
|
return min_power;
|
}
|
|
u8 nearmsg_mainbase=0,rec_tagpos_emptylist[32];
|
|
int8_t correction_time,new_tagid=0;
|
extern uint8_t sync_seq;
|
u16 taglist_num=0,taglist_pos;
|
u16 tagid_list[TAG_NUM_IN_SYS];
|
u8 tagofflinetime[TAG_NUM_IN_SYS];
|
int32_t tagdist_list[TAG_NUM_IN_SYS];
|
void TagListUpdate(void)
|
{
|
u16 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;
|
}
|
u16 CmpTagInList(u16 tagid)
|
{u16 i;
|
for(i=0;i<taglist_num;i++)
|
{
|
if(memcmp(&tagid,&tagid_list[i],2)==0)
|
break;
|
}
|
return i;
|
}
|
u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,waittagconfig_reponse,motorstate=0;
|
uint32_t frame_len;
|
uint32_t resp_tx_time;
|
uint8_t rec_nearbase_num,anc_report_num;
|
void Anchor_RecPoll(void)
|
{
|
tmp_time=TIM3->CNT;
|
memcpy(&tx_resp_msg[ANCTIMEMS],&sync_timer,2);
|
memcpy(&tx_resp_msg[ANCTIMEUS],&tmp_time,2);
|
memcpy(&tx_resp_msg[TAGSLOTPOS],&taglist_pos,1);
|
|
poll_rx_ts = get_rx_timestamp_u64();//»ñµÃPoll°ü½ÓÊÕʱ¼äT2
|
|
resp_tx_time = (poll_rx_ts + (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//¼ÆËãResponse·¢ËÍʱ¼äT3¡£
|
dwt_setdelayedtrxtime(resp_tx_time);//ÉèÖÃResponse·¢ËÍʱ¼äT3
|
dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);//ÉèÖ÷¢ËÍÍê³Éºó¿ªÆô½ÓÊÕÑÓ³Ùʱ¼ä
|
dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//½ÓÊÕ³¬Ê±Ê±¼ä
|
|
memcpy(&tx_resp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
|
memcpy(&tx_resp_msg[TAG_ID_IDX],&tag_id_recv,2);
|
|
dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);//дÈë·¢ËÍÊý¾Ý
|
dwt_writetxfctrl(sizeof(tx_resp_msg), 0);//É趨·¢Ëͳ¤¶È
|
result = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//ÑÓ³Ù·¢ËÍ£¬µÈ´ý½ÓÊÕ
|
|
|
frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
|
if(result==0)
|
{
|
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))///²»¶Ï²éѯоƬ״ֱ̬µ½½ÓÊճɹ¦»òÕß³öÏÖ´íÎó
|
{ };
|
}
|
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] == FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,2)) //ÅжÏÊÇ·ñΪFinal°ü
|
{
|
uint32_t poll_tx_ts, resp_rx_ts, final_tx_ts;
|
uint32_t poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
|
double Ra, Rb, Da, Db;
|
int64_t tof_dtu;
|
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_TS_IDX], &resp_rx_ts);
|
final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);
|
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;//¾àÀë=¹âËÙ*·ÉÐÐʱ¼ä
|
dist_no_bias = distance - dwt_getrangebias(config.chan, (float)distance, config.prf); //¾àÀë¼õÈ¥½ÃÕýϵÊý
|
dist_cm = dist_no_bias * 100; //dis Ϊµ¥Î»ÎªcmµÄ¾àÀë
|
/*--------------------------ÒÔÏÂΪ·Ç²â¾àÂß¼------------------------*/
|
//dist_cm=33000;
|
|
LED0_BLINK; //ÿ³É¹¦Ò»´ÎͨѶÔòÉÁ˸һ´Î
|
dis_after_filter=dist_cm;
|
hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
|
if(hex_dist>-1000&&hex_dist<200000)
|
{
|
g_flag_Taggetdist[taglist_pos]=0;
|
if(abs(hex_dist-his_dist[taglist_pos])<1500||misdist_num[taglist_pos]>3)
|
{
|
misdist_num[taglist_pos]=0;
|
tagdist_list[taglist_pos] = hex_dist;
|
his_dist[taglist_pos]=hex_dist;
|
g_Tagdist[taglist_pos]=hex_dist;
|
#ifndef USART_INTEGRATE_OUTPUT
|
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);
|
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-firstpath_power)*10;
|
usart_send[17] = tag_frequency;
|
usart_send[18] = tag_slotpos;
|
checksum = Checksum_u16(&usart_send[2],17);
|
memcpy(&usart_send[19],&checksum,2);
|
UART_PushFrame(usart_send,21);
|
#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
|
#ifdef DEBUG_INF
|
printf("ÐòºÅ£º%d\r\n",frame_seq_nb2);
|
#endif
|
}else{
|
// printf("%d",hex_dist);
|
misdist_num[taglist_pos]++;
|
}
|
}
|
}
|
}else{
|
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
|
}
|
}
|
uint32_t time_monitor[10];
|
extern uint16_t configremotetagID;
|
extern u8 remotetag_paralen;
|
u8 finalsend=0;
|
u16 signalpower_list[TAG_NUM_IN_SYS];
|
uint32_t poll_tx_ts, resp_rx_ts, final_tx_ts;
|
uint32_t poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
|
double Ra, Rb, Da, Db;
|
int64_t tof_dtu;
|
u32 errortimes,goodtimes;
|
u8 tagpossend_flag;
|
u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase 1 first near_base
|
{
|
|
tmp_time=TIM3->CNT;
|
memcpy(&tx_nearresp_msg[NR_NEARSWITCH_DISTANCE],&g_com_map[NEARSWITCH_DISTANCE_INDEX],2);
|
memcpy(&tx_nearresp_msg[ANCTIMEMS],&sync_timer,2);
|
memcpy(&tx_nearresp_msg[ANCTIMEUS],&tmp_time,2);
|
memcpy(&tx_nearresp_msg[TAGSLOTPOS],&taglist_pos,2);
|
memcpy(&tx_nearresp_msg[TAG_ID_IDX],&tag_id_recv,2);
|
poll_rx_ts = get_rx_timestamp_u64();//»ñµÃPoll°ü½ÓÊÕʱ¼äT2
|
|
resp_tx_time = (poll_rx_ts + ((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_readdiagnostics(&d1);
|
if(tagpos[taglist_pos].tagid==tag_id_recv)
|
{
|
memcpy(&tx_nearresp_msg[TAGPOS_INDEX],&tagpos[taglist_pos],tagpos[taglist_pos].datalen+3);
|
tagpossend_flag = 1;
|
}else{
|
tagpossend_flag = 0;
|
}
|
if(new_tagid)
|
{
|
tagdist_list[taglist_pos]=0x1ffff;
|
}
|
memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
|
memcpy(&tx_nearresp_msg[ANC_SIGNALPOWER],&signalpower_list[taglist_pos],2);
|
tx_nearresp_msg[MAINBASE_INDEX]=flag_syncbase;
|
tx_nearresp_msg[MESSAGE_TYPE_IDX]=NEAR_RESPONSE;
|
tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)|motorstate;
|
waittagconfig_reponse=0;
|
finalsend = 0;
|
if(pwtag.remain_time>0)
|
{uint8_t i;
|
for(i=0;i<pwtag.groupnum;i++)
|
{
|
if(pwtag.groupid[i][0]<=tag_id_recv&&tag_id_recv<=pwtag.groupid[i][1])
|
{
|
remotesend_state=1;
|
tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)|motorstate;
|
remotetag_paralen = 2+3;
|
remotetag_para[0] = 2;
|
remotetag_para[1] = pwtag.index;
|
remotetag_para[2] = 2;
|
memcpy(&remotetag_para[3],&pwtag.group_interval[i],2);
|
memcpy(&tx_nearresp_msg[REMOTEPARA_INDEX],remotetag_para,remotetag_paralen);
|
dwt_writetxdata(24+remotetag_paralen, tx_nearresp_msg, 0);//дÈë·¢ËÍÊý¾Ý
|
dwt_writetxfctrl(24+remotetag_paralen, 0);//É趨·¢Ëͳ¤¶È
|
remotesend_state=0;
|
finalsend = 1;
|
break;
|
}
|
}
|
}else{
|
|
}
|
|
if(finalsend == 0)
|
{
|
memcpy(&tx_nearresp_msg[MAXRANGE_DISTANCE],&g_com_map[MAXRANGDIST_INDEX],2);
|
dwt_writetxdata(28+tagpossend_flag*(tagpos[taglist_pos].datalen+3), tx_nearresp_msg, 0);//§Õ????????
|
dwt_writetxfctrl(28+tagpossend_flag*(tagpos[taglist_pos].datalen+3), 0);//?Ú
???????
|
}
|
|
// if(remotesend_state&&tag_id_recv==configremotetagID)
|
// {
|
// memcpy(&tx_nearresp_msg[REMOTEPARA_INDEX],remotetag_para,remotetag_paralen);
|
// dwt_writetxdata(22+remotetag_paralen, tx_nearresp_msg, 0);//дÈë·¢ËÍÊý¾Ý
|
// dwt_writetxfctrl(22+remotetag_paralen, 0);//É趨·¢Ëͳ¤¶È
|
// waittagconfig_reponse=1;
|
// }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)))///²»¶Ï²éѯоƬ״ֱ̬µ½½ÓÊճɹ¦»òÕß³öÏÖ´íÎó
|
{ };
|
}else{
|
result++;
|
}
|
|
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(seize_anchor&&memcmp(&rx_buffer[ANCHOR_ID_IDX],&dev_id,2)) //ÇÀÕ¼anchor ʧ°Ü
|
{
|
return 1;
|
}
|
if (rx_buffer[MESSAGE_TYPE_IDX] == NEAR_FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,2)) //ÅжÏÊÇ·ñΪFinal°ü
|
{
|
|
resp_tx_ts = get_tx_timestamp_u64();//»ñµÃresponse·¢ËÍʱ¼äT3
|
final_rx_ts = get_rx_timestamp_u64();//»ñµÃfinal½ÓÊÕʱ¼äT6
|
|
// dwt_setrxtimeout(0);//É趨½ÓÊÕ³¬Ê±Ê±¼ä£¬0λûÓг¬Ê±Ê±¼ä
|
// dwt_rxenable(0);//´ò¿ª½ÓÊÕ
|
|
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);
|
|
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;
|
// time_monitor[1] = sync_timer*1000+TIM3->CNT;
|
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;//¾àÀë=¹âËÙ*·ÉÐÐʱ¼ä
|
dist_no_bias = distance - dwt_getrangebias(config.chan, (float)distance, config.prf); //¾àÀë¼õÈ¥½ÃÕýϵÊý
|
dist_cm = dist_no_bias * 100; //dis Ϊµ¥Î»ÎªcmµÄ¾àÀë
|
dwt_readdiagnostics(&d1);
|
// time_monitor[2] = sync_timer*1000+TIM3->CNT;
|
LOS(&d1);
|
// time_monitor[3] = sync_timer*1000+TIM3->CNT;
|
/*--------------------------ÒÔÏÂΪ·Ç²â¾àÂß¼------------------------*/
|
//dist_cm=33000;
|
//GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_RESET);
|
LED0_BLINK; //ÿ³É¹¦Ò»´ÎͨѶÔòÉÁ˸һ´Î
|
dis_after_filter=dist_cm;
|
hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
|
|
if(hex_dist>-1000&&hex_dist<200000)
|
{
|
g_flag_Taggetdist[taglist_pos]=0;
|
if(abs(hex_dist-his_dist[taglist_pos])<1500||misdist_num[taglist_pos]>3)
|
{
|
goodtimes++;
|
misdist_num[taglist_pos]=0;
|
tagdist_list[taglist_pos] = hex_dist;
|
his_dist[taglist_pos]=hex_dist;
|
g_Tagdist[taglist_pos]=hex_dist;
|
signalpower_list[taglist_pos] = ((uint8_t)firstpath_power<<8)|(uint8_t)((rx_power-firstpath_power)*10);
|
// #ifndef USART_INTEGRATE_OUTPUT
|
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);
|
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-firstpath_power)*10;
|
usart_send[17] = tag_frequency;
|
usart_send[18] = tag_slotpos;
|
checksum = Checksum_u16(&usart_send[2],17);
|
memcpy(&usart_send[19],&checksum,2);
|
//UART_PushFrame(usart_send,21);
|
// #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
|
// #ifdef DEBUG_INF
|
// printf("ÐòºÅ£º%d\r\n",frame_seq_nb2);
|
// #endif
|
}else{
|
// printf("%d",hex_dist);
|
misdist_num[taglist_pos]++;
|
}
|
}else{
|
errortimes++;
|
usart_send[13] = battary;
|
}
|
if(rx_buffer[TAGCONFIGSUCCESS_INDEX]==1)
|
{
|
|
remotesend_state = 0;
|
usart_send[2] = 7;//Õý³£Ä£Ê½
|
usart_send[3] = 5;//Êý¾Ý¶Î³¤¶È
|
memcpy(&usart_send[4],&tag_id_recv,2);
|
usart_send[6] = 1;//Êý¾Ý¶Î³¤¶È
|
checksum = Checksum_u16(&usart_send[2],5);
|
memcpy(&usart_send[7],&checksum,2);
|
UART_PushFrame(usart_send,9);
|
}
|
}
|
}else{
|
#ifdef DEBUG_INF
|
printf("final°üʧ°Ü£º%x/n",status_reg);
|
#endif
|
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
|
}
|
}
|
void Anchor_Start(void)
|
{
|
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR| SYS_STATUS_TXFRS |SYS_STATUS_RXFCG);
|
g_start_sync_flag=0;
|
|
dwt_setrxtimeout(1000);//É趨½ÓÊÕ³¬Ê±Ê±¼ä£¬0λûÓг¬Ê±Ê±¼ä
|
dwt_rxenable(0);//´ò¿ª½ÓÊÕ
|
// while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))&&!g_start_send_flag&&!g_start_sync_flag)//²»¶Ï²éѯоƬ״ֱ̬µ½½ÓÊճɹ¦»òÕß³öÏÖ´íÎó
|
// {
|
// //IdleTask();
|
// };
|
}
|
extern int32_t intheight;
|
extern u16 synclost_count;
|
uint32_t current_syncid=0xffffffff,synclost_timer;
|
extern u8 flag_syncbase,waitsync_flag;
|
u8 tagpos_rec[50],tagpos_send[50],ancidlist_num,zero_matrix[100]={0};
|
u16 ancidlist_rec[20],ancidlist_send[20];
|
u16 recnearbaselist_id[30],rec_taganc_signalpower_list[11];
|
int32_t recnearbaselist_dist[30];
|
int16_t rec_tagheight;
|
u16 timm = 0;
|
void Anchor_App(void)
|
{
|
|
u8 send_len,i;
|
u16 tempid;
|
uint32_t rec_syncid;
|
|
// 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);
|
// while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//²»¶Ï²éѯоƬ״ֱ̬µ½½ÓÊճɹ¦»òÕß³öÏÖ´íÎó
|
// {
|
// //IdleTask();
|
// };
|
//GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_RESET);
|
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);
|
status_reg = dwt_read32bitreg(SYS_STATUS_ID);
|
if (status_reg & SYS_STATUS_RXFCG)//³É¹¦½ÓÊÕ
|
{ u16 tag_recv_interval;
|
float temp_tagpos;
|
// time_monitor[0] = sync_timer*1000+TIM3->CNT;
|
g_Resttimer=0;
|
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//Çå³ý±ê־λ
|
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//»ñµÃ½ÓÊÕÊý¾Ý³¤¶È
|
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])
|
{
|
uint16_t checksum;
|
case DISCOVERPOLL:
|
if (anchor_type == rx_buffer[ANCHOR_ID_IDX])
|
{
|
taglist_pos=CmpTagInList(tag_id_recv);
|
if(taglist_pos==taglist_num)
|
{
|
taglist_pos=taglist_num;
|
tagid_list[taglist_num++]=tag_id_recv;
|
|
new_tagid=1;
|
}else{
|
new_tagid=0;
|
}
|
tagofflinetime[taglist_pos]=0;
|
|
rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
|
if(rec_nearbase_num>=30)
|
{
|
break;
|
}
|
battary = rx_buffer[BATTARY_IDX];
|
button = rx_buffer[BUTTON_IDX];
|
frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
|
tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];
|
tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];
|
|
////////////////»ùÕ¾»ã×ÜģʽÊý¾Ý
|
memcpy(&rec_tagheight,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*6],2);
|
memcpy(&recnearbaselist_id,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);
|
memcpy(&recnearbaselist_dist,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*2],rec_nearbase_num*4);
|
Anchor_RecPoll();
|
}
|
break;
|
case SPOLL:
|
//if (anchor_type == rx_buffer[ANC_TYPE_IDX])
|
if(dev_id==anc_id_recv)
|
{
|
taglist_pos=CmpTagInList(tag_id_recv);
|
if(taglist_pos==taglist_num)
|
{
|
taglist_pos=taglist_num;
|
tagid_list[taglist_num++]=tag_id_recv;
|
|
new_tagid=1;
|
}else{
|
new_tagid=0;
|
}
|
tagofflinetime[taglist_pos]=0;
|
|
rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
|
if(rec_nearbase_num>=30)
|
{
|
break;
|
}
|
battary = rx_buffer[BATTARY_IDX];
|
button = rx_buffer[BUTTON_IDX];
|
frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
|
tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];
|
tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];
|
|
////////////////»ùÕ¾»ã×ÜģʽÊý¾Ý
|
memcpy(&rec_tagheight,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*6],2);
|
memcpy(&recnearbaselist_id,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);
|
memcpy(&recnearbaselist_dist,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*2],rec_nearbase_num*4);
|
usart_send[2] = 0x0c;//Õý³£Ä£Ê½
|
usart_send[3] = 15+8*(rec_nearbase_num);//Êý¾Ý¶Î³¤¶È
|
memcpy(&usart_send[4],&tag_id_recv,2);
|
usart_send[6] = rx_buffer[SEQUENCE_IDX];
|
usart_send[7] = rx_buffer[SEQUENCEH_IDX];
|
usart_send[8] = battary;
|
usart_send[9] = button;
|
memcpy(&usart_send[10],&rec_tagheight,2);
|
usart_send[12] = tag_frequency;
|
usart_send[13] = tag_slotpos;
|
usart_send[14] = 0;
|
usart_send[15] = 0;
|
usart_send[16] = rec_nearbase_num;
|
|
memcpy(&usart_send[17],&recnearbaselist_id,2*rec_nearbase_num);
|
memcpy(&usart_send[17+rec_nearbase_num*2],&recnearbaselist_dist,4*rec_nearbase_num);
|
memcpy(&usart_send[17+rec_nearbase_num*6],zero_matrix,2*rec_nearbase_num);
|
|
checksum = Checksum_u16(&usart_send[2],15+8*rec_nearbase_num);
|
memcpy(&usart_send[17+8*rec_nearbase_num],&checksum,2);
|
UART_PushFrame(usart_send,19+8*rec_nearbase_num);
|
Anchor_RecPoll();
|
}
|
break;
|
case SYNC:
|
memcpy(&rec_syncid,&rx_buffer[ANCHOR_ID_IDX],4);
|
if(anc_id_recv==g_com_map[SYNCBASEID])
|
{
|
synclost_count = 0;
|
flag_syncbase=0;
|
sync_seq=rx_buffer[SYNC_SEQ_IDX]+1;
|
TIM3->CNT = (sync_seq*325%1000)-27;
|
sync_timer = (sync_seq*325/1000);
|
synclost_timer=0;
|
if(g_com_map[BASESYNCSEQ]==2)
|
SyncPoll(sync_seq);
|
}
|
break;
|
case NEAR_MSG:
|
if(anc_id_recv==dev_id)
|
{
|
rx_buffer[TAGSLOTPOS]=taglist_pos;
|
tx_near_msg[MESSAGE_TYPE_IDX] = NEAR_MSG;
|
memcpy(&tx_near_msg[TAG_ID_IDX],&tag_id_recv,2);
|
memcpy(&tx_near_msg[NEARMSG_EMPTYSLOTPOS_INDEX],&tagpos_binary,4);
|
ancidlist_num=g_com_map[NEARBASE_NUM];
|
tx_near_msg[NEARBASENUM_INDEX]=g_com_map[NEARBASE_NUM];
|
memcpy(&tx_near_msg[NEARBASEID_INDEX],&g_com_map[NEARBASE_ID1],g_com_map[NEARBASE_NUM]*2);
|
send_len=21+ancidlist_num*2;
|
dwt_writetxdata(send_len, tx_near_msg, 0);//½«Poll°üÊý¾Ý´«¸øDW1000£¬½«ÔÚ¿ªÆô·¢ËÍʱ´«³öÈ¥
|
dwt_writetxfctrl(send_len, 0);//ÉèÖó¬¿í´ø·¢ËÍÊý¾Ý³¤¶È
|
dwt_starttx(DWT_START_TX_IMMEDIATE);
|
}
|
break;
|
case REG_POLL:
|
for(i=0;i<MAX_REGTAGNUM;i++)
|
{
|
if(tag_id_recv==regtag_map.tagid[i]&®tag_map.remain_time[i]>0)
|
{
|
tx_near_msg[REGR_TAGSLOTPOS_INDEX]=regtag_map.tag_slotpos[i];
|
tx_near_msg[REGR_TAGFREQ_INDEX] = regtag_map.tag_frequency[i];
|
memcpy(&tx_near_msg[TAG_ID_IDX],&tag_id_recv,2);
|
memcpy(&tx_near_msg[ANCHOR_ID_IDX],&dev_id,2);
|
tx_near_msg[MESSAGE_TYPE_IDX] = REG_RESPONSE;
|
ancidlist_num=g_com_map[NEARBASE_NUM];
|
tx_near_msg[NEARBASENUM_INDEX]=g_com_map[NEARBASE_NUM];
|
memcpy(&tx_near_msg[NEARBASEID_INDEX],&g_com_map[NEARBASE_ID1],g_com_map[NEARBASE_NUM]*2);
|
send_len=21+ancidlist_num*2;
|
dwt_writetxdata(send_len, tx_near_msg, 0);//½«Poll°üÊý¾Ý´«¸øDW1000£¬½«ÔÚ¿ªÆô·¢ËÍʱ´«³öÈ¥
|
dwt_writetxfctrl(send_len, 0);//ÉèÖó¬¿í´ø·¢ËÍÊý¾Ý³¤¶È
|
dwt_starttx(DWT_START_TX_IMMEDIATE);
|
regtag_map.tagid[i] = 0;
|
break;
|
}
|
|
}
|
if(i==MAX_REGTAGNUM)
|
{
|
dwt_readdiagnostics(&d1);
|
LOS(&d1);
|
usart_send[2]=0x0a;
|
usart_send[3]=20;
|
memcpy(&usart_send[4],&dev_id,2);
|
memcpy(&usart_send[6],&tag_id_recv,2);
|
memcpy(&usart_send[8],&rx_buffer[10],9);
|
usart_send[17] = firstpath_power;
|
memcpy(&usart_send[18],&rx_buffer[19],2);
|
checksum = Checksum_u16(&usart_send[2],20);
|
memcpy(&usart_send[22],&checksum,2);
|
UART_PushFrame(usart_send,24);
|
}
|
break;
|
case NEAR_POLL:
|
// GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_SET);
|
|
memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
|
taglist_pos=CmpTagInList(tag_id_recv);
|
if(taglist_pos==taglist_num)
|
{
|
taglist_pos=taglist_num;
|
tagid_list[taglist_num++]=tag_id_recv;
|
|
new_tagid=1;
|
}else{
|
new_tagid=0;
|
}
|
tagofflinetime[taglist_pos]=0;
|
temp_tagpos=round((float)(sync_timer%g_com_map[COM_INTERVAL])/slottime);
|
tagpos_rec[(u8)temp_tagpos]=1;
|
rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
|
if(rec_nearbase_num>30)
|
{
|
break;
|
}
|
battary = rx_buffer[BATTARY_IDX];
|
button = rx_buffer[BUTTON_IDX];
|
frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
|
tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];
|
tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];
|
////////////////»ùÕ¾»ã×ÜģʽÊý¾Ý
|
memcpy(&rec_tagheight,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*8+6],2);
|
memcpy(&recnearbaselist_id,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);
|
memcpy(&recnearbaselist_dist,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*2],rec_nearbase_num*4+4);
|
memcpy(&rec_taganc_signalpower_list,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*6+4],rec_nearbase_num*2+2);
|
if(anc_id_recv == dev_id||g_com_map[OUTPUTALLNEARPOLLMSG] == 1 )
|
{
|
usart_send[2] = 0x0c;//Õý³£Ä£Ê½
|
usart_send[3] = 15+8*(rec_nearbase_num+1);//Êý¾Ý¶Î³¤¶È
|
memcpy(&usart_send[4],&tag_id_recv,2);
|
usart_send[6] = rx_buffer[SEQUENCE_IDX];
|
usart_send[7] = rx_buffer[SEQUENCEH_IDX];
|
usart_send[8] = battary;
|
usart_send[9] = button;
|
|
memcpy(&usart_send[10],&rec_tagheight,2);
|
usart_send[12] = tag_frequency;
|
usart_send[13] = tag_slotpos;
|
usart_send[14] = 0;
|
usart_send[15] = 0;
|
usart_send[16] = rec_nearbase_num+1;
|
memcpy(&usart_send[17],&anc_id_recv,2);
|
memcpy(&usart_send[19],&recnearbaselist_id,2*rec_nearbase_num);
|
memcpy(&usart_send[19+rec_nearbase_num*2],&recnearbaselist_dist,4*rec_nearbase_num+4);
|
memcpy(&usart_send[19+rec_nearbase_num*6+4],&rec_taganc_signalpower_list,2*rec_nearbase_num+2);
|
|
checksum = Checksum_u16(&usart_send[2],23+8*rec_nearbase_num);
|
memcpy(&usart_send[25+8*rec_nearbase_num],&checksum,2);
|
UART_PushFrame(usart_send,27+8*rec_nearbase_num);
|
// UART_CheckSend();
|
|
timm++;
|
}
|
//////////////////////////////////////////////////////////////
|
|
|
GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_SET);
|
if(anc_id_recv==dev_id)
|
{
|
Anchor_RecNearPoll(rec_nearbase_num);
|
}else{
|
|
for(i=0;i<rec_nearbase_num;i++)
|
{
|
memcpy(&tempid,&rx_buffer[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);
|
// }
|
// if(tag_id_recv==0x4008)
|
// printf("±êÇ©¸ß¶È: %d,»ùÕ¾¸ß¶È: %d,¸ß¶È²î£º %d. \r\n",rec_tagheight,intheight,intheight-rec_tagheight);
|
|
break;
|
default:
|
|
break;
|
}
|
}else{
|
#ifdef DEBUG_INF
|
printf("poll°üʧ°Ü£º%x/n",status_reg);
|
#endif
|
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
|
}
|
UART_CheckSend();
|
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), 1);
|
dwt_setrxtimeout(0);//É趨½ÓÊÕ³¬Ê±Ê±¼ä£¬0λûÓг¬Ê±Ê±¼ä
|
dwt_rxenable(0);//´ò¿ª½ÓÊÕ
|
|
}
|
