From 1ebb1f507bf7e2b95782bff74ffc5d48e9b5394b Mon Sep 17 00:00:00 2001
From: chen <15335560115@163.com>
Date: 星期四, 26 九月 2024 11:49:09 +0800
Subject: [PATCH] 车载V1.5版本，增加滤波算法和上传数据修改，延长更新现存列表时间，解决无法对所有通信设备全部校准offset的bug
---
APL/dw_ancapp.c | 67 ++++++++++++++++++++++++---------
1 files changed, 48 insertions(+), 19 deletions(-)
diff --git a/APL/dw_ancapp.c b/APL/dw_ancapp.c
index 3e0c92b..9a9bf15 100644
--- a/APL/dw_ancapp.c
+++ b/APL/dw_ancapp.c
@@ -26,7 +26,7 @@
#define POLL_TX_TO_RESP_RX_DLY_UUS 150
#define RESP_RX_TO_FINAL_TX_DLY_UUS 410
#define RESP_RX_TIMEOUT_UUS 2000
-#define DELAY_BETWEEN_TWO_FRAME_UUS 400
+#define DELAY_BETWEEN_TWO_FRAME_UUS 1200
#define POLL_RX_TO_RESP_TX_DLY_UUS 670
#define RESP_TX_TO_FINAL_RX_DLY_UUS 10
#define FINAL_RX_TIMEOUT_UUS 4300
@@ -54,11 +54,11 @@
static uint16_t ancidlist_rec[TAG_NUM_IN_SYS],ancidlist_send[TAG_NUM_IN_SYS],rec_ancidlist[TAG_NUM_IN_SYS];
static int16_t rec_ancdistlist[TAG_NUM_IN_SYS];
static int16_t rec_antdelay;
-static uint16_t taglist_num=0,taglist_pos,tmp_time;
-static uint16_t tagid_list[TAG_NUM_IN_SYS];
+ uint16_t taglist_num=0,taglist_pos;
+ uint16_t tagid_list[TAG_NUM_IN_SYS];
uint8_t tagbattary_list[TAG_NUM_IN_SYS];
static uint8_t tagofflinetime[TAG_NUM_IN_SYS];
-static int32_t tagdist_list[TAG_NUM_IN_SYS];
+ int32_t tagdist_list[TAG_NUM_IN_SYS];
static uint8_t tagpos_rec[50];
static uint64_t poll_rx_ts;
static uint8_t g_flag_Taggetdist[TAG_NUM_IN_SYS],flag_tag_distsmooth[TAG_NUM_IN_SYS],misdist_num[TAG_NUM_IN_SYS];
@@ -76,7 +76,7 @@
const double A = 121.74;
double min_power;
static dwt_rxdiag_t d1;
-static int32_t hex_dist,his_dist[TAG_NUM_IN_SYS];
+static int32_t hex_dist;
extern uint16_t g_Resttimer;
static int32_t filter_dist,g_Tagdist[TAG_NUM_IN_SYS];
double LOS(dwt_rxdiag_t *dia) {
@@ -139,6 +139,7 @@
*ts += ts_field[i] << (i * 8);
}
}
+int16_t his_dist[100];
void TagListUpdate(void)
{
uint16_t i,j=0,temp[TAG_NUM_IN_SYS];
@@ -147,7 +148,11 @@
if(tagofflinetime[i]++<QUIT_SLOT_TIME)
{
tagid_list[j]=tagid_list[i];
+// tagid_distlist[j] =
+ tagdist_list[j]=tagdist_list[i];
+ his_dist[j] = his_dist[i];
tagofflinetime[j++]=tagofflinetime[i];
+
}
}
taglist_num=j;
@@ -177,19 +182,19 @@
uint8_t Anchor_RecNearPoll(uint8_t ancrec_nearbasepos) //0 mainbase 1 first near_base
{
uint8_t motorstate;
- tmp_time=TIM3->CNT;
+ // tmp_time=TIM3->CNT;
temp_nearbaspos = ancrec_nearbasepos;
memcpy(&send_buffer[ANCHOR_ID_IDX],&dev_id,2);
memcpy(&send_buffer[ANCTIMEMS],&basesystimer,2);
- memcpy(&send_buffer[ANCTIMEUS],&tmp_time,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
if(rec_nearbase_num == ancrec_nearbasepos)//抢占模式
{
- random_time = (HIDO_TimerGetTick()%100);
- resp_tx_time = (poll_rx_ts+((random_time+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。
+ // random_time = (HIDO_TimerGetTick()%100);
+ 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。
}else{
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。
@@ -202,15 +207,15 @@
/* 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);
- g_com_map[DIST_OFFSET]=0;//测试记得删
+ //g_com_map[DIST_OFFSET]=0;//测试记得删
memcpy(&send_buffer[RESP_MSG_ANC_DISTOFFSET],&g_com_map[DIST_OFFSET],2);
- if(new_tagid)
- {
- tagdist_list[taglist_pos]=0x1ffff;
- memcpy(&send_buffer[DIST_IDX], &tagdist_list[taglist_pos], 4);
- }else{
- memcpy(&send_buffer[DIST_IDX], &tagdist_list[taglist_pos], 4);
- }
+// if(new_tagid)
+// {
+// tagdist_list[taglist_pos]=0x1ffff;
+// memcpy(&send_buffer[DIST_IDX], &tagdist_list[taglist_pos], 4);
+// }else{
+// memcpy(&send_buffer[DIST_IDX], &tagdist_list[taglist_pos], 4);
+// }
// motorstate =0;
// if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE1]||tagdist_list[taglist_pos]<0)
// {
@@ -510,7 +515,26 @@
// }
// }
}
+
+int16_t DistFilter(int16_t dist,int8_t pos)
+{
+ if(abs(dist-his_dist[pos])<1000||misdist_num[pos]>3)
+ {
+// if(tagdist_list[pos]>0&&tagdist_list[pos]<50000)
+// {
+// filter_dist=(dist/10)*0.5+0.5*tagdist_list[pos];
+// }else{
+// filter_dist = dist/10;
+// }
+ misdist_num[pos]=0;
+ his_dist[pos]=dist;
+ }else{
+ misdist_num[pos]++;
+ }
+ return his_dist[pos];
+}
uint16_t target_tagid[12] = {0x1316,0x1394,0x1265,0x1318,0x1306,0x1304,0x1376,0x1312,0x1277,0x1388,0x1140,0x1392};
+int16_t anchor_dist;
void Anchor_App(void)
{
u8 send_len,i;
@@ -558,13 +582,18 @@
{
if(rec_ancidlist[i] == dev_id)
{ //rec_ancdistlist[i]+=(int16_t)g_com_map[DIST_OFFSET];
- PushAnchorDataArray(tag_id_recv,rec_ancdistlist[i],-(int8_t)firstpath_power);
+
+ anchor_dist = rec_ancdistlist[i]+(int16_t)g_com_map[DIST_OFFSET];
+ break;
+ // PushAnchorDataArray(tag_id_recv,rec_ancdistlist[i],-(int8_t)firstpath_power);
// PushAnchorDataArray(tag_id_recv,rec_ancdistlist[i],battary);
}
}
taglist_pos=CmpTagInList(tag_id_recv);
- if(taglist_num>=TAG_NUM_IN_SYS)
+ if(rec_ancdistlist[i]!=-1)
+ tagdist_list[taglist_pos] = DistFilter(anchor_dist,taglist_pos);
+// if(taglist_num>=TAG_NUM_IN_SYS)
// break;
if(taglist_pos==taglist_num)
{
--
Gitblit v1.9.3