| | |
|---|
| | | int32_t test2; |
|---|
| | | uint16_t u16_nearbase_distlist[MAX_NEARBASE_NUM]; |
|---|
| | | uint8_t get_newdist,notenoughdist_count; |
|---|
| | | void SetANCTimestap(uint8_t i,uint8_t* pollrx,uint8_t* resptx,uint32_t resprx,uint8_t* distoffset) |
|---|
| | | void SetANCTimestap(uint8_t i,uint8_t* pollrx,uint8_t* resptx,uint32_t resprx,uint8_t* distoffset) //写入时间戳信息 |
|---|
| | | { |
|---|
| | | memcpy(&anc_pollrx[i],pollrx,4); |
|---|
| | | memcpy(&anc_resptx[i],resptx,4); |
|---|
| | |
|---|
| | | memcpy(&anc_distoffset[i],distoffset,2); |
|---|
| | | anc_clockoffset[i] = test2; |
|---|
| | | } |
|---|
| | | void CalculateDists(void) |
|---|
| | | void CalculateDists(void) //计算距离 |
|---|
| | | { |
|---|
| | | for(int i=0;i<MAX_NEARBASE_NUM;i++) |
|---|
| | | { |
|---|
| | |
|---|
| | | { |
|---|
| | | nearbase_num = MAX_NEARBASE_NUM-1; |
|---|
| | | } |
|---|
| | | // HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_SET); |
|---|
| | | // last_nearbase_num=next_nearbase_num; |
|---|
| | | // nearbase_num=next_nearbase_num; |
|---|
| | | recbase_num=0; |
|---|
| | | // motor_state=0; |
|---|
| | | for(uint8_t i=0;i<nearbase_num;i++) |
|---|
| | | for(uint8_t i=0;i<nearbase_num;i++) //标签距离32位转成16位 |
|---|
| | | { |
|---|
| | | u16_nearbase_distlist[i] = nearbase_distlist[i]; |
|---|
| | | } |
|---|
| | |
|---|
| | | |
|---|
| | | dwt_forcetrxoff(); |
|---|
| | | flag_rxon=0; |
|---|
| | | if(flag_ancreadpara) |
|---|
| | | if(flag_ancreadpara) //只有在读取参数的时候需要回复 |
|---|
| | | { |
|---|
| | | u16 calCRC; |
|---|
| | | tx_near_msg[MESSAGE_TYPE_IDX] = MBX_PRAAREPLY; |
|---|
| | |
|---|
| | | if(sync_timer>=1010) |
|---|
| | | {sync_timer=0;} |
|---|
| | | } |
|---|
| | | // TIM3->CNT=tmp_time; |
|---|
| | | // if(tagslotpos>max_slotpos) |
|---|
| | | // tagslotpos=tagslotpos%(max_slotpos+1); |
|---|
| | | // tyncpoll_time=(tagslotpos-1)*slottime; |
|---|
| | | //////////////////////////// |
|---|
| | | rec_nearbasepos=0; |
|---|
| | | exsistbase_list[rec_nearbasepos]=KEEP_TIMES; |
|---|
| | | |
|---|
| | |
|---|
| | | dwt_writetxfctrl(28+nearbase_num*4, 0);//设定发送数据长度 |
|---|
| | | flag_getresponse=1; |
|---|
| | | memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4); |
|---|
| | | |
|---|
| | | |
|---|
| | | //时间同步 |
|---|
| | | // ancsync_time=((sync_timer)*1000+tmp_time); |
|---|
| | | // last_slotnum=current_slotnum; |
|---|
| | | // current_slotnum=((float)sync_timer/g_com_map[COM_INTERVAL])+1; |
|---|
| | | // if(current_slotnum==last_slotnum-1) |
|---|
| | | // {flag_getresponse=1;} |
|---|
| | | // lastsync_timer=sync_timer; |
|---|
| | | // offsettimeus=ancsync_time-current_count*LPTIMER_LSB+offset; |
|---|
| | | // SetNextPollTime(tyncpoll_time); |
|---|
| | | // if(rec_remotepara_state==1) |
|---|
| | | // { |
|---|
| | | // memcpy(rec_remotepara,&rx_buffer[REMOTEPARA_INDEX],REMOTEPARA_LEN); |
|---|
| | | // memcpy(&g_com_map[COM_INTERVAL],&rec_remotepara[0],8); |
|---|
| | | // memcpy(&g_com_map[POWER],&rec_remotepara[8],10); |
|---|
| | | // if(g_com_map[COM_INTERVAL]>0&&g_com_map[COM_INTERVAL]<1000&&g_com_map[POWER]<67&&g_com_map[POWER]>0) |
|---|
| | | // {save_com_map_to_flash(); |
|---|
| | | // delay_ms(100); |
|---|
| | | // } |
|---|
| | | // NVIC_SystemReset(); |
|---|
| | | // } |
|---|
| | | }else{ |
|---|
| | | rec_nearbasepos=FindNearBasePos(rec_nearbaseid); |
|---|
| | | SetANCTimestap(rec_nearbasepos,&rx_buffer[RESP_MSG_POLL_RX_TS_IDX],&rx_buffer[RESP_MSG_RESP_TX_TS_IDX],resp_rx_ts,&rx_buffer[RESP_MSG_ANC_DISTOFFSET]); |
|---|
| | |
|---|
| | | |
|---|
| | | dwt_writetxdata(28+nearbase_num*4, tx_near_msg, 0);//将发送数据写入DW1000 |
|---|
| | | dwt_writetxfctrl(28+nearbase_num*4, 0);//设定发送数据长度 |
|---|
| | | |
|---|
| | | |
|---|
| | | //dwt_setdelayedtrxtime(final_tx_time);//设置final包发送时间T5 |
|---|
| | | // result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送 |
|---|
| | | //dwt_writetxdata(4,&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)*4], FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)*4);//将发送数据写入DW1000 |
|---|
| | | } |
|---|
| | | } |
|---|
| | | }else{ |
|---|
| | |
|---|
| | | } |
|---|
| | | |
|---|
| | | report_num=0; |
|---|
| | | // if(last_nearbase_num>4) |
|---|
| | | // { |
|---|
| | | // last_nearbase_num = 5; |
|---|
| | | // } |
|---|
| | | for (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]; |
|---|
| | | } |
|---|
| | | // #ifdef USART_INTEGRATE_OUTPUT |
|---|
| | | // usart_send[0] = 0x55; |
|---|
| | | // usart_send[1] = 0xAA; |
|---|
| | | // usart_send[2] = 4;//正常模式 |
|---|
| | | // usart_send[3] = report_num*6+2;//正常模式 |
|---|
| | | // checksum = Checksum_u16(&usart_send[2],report_num*6+2); |
|---|
| | | // memcpy(&usart_send[4+report_num*6],&checksum,2); |
|---|
| | | // USART_puts(usart_send,6+report_num*6); |
|---|
| | | // #endif |
|---|
| | | |
|---|
| | | dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR| SYS_STATUS_TXFRS |SYS_STATUS_RXFCG); |
|---|
| | | //HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_RESET); |
|---|
