#include "dw_mbx_tag.h"
|
//#include "dps310.h"
|
#include "WS2812.h"
|
#include "syn6288.h"
|
#define DELAY_BETWEEN_TWO_FRAME_UUS 400
|
#define POLL_RX_TO_RESP_TX_DLY_UUS 470
|
#define UUS_TO_DWT_TIME 65536
|
|
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 resp_msg_set_ts(uint8_t *ts_field, const uint64_t ts)
|
{
|
int i;
|
for (i = 0; i < 4; i++)
|
{
|
ts_field[i] = (ts >> (i * 8)) & 0xFF;
|
}
|
}
|
static uint16_t current_time,start_time,end_time;
|
static u8 frame_len,timeout,rec_nearbase_num,result;
|
static u8 rx_buffer[200],send_buffer[200];
|
static u16 anc_id_recv,tag_id_recv;
|
static uint64_t poll_rx_ts;
|
static uint32_t status_reg,resp_tx_time;
|
static uint64_t resp_tx_ts;
|
|
#define FZ_NEARBASENUM_INDEX 10
|
#define FZ_NEARBASEID_INDEX 11
|
#define TAG_NUM_IN_SYS 25
|
|
#define TAG_KEEPTIMES 30
|
#define REPORT_TAG_KEEPTIMES 5
|
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 u8 frame_seq_nb2,battary,button,uwb_losttimer,taglist_pos;
|
|
static u8 tagofflinetime[ANC_MAX_NUM];
|
u8 taglist_num;
|
uint16_t tagid_list[ANC_MAX_NUM],tagdist_list[ANC_MAX_NUM];
|
uint8_t Anchor_RecNearPoll(uint8_t ancrec_nearbasepos) //0 mainbase 1 first near_base
|
{
|
uint8_t motorstate;
|
memcpy(&send_buffer[ANCHOR_ID_IDX],&dev_id,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
|
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) ;
|
|
/* 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;
|
|
|
|
dwt_writetxdata(38, send_buffer, 0);//дÈë·¢ËÍÊý¾Ý
|
dwt_writetxfctrl(38, 0);//É趨·¢Ëͳ¤¶È
|
result = dwt_starttx(DWT_START_TX_DELAYED);
|
|
if(result==0)
|
{
|
start_time=HAL_LPTIM_ReadCounter(&hlptim1);
|
timeout=100; //µ¥Î»0.1ms
|
end_time=start_time+(timeout<<2);
|
if(end_time>=32768)
|
{end_time-=32768;}
|
current_time=HAL_LPTIM_ReadCounter(&hlptim1);
|
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & SYS_STATUS_TXFRS ))///²»¶Ï²éѯоƬ״ֱ̬µ½½ÓÊճɹ¦»òÕß³öÏÖ´íÎó
|
{
|
current_time=HAL_LPTIM_ReadCounter(&hlptim1);
|
if(current_time>=end_time&¤t_time<end_time+15000)
|
break;
|
};
|
tagofflinetime[taglist_pos] = 0;
|
}
|
// printf("²â¾à³É¹¦£¬ÐòºÅ%d£¬¾àÀë%d\r\n",frame_seq_nb2,tagdist_list[taglist_pos]);
|
// delay_ms(10);
|
}
|
uint8_t report_ancnum;
|
uint16_t report_ancdist[ANC_MAX_NUM],report_ancid[ANC_MAX_NUM];
|
void TagListUpdate(void)
|
{
|
uint16_t i,j=0,k=0,temp[TAG_NUM_IN_SYS];
|
for(i=0;i<taglist_num;i++)
|
{
|
if(tagofflinetime[i]++<REPORT_TAG_KEEPTIMES)
|
{
|
report_ancid[k]=tagid_list[i];
|
report_ancdist[k++]=tagdist_list[i];
|
}
|
if(tagofflinetime[i]++<TAG_KEEPTIMES)
|
{
|
tagid_list[j]=tagid_list[i];
|
tagdist_list[j] = tagdist_list[i];
|
tagofflinetime[j++]=tagofflinetime[i];
|
}
|
}
|
report_ancnum = k;
|
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;
|
}
|
return i;
|
}
|
extern uint8_t gps_state;
|
uint32_t temp231;
|
extern uint32_t uwbled,gpsled,loraled,powerled;
|
uint32_t uwbdezhuangtai;
|
u8 uwb_rx_remaintime_ms,gps_chafenlingqi;
|
uint16_t test_ancdist;
|
uint16_t test_ancid;
|
void MbxTagUwbRec(void)
|
{
|
|
u8 send_len,i;
|
u16 tempid;
|
uint32_t rec_syncid,status_reg;
|
uint16_t checksum;
|
uint8_t kk;
|
status_reg = dwt_read32bitreg(SYS_STATUS_ID);
|
// printf("UWB״̬:%x\r\n",uwbdezhuangtai);
|
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;//»ñµÃ½ÓÊÕÊý¾Ý³¤¶È
|
|
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);
|
//gps_state=tag_id_recv;
|
switch(rx_buffer[MESSAGE_TYPE_IDX])
|
{uint8_t target_tagi;
|
case MBX_REG:
|
break;
|
case MBX_POLL:
|
if(group_id==rx_buffer[GROUP_ID_IDX])
|
{
|
// dwt_readdiagnostics(&d1);
|
// LOS(&d1);
|
// if(firstpath_power<-100)
|
// {
|
// 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);
|
|
test_ancid= tag_id_recv;
|
test_ancdist= rec_ancdistlist[0];
|
|
uwb_losttimer = 0;
|
|
taglist_pos=CmpTagInList(tag_id_recv);
|
if(taglist_num>=ANC_MAX_NUM)
|
break;
|
|
for(i=0;i<rec_nearbase_num;i++)
|
{
|
memcpy(&tempid,&rx_buffer[FZ_NEARBASEID_INDEX+i*2],2);
|
if(tempid==dev_id)
|
{
|
Anchor_RecNearPoll(i);
|
tagdist_list[taglist_pos]=rec_ancdistlist[i];
|
if(taglist_pos==taglist_num)// taglist_pos==taglist_num ˵Ã÷Õâ¸ö»ùÕ¾²»ÔÚµ±Ç°ÁбíÖÐ
|
{ //tempid==dev_id ˵Ã÷»ùվϷ¢²â¾à±¨ÎÄÓÐÕâ¸ö±êÇ©µÄÐÅÏ¢
|
taglist_num++; //Âú×ãÉÏÊöÁ½ÖÖÇé¿ö²Å»áÌí¼Ó»ùÕ¾ID½øÈëÁбíÖУ¬·ñÔò»á³öÏÖ±êÇ©²»ÔÚ»ùÕ¾ÁбíÖУ¬±êÇ©Ò²²»ÏìÓ¦µÄÇé¿ö
|
tagid_list[taglist_pos] = tag_id_recv;
|
}
|
break;
|
}
|
}
|
if(i==rec_nearbase_num&&taglist_pos==taglist_num)//µ±»ùÕ¾²»ÔÚ±êÇ©ÁбíÖУ¬±êÇ©Ò²²»ÔÚ»ùÕ¾ÁбíÖÐʱ£¬Ëæ»úÕÒ¸öʱ¼äƬ»Ø¸´»ùÕ¾¡£
|
{
|
Anchor_RecNearPoll(rec_nearbase_num+start_time%5);
|
// Anchor_RecNearPoll(rec_nearbase_num);
|
}
|
}
|
|
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);
|
}
|
if(uwb_rx_remaintime_ms>0)
|
{
|
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(uwb_rx_remaintime_ms*1000);//É趨½ÓÊÕ³¬Ê±Ê±¼ä£¬0λûÓг¬Ê±Ê±¼ä
|
dwt_rxenable(0);//´ò¿ª½ÓÊÕ
|
}else{
|
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);
|
dwt_forcetrxoff();
|
dwt_entersleep();
|
}
|
}
|
wg_state_enum wg_state = WG_Lost;
|
uint8_t lora_sendbuffer[200];
|
u8 seq_num;
|
extern u8 wg_lost_count;
|
uint16_t wg_report_freq,wg_report_id;
|
uint8_t wg_report_sf;
|
#define WG_LOST_SWITCH_THRES 3
|
#define WG_LOST_NOUWB_COUNT 60
|
#define DEFAULT_WG_ID 0xFFFF
|
#define DEFAULT_LR_WG_ID 0xFFFE
|
u8 lora_jianting_flag = 1;
|
void LoraReportFreqPoll(void)
|
{
|
// wg_state = WG_Lost;
|
if(wg_lost_count++>WG_LOST_NOUWB_COUNT)
|
{
|
lora_jianting_flag = 0;
|
} else {
|
lora_jianting_flag = 1;
|
}
|
if(wg_state==WG_Lost)
|
{static u8 channel_switch_count = 0;
|
// channel_switch_count = 10;
|
if(channel_switch_count++>1)
|
{
|
channel_switch_count = 0;
|
wg_report_id = DEFAULT_LR_WG_ID;
|
wg_report_freq = LR_DATA_CHANNEL_FRQ; //Èç¹û¶ªÊ§Á´½Ó¾Í½øÈëWG¹ÜÀíÐŵÀ¡£
|
wg_report_sf = LR_DATA_CHANNEL_SF;
|
|
}else{
|
wg_report_id = DEFAULT_WG_ID;
|
wg_report_freq = REPORT_MANGE_CHANNEL_FRQ; //Èç¹û¶ªÊ§Á´½Ó¾Í½øÈëWG¹ÜÀíÐŵÀ¡£
|
wg_report_sf = REPORT_CHANNEL_SF;
|
}
|
} else {
|
if(wg_lost_count>WG_LOST_SWITCH_THRES)
|
{
|
wg_state = WG_Lost;
|
}
|
}
|
}
|
static u16 checksum;
|
u8 gps_state,gps_satel_num,gps_signalpower;
|
double gps_jingdu,gps_weidu;
|
float gps_height;
|
extern u8 bat_percent,stationary_flag;
|
int16_t sendcount = 0,tp1,bar_height;
|
u8 flag_getwgresp;
|
void LoraHeartBeartPoll(void)
|
{
|
lora_sendbuffer[MSG_TYPE_IDX] = LORA_MSGTYPE_TAGHEARTBEAT;
|
lora_sendbuffer[MSG_LENGTH] = 14;
|
memcpy(&lora_sendbuffer[SOURCE_ID_IDX],&g_com_map[DEV_ID],2);
|
memcpy(&lora_sendbuffer[DEST_ID_IDX],&wg_report_id,2);
|
memcpy(&lora_sendbuffer[HB_VERSION_IDX],&g_com_map[VERSION],2);
|
lora_sendbuffer[HB_UWBPOWER_IDX] = g_com_map[POWER];
|
lora_sendbuffer[HB_LORAPOWER_IDX] = g_com_map[LORA_POWER];
|
checksum = Checksum_u16(lora_sendbuffer,14);
|
memcpy(&lora_sendbuffer[14],&checksum,2);
|
Radio.Send(lora_sendbuffer,16);
|
}
|
#define HEATBEAT_UPDATE_TIME 3600
|
uint16_t heatbeat_count = HEATBEAT_UPDATE_TIME-10;
|
int16_t intheight;
|
#define LORA_REPORT_MAXANC_NUM 7
|
u8 tarray[20]={1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20},test_num;
|
extern float Height;
|
extern IWDG_HandleTypeDef hiwdg;
|
u8 uwb_rxflag;
|
void LoraReportPoll(void)
|
{
|
if(sendcount++>0)
|
TagListUpdate();
|
|
LoraReportFreqPoll();
|
flag_getwgresp = 0;
|
// wg_report_freq = REPORT_MANGE_CHANNEL_FRQ;
|
|
if(heatbeat_count++>HEATBEAT_UPDATE_TIME && WG_Connected == wg_state) //Èç¹ûÐÄÌø°üµ½´ïÉÏ´«Ê±¼ä£¬²¢ÇÒÍø¹Ø´¦ÓÚÁ´½Ó״̬£¬¾ÍÉÏ´«ÐÄÌø°ü
|
{
|
heatbeat_count = 0;
|
LoraHeartBeartPoll();
|
return;
|
}
|
for(u16 i=0; i<report_ancnum-1; i++)
|
{
|
for(u16 j=0; j<report_ancnum-1-i; j++)
|
{
|
if(report_ancdist[j]>report_ancdist[j+1])
|
{
|
u16 id,dist;
|
u8 bat;
|
id = report_ancid[j];
|
dist = report_ancdist[j];
|
report_ancid[j] = report_ancid[j+1];
|
report_ancdist[j] = report_ancdist[j+1];
|
report_ancid[j+1] = id;
|
report_ancdist[j+1] = dist;
|
}
|
}
|
}
|
if(report_ancnum>0)
|
{
|
uwbled=BLUE;
|
}else{
|
uwbled=RED;
|
}
|
if(report_ancnum>LORA_REPORT_MAXANC_NUM) //¿¼ÂÇlora´«Êäʱ¼ä£¬×î¶à·¢ËÍ10¸ö»ùÕ¾Êý¾Ý¡£
|
report_ancnum = LORA_REPORT_MAXANC_NUM;
|
|
#ifdef USE_GPS
|
lora_sendbuffer[MSG_TYPE_IDX] = LORA_MSGTYPE_TAGMSGTOWG_GPS;
|
lora_sendbuffer[MSG_LENGTH] = 4*report_ancnum+ANCID_IDX;
|
#else
|
lora_sendbuffer[MSG_TYPE_IDX] = LORA_MSGTYPE_TAGMSGTOWG;
|
lora_sendbuffer[MSG_LENGTH] = 4*report_ancnum+ANCID_IDX;
|
#endif
|
// 2000f026 02020002 //000220200
|
memcpy(&lora_sendbuffer[SOURCE_ID_IDX],&g_com_map[DEV_ID],2);
|
memcpy(&lora_sendbuffer[DEST_ID_IDX],&wg_report_id,2);
|
lora_sendbuffer[SEQNUM_IDX] = seq_num++;
|
lora_sendbuffer[BAT_IDX] = bat_percent;
|
lora_sendbuffer[STATE_IDX] = !GET_USERKEY|stationary_flag<<1;
|
#ifdef USE_GPS
|
memcpy(&lora_sendbuffer[GPS_JINGDU_IDX],&gps_jingdu,8);
|
memcpy(&lora_sendbuffer[GPS_WEIDU_IDX],&gps_weidu,8);
|
memcpy(&lora_sendbuffer[GPS_HEIGHT_IDX],&gps_height,8);
|
lora_sendbuffer[GPS_STATE_IDX] = gps_state;
|
lora_sendbuffer[GPS_SATEL_NUM_IDX] = gps_satel_num;
|
lora_sendbuffer[GPS_SPOWER_IDX] = gps_signalpower;
|
lora_sendbuffer[GPS_CHAFENLINGQI] = gps_chafenlingqi;
|
#endif
|
memcpy(&lora_sendbuffer[BAR_HEIGHT_IDX],&intheight,2);
|
lora_sendbuffer[ANCNUM_IDX] = report_ancnum;
|
|
memcpy(&lora_sendbuffer[ANCID_IDX],report_ancid,report_ancnum*2);
|
memcpy(&lora_sendbuffer[ANCID_IDX+report_ancnum*2],report_ancdist,report_ancnum*2);
|
|
checksum = Checksum_u16(lora_sendbuffer,4*report_ancnum+ANCID_IDX);
|
memcpy(&lora_sendbuffer[ANCID_IDX+report_ancnum*4],&checksum,2);
|
|
// UWB_RXStart(100);
|
SwitchLoraSettings(wg_report_freq,wg_report_sf,g_com_map[LORA_POWER]);
|
Radio.Send(lora_sendbuffer,ANCID_IDX+report_ancnum*4+2);
|
// RadioGetStatus();
|
// UWB_RXStart(60);
|
// RadioGetStatus();
|
UWB_RXON();
|
HAL_IWDG_Refresh(&hiwdg);
|
uwb_rxflag=1;
|
// GetPressAndHeight();
|
// intheight = Height*100;
|
|
}
|
