| | |
| | | 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,firstpath_power2, rx_power,rec_firstpath_power; |
| | | double f1, f2, r1, r2; |
| | | uint16_t F1,F2,F3,N,C; |
| | | double B = 131072; |
| | | double A = 121.74; |
| | | double min_power; |
| | | dwt_rxdiag_t d1; |
| | | 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; |
| | | } |
| | | void NearPoll(void) |
| | | { |
| | | static u8 mainbase_lost_count=0,flag_finalsend,flag_rxon; |
| | |
| | | dwt_writetxdata(28+nearbase_num*4, tx_nearfinal_msg, 0);//å°åéæ°æ®åå
¥DW1000 |
| | | dwt_writetxfctrl(28+nearbase_num*4, 0);//设å®åéæ°æ®é¿åº¦ |
| | | memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4); |
| | | dwt_readdiagnostics(&d1); |
| | | LOS(&d1); |
| | | tx_nearfinal_msg[TAGRXPOWER_IDX] = -(int)firstpath_power; |
| | | if(temp_dist!=0x1ffff) |
| | | { |
| | | g_Resttimer=0; |
| | |
| | | dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); |
| | | } |
| | | } |
| | | double firstpath_power, rx_power,rec_firstpath_power; |
| | | double f1, f2, r1, r2; |
| | | uint16_t F1,F2,F3,N,C; |
| | | double B = 131072; |
| | | double A = 121.74; |
| | | double min_power; |
| | | dwt_rxdiag_t d1; |
| | | 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; |
| | | } |
| | | u16 smallcar_idlist[10]={0x4052,0x4032,0x4055,0x4034,0x4016,0x4010,0x4036,0x4027,0x4024,0x4047}; |
| | | extern u16 dist_threshold; |
| | | u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,getrange_success=0; |
| | |
| | | //dist_cm=33000; |
| | | getrange_success = 1; |
| | | |
| | | LED0_BLINK; //æ¯æå䏿¬¡é讯åéªç䏿¬¡ |
| | | //LED0_BLINK; //æ¯æå䏿¬¡é讯åéªç䏿¬¡ |
| | | dis_after_filter=dist_cm; |
| | | hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10; |
| | | |
| | |
| | | } |
| | | if(abs(hex_dist-his_dist[taglist_pos])<dist_threshold||misdist_num[taglist_pos]>3) |
| | | { |
| | | |
| | | u8 error_flag = 0; |
| | | g_Resttimer=0; |
| | | IWDG_Feed(); |
| | | #ifdef TDFILTER |
| | |
| | | #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; |
| | |
| | | usart_send[16] = rx_power; |
| | | checksum = Checksum_u16(&usart_send[2],17); |
| | | memcpy(&usart_send[19],&checksum,2); |
| | | UART_PushFrame(usart_send,21); |
| | | firstpath_power2 = rx_buffer[TAGRXPOWER_IDX]; |
| | | firstpath_power = -(int)firstpath_power; |
| | | if(firstpath_power2>firstpath_power) |
| | | { |
| | | if(firstpath_power2>93) |
| | | { |
| | | printf("é误1ï¼è¢«æµæ¨¡åæ¥æ¶æå\r\n"); |
| | | error_flag = 1; |
| | | } |
| | | if(firstpath_power<84) |
| | | { |
| | | printf("é误2ï¼åçè¿å¤§\r\n"); |
| | | error_flag = 1; |
| | | } |
| | | }else{ |
| | | if(firstpath_power>93) |
| | | { |
| | | printf("é误3ï¼è¢«æµæ¨¡ååå°æå\r\n"); |
| | | error_flag = 1; |
| | | } |
| | | if(firstpath_power2<84) |
| | | { |
| | | printf("é误2ï¼åçè¿å¤§\r\n"); |
| | | error_flag = 1; |
| | | } |
| | | } |
| | | if(error_flag == 0&&his_dist[taglist_pos]!=hex_dist) |
| | | { |
| | | LED0_BLINK; |
| | | printf("éè¿\r\n"); |
| | | } |
| | | his_dist[taglist_pos]=hex_dist; |
| | | printf("å
åºï¼%dãè·ç¦»ï¼%d cmãæ¥æ¶å¼ºåº¦: %dãåå°å¼ºåº¦ï¼%då¼ºåº¦åæ ¼èå´ï¼å¤§äº83å°äº94ï¼\r\n",frame_seq_nb2,g_Tagdist[taglist_pos],(int)firstpath_power,(int)firstpath_power2); |
| | | //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); |