#include "dw_mbx_tag.h"
|
#include "dps310.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);
|
|
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;
|
}
|
uint32_t temp231;
|
void MbxTagUwbRec(void)
|
{
|
|
u8 send_len,i;
|
u16 tempid;
|
uint32_t rec_syncid,status_reg;
|
uint16_t checksum;
|
uint8_t kk;
|
|
dwt_setrxtimeout(3000);//É趨½ÓÊÕ³¬Ê±Ê±¼ä£¬0λûÓг¬Ê±Ê±¼ä
|
dwt_rxenable(0);//´ò¿ª½ÓÊÕ
|
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_6, GPIO_PIN_RESET);
|
temp231 = dwt_read32bitreg(CHAN_CTRL_ID) ;
|
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_RXFCG | SYS_STATUS_ALL_RX_ERR)))//²»¶Ï²éѯоƬ״ֱ̬µ½½ÓÊճɹ¦»òÕß³öÏÖ´íÎó
|
{
|
current_time=HAL_LPTIM_ReadCounter(&hlptim1);
|
if(current_time>=end_time&¤t_time<end_time+15000)
|
break;
|
};
|
|
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);
|
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);
|
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);
|
}
|
|
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;
|
#define WG_LOST_SWITCH_THRES 5
|
#define WG_LOST_NOUWB_COUNT 60
|
#define DEFAULT_WG_ID 0xFFFF
|
u8 lora_jianting_flag = 1;
|
void LoraReportFreqPoll(void)
|
{
|
if(wg_lost_count++>WG_LOST_NOUWB_COUNT) //Íø¹Ø60ÃëûÓÐͨѶ£¬¾Í²»¿ªÆôUWB²â¾àlora¼àÌý
|
{
|
lora_jianting_flag = 0;
|
} else {
|
lora_jianting_flag = 1;
|
}
|
if(wg_state==WG_Lost)
|
{
|
wg_report_id = DEFAULT_WG_ID;
|
wg_report_freq = REPORT_MANGE_CHANNEL_FRQ; //Èç¹û¶ªÊ§Á´½Ó¾Í½øÈëWG¹ÜÀíÐŵÀ¡£
|
} else {
|
if(wg_lost_count>WG_LOST_SWITCH_THRES)
|
{
|
wg_state = WG_Lost;
|
}
|
}
|
}
|
static u16 checksum;
|
u8 gps_state,gps_chafenlingqi,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);
|
}
|
uint16_t heatbeat_count = 3600;
|
int16_t intheight;
|
#define HEATBEAT_UPDATE_TIME 3600
|
#define LORA_REPORT_MAXANC_NUM 20
|
extern float Height;
|
u8 tarray[20]= {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20};
|
void LoraReportPoll(void)
|
{
|
// delay_ms(100);
|
#ifdef _USE_BAR
|
GetPressAndHeight();
|
intheight = Height*100;
|
#endif
|
#ifdef _SMT_TEST
|
printf("Æøѹֵ£º%d",intheight);
|
#endif
|
|
TagListUpdate();
|
|
LoraReportFreqPoll();
|
flag_getwgresp = 0;
|
// wg_report_freq = REPORT_MANGE_CHANNEL_FRQ;
|
SwitchLoraSettings(wg_report_freq,REPORT_CHANNEL_SF,g_com_map[LORA_POWER]);
|
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;
|
}
|
}
|
}
|
lora_sendbuffer[BAT_IDX] = report_ancnum;//²â¾à»ùÕ¾ÊýÁ¿
|
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+30;
|
#else
|
lora_sendbuffer[MSG_TYPE_IDX] = LORA_MSGTYPE_TAGMSGTOWG;
|
lora_sendbuffer[MSG_LENGTH] = 4*report_ancnum+ANCID_IDX;
|
#endif
|
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_jingdu,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],&bar_height,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);
|
Radio.Send(lora_sendbuffer,ANCID_IDX+report_ancnum*4+2);
|
LED_TB_ON;
|
bat_percent=Get_Battary();
|
|
}
|
uint8_t Lora_send_flag;
|
void LedTask(void)
|
{
|
if(report_ancnum>0)
|
{
|
LED2_TB_ON;
|
#ifdef _SMT_TEST
|
// tflag1 = 1;
|
printf("²â¾àÖµ%d\r\n",tagdist_list[0]);
|
#endif
|
}
|
if(wg_lost_count<=1)
|
{
|
#ifdef _SMT_TEST
|
// tflag2 = 1;
|
printf("Íø¹ØͨѶÕý³£\r\n");
|
#endif
|
LED_TB_ON;
|
}
|
if(!HAL_GPIO_ReadPin(INPUT_5V_GPIO_Port,INPUT_5V_Pin))
|
{
|
if(bat_percent<15)
|
{
|
LED_TR_ON;
|
} else {
|
LED2_TB_ON;
|
}
|
}
|
|
}
|
void LedOff_task(void)
|
{
|
LED_TB_OFF;
|
LED_TR_OFF;
|
LED2_TB_OFF;
|
LED2_TR_OFF;
|
}
|
extern uint8_t rec_secdelay;
|
u16 lora_send_count;
|
uint8_t send_count;
|
#define FASONG_TIME 2
|
void Lora_Send_Poll(void)
|
{
|
if(Lora_send_flag)
|
{
|
if(lora_send_count++%10==0||lora_jianting_flag)
|
{Lora_send_flag=0;
|
if(send_count++%FASONG_TIME==0)
|
{
|
|
if(rec_secdelay>0)
|
{
|
// loraled=YELLOW;
|
rec_secdelay--;
|
return;
|
}
|
LedTask();
|
LoraReportPoll();
|
LedOff_task();
|
}
|
}
|
}
|
}
|
