V1.46增加MODBUS自动回复功能

zhyinch

2021-11-16 e3dfcf2b4eafdb5bace59d796a270f8e8d532763

 Ô´Âë/ºËÐÄ°å/Src/application/dw_app.c

@@ -30,6 +30,8 @@
#include "beep.h"
#include "modbus.h"
//#define DEBUG_OUTPUT
#define TDFILTER
//#define CONSTANT_FILTER
/*------------------------------------ Marcos ------------------------------------------*/
/* Inter-ranging delay period, in milliseconds. */
#define RNG_DELAY_MS 100
@@ -77,6 +79,8 @@
#define BATTARY_IDX                  15
#define BUTTON_IDX                  16
#define SEQUENCE_IDX               17
#define DISTNUM_INDEX                   18
#define DISTARRAY_INDEX                 19
//respose
#define DIST_IDX                      10
#define ANCTIMEMS             14
@@ -342,15 +346,45 @@
 //   min_power =  - 10 * log10((F1 *F1 + F2 * F2 + F3 * F3) / (C *B));
    return min_power;
  }
uint16_t g_Resttimer;
#define CONSTANT_LEN 50
extern u16 dist_threshold;
int32_t ConstantFilter(int32_t currentdist,u8 channel)
  {
      static int32_t cs_lastdist[10],cs_lastvalid[10];
      static u8 cfstart_flag[10] = {1};
      static u8 constant_count[10] = {100};
      if(cfstart_flag[channel])
      {
        cfstart_flag[channel] = 0;
        cs_lastdist[channel] = currentdist;
      }
      if( abs(currentdist - cs_lastdist[channel])<100)
      {
          if(constant_count[channel]<CONSTANT_LEN)
              constant_count[channel]++;     
      }else{
         constant_count[channel] = 0; 
      }
      if(constant_count[channel] == CONSTANT_LEN)
      {
        cs_lastvalid[channel] = currentdist;
      }
      cs_lastdist[channel] = currentdist;
      return cs_lastvalid[channel];
  }
#define TAGARRAY_LEN 20
uint16_t g_Resttimer,tag_IDarray[TAGARRAY_LEN];
uint8_t result;
u8 tag_succ_times=0;
int32_t hex_dist,hex_dist2;
int32_t hex_dist,hex_dist2,tag_distarray[TAGARRAY_LEN];
u16 checksum;
int8_t tag_delaytime;
extern uint16_t sync_timer;
u16 tmp_time;
u32 getdatatimer[TAGARRAY_LEN];
#define DATALOSTTIME 2000
extern float dw_vbat;
extern u8 supergroupid,modbusread_start;
extern u16 slottime,max_slotnum,current_slotpos,tyncpoll_time;
void Tag_App(void)//发送模式(TAG标签)
{
@@ -370,14 +404,24 @@
   tx_poll_msg[BATTARY_IDX] = bat_percent;//Get_Battary();
   tx_poll_msg[BUTTON_IDX] = !READ_KEY0;
   tx_poll_msg[SEQUENCE_IDX] = frame_seq_nb++;
    tx_poll_msg[DISTNUM_INDEX] = g_com_map[MAX_REPORT_ANC_NUM];
    for(i=0;i<g_com_map[MAX_REPORT_ANC_NUM];i++)
   {
        if(time32_incr-getdatatimer[i]>DATALOSTTIME)
        {
            tag_distarray[i] = 0x1ffff;
        }
    }
    memcpy(&tx_poll_msg[DISTARRAY_INDEX],tag_IDarray,2*g_com_map[MAX_REPORT_ANC_NUM]);
    memcpy(&tx_poll_msg[DISTARRAY_INDEX+2*g_com_map[MAX_REPORT_ANC_NUM]],tag_distarray,4*g_com_map[MAX_REPORT_ANC_NUM]);
   GPIO_WriteBit(GPIOA, GPIO_Pin_9, Bit_RESET);
   for(i=0;i<g_com_map[MAX_REPORT_ANC_NUM];i++)
   {
   /* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */
   tx_poll_msg[ANC_TYPE_IDX] = i;
      
   dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
   dwt_writetxfctrl(sizeof(tx_poll_msg), 0);//设置超宽带发送数据长度
   dwt_writetxdata(22+6*g_com_map[MAX_REPORT_ANC_NUM], tx_poll_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
   dwt_writetxfctrl(22+6*g_com_map[MAX_REPORT_ANC_NUM], 0);//设置超宽带发送数据长度

   /* Start transmission, indicating that a response is expected so that reception is enabled automatically after the frame is sent and the delay
    * set by dwt_setrxaftertxdelay() has elapsed. */
@@ -414,7 +458,7 @@
      /* Check that the frame is the expected response from the companion "DS TWR responder" example.
       * As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
      
      if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //判断接收到的数据是否是response数据
      if ((rx_buffer[GROUP_ID_IDX]==supergroupid||rx_buffer[GROUP_ID_IDX] == group_id||group_id==supergroupid)&&rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //判断接收到的数据是否是response数据
      { u16 anc_id_recv,rec_com_interval;
         /* Retrieve poll transmission and response reception timestamp. */
         poll_tx_ts = get_tx_timestamp_u64();                              //获得POLL发送时间T1
@@ -435,7 +479,9 @@
         }
         TIM3->CNT=tmp_time;
      }
         memcpy(&hex_dist2, &rx_buffer[DIST_IDX], 4);
         memcpy(&hex_dist2, &rx_buffer[DIST_IDX], 4);        
            memcpy(&tag_distarray[i], &rx_buffer[DIST_IDX], 4);
            memcpy(&tag_IDarray[i],&rx_buffer[ANCHOR_ID_IDX],2);
            rec_firstpath_power = rx_buffer[SIGNALPOWER];
         memcpy(&tx_final_msg[ANCHOR_ID_IDX], &rx_buffer[ANCHOR_ID_IDX], 4);
         memcpy(&rec_com_interval,&rx_buffer[ANCSEND_INTERVAL],  2);
@@ -468,7 +514,7 @@
             #ifdef DEBUG_OUTPUT
            printf("F包发送,基站ID: %d .\r\n",i);
            #endif
            
            getdatatimer[i] = time32_incr;
         tag_succ_times++;
         
            LED0_BLINK;
@@ -496,6 +542,8 @@
               memcpy(&usart_send[19],&checksum,2);
               UART_PushFrame(usart_send,21);
               }
              Modbus_HoldReg[anc_id_recv*2]=hex_dist2>>16;
         Modbus_HoldReg[anc_id_recv*2+1]=hex_dist2;
            }
      //         memcpy(&Modbus_HoldReg[anc_id_recv*2],&hex_dist,4);
         /* Poll DW1000 until TX frame sent event set. See NOTE 8 below. */
@@ -535,15 +583,20 @@
   }

   /* Execute a delay between ranging exchanges. */
	
   if(modbusread_start)
   {
      Modbus_03_Solve();
   }
}

int8_t correction_time;
extern uint8_t sync_seq;
#define TDFILTER

//#define CHECK_UID
extern uint8_t UID_ERROR;
extern u16 dist_threshold;
u16 ancrec_IDarray[TAGARRAY_LEN];
u32 ancrec_distarray[TAGARRAY_LEN];
u32 ancrec_distnum;
u8 misdist_num[TAG_NUM_IN_SYS];
void Anchor_App(void)
{
@@ -587,7 +640,7 @@
//         tag_recv_interval = tag_recv_timer + 65535 - tag_time_recv[tag_id_recv];
//      }
      
      if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == POLL&&(anchor_type == rx_buffer[ANC_TYPE_IDX])) //判断是否是poll包数据
      if ((rx_buffer[GROUP_ID_IDX]==supergroupid||rx_buffer[GROUP_ID_IDX] == group_id||group_id==supergroupid)&&rx_buffer[MESSAGE_TYPE_IDX] == POLL&&(anchor_type == rx_buffer[ANC_TYPE_IDX])) //判断是否是poll包数据
      {
         tmp_time=TIM3->CNT;
         memcpy(&tx_resp_msg[ANCTIMEMS],&sync_timer,2);
@@ -598,7 +651,9 @@
                     
         /* Retrieve poll reception timestamp. */
         poll_rx_ts = get_rx_timestamp_u64();//获得Poll包接收时间T2

            ancrec_distnum = rx_buffer[DISTNUM_INDEX];
            memcpy(ancrec_IDarray,&rx_buffer[DISTARRAY_INDEX],2*g_com_map[MAX_REPORT_ANC_NUM]);
            memcpy(ancrec_distarray,&rx_buffer[DISTARRAY_INDEX+2*g_com_map[MAX_REPORT_ANC_NUM]],4*g_com_map[MAX_REPORT_ANC_NUM]);
         /* Set send time for response. See NOTE 8 below. */
         resp_tx_time = (poll_rx_ts + (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//计算Response发送时间T3。
         dwt_setdelayedtrxtime(resp_tx_time);//设置Response发送时间T3
@@ -645,7 +700,7 @@
            /* Check that the frame is a final message sent by "DS TWR initiator" example.
             * As the sequence number field of the frame is not used in this example, it can be zeroed to ease the validation of the frame. */
         
            if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,4)&&!memcmp(&rx_buffer[ANCHOR_ID_IDX],&dev_id,4)) //判断是否为Final包
            if ((rx_buffer[GROUP_ID_IDX]==supergroupid||rx_buffer[GROUP_ID_IDX] == group_id||group_id==supergroupid)&&rx_buffer[MESSAGE_TYPE_IDX] == FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,4)&&!memcmp(&rx_buffer[ANCHOR_ID_IDX],&dev_id,4)) //判断是否为Final包
            {
               uint32_t poll_tx_ts, resp_rx_ts, final_tx_ts;
               uint32_t poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
@@ -694,7 +749,7 @@
               if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
               {
               if(abs(hex_dist-his_dist[tag_id_recv-TAG_ID_START])<dist_threshold||misdist_num[tag_id_recv-TAG_ID_START]>4)
               {
               {u8 i;
                  int32_t filter_dist;
                  misdist_num[tag_id_recv-TAG_ID_START]=0;
               if(hex_dist<1000000&&hex_dist>-10000)
@@ -705,31 +760,63 @@
                  #else
                  filter_dist=hex_dist/10;
                  #endif
                        
                        #ifdef  CONSTANT_FILTER
                        filter_dist = ConstantFilter(filter_dist,tag_id_recv-TAG_ID_START);
                        #endif                       
                  anchor_dist_last_frm[tag_id_recv-TAG_ID_START]=filter_dist;
                  g_Tagdist[tag_id_recv]=   filter_dist;
               

                        
               his_dist[tag_id_recv-TAG_ID_START]=hex_dist;
               g_flag_Taggetdist[tag_id_recv]=0;
               if(!g_com_map[MODBUS_MODE])
               {
                  usart_send[2] = 1;//正常模式
                  usart_send[3] = 17;//数据段长度
                  usart_send[4] = frame_seq_nb2;//数据段长度
                  memcpy(&usart_send[5],&tag_id_recv,2);
                  memcpy(&usart_send[7],&dev_id,2);
						
                  memcpy(&usart_send[9],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
                  usart_send[13] = battary;
                  usart_send[14] = button;
                        usart_send[15] = firstpath_power;
                  checksum = Checksum_u16(&usart_send[2],17);
                  memcpy(&usart_send[19],&checksum,2);
                  UART_PushFrame(usart_send,21);
               { 
                       for(i=0;i<g_com_map[MAX_REPORT_ANC_NUM];i++)
                        {
                            if(ancrec_distarray[i] !=0x1ffff)
                            {
                               usart_send[2] = 1;//正常模式
                                usart_send[3] = 17;//数据段长度
                                usart_send[4] = frame_seq_nb2;//数据段长度
                                memcpy(&usart_send[5],&tag_id_recv,2);
                                memcpy(&usart_send[7],&ancrec_IDarray[i],2);
                                
                                memcpy(&usart_send[9],&ancrec_distarray[i],4);
                                usart_send[13] = battary;
                                usart_send[14] = button;
                                usart_send[15] = firstpath_power;
                                checksum = Checksum_u16(&usart_send[2],17);
                                memcpy(&usart_send[19],&checksum,2);
                                UART_PushFrame(usart_send,21);
                            
                            }
                        }
//                  usart_send[2] = 1;//正常模式
//                  usart_send[3] = 17;//数据段长度
//                  usart_send[4] = frame_seq_nb2;//数据段长度
//                  memcpy(&usart_send[5],&tag_id_recv,2);
//                  memcpy(&usart_send[7],&dev_id,2);
//						
//                  memcpy(&usart_send[9],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
//                  usart_send[13] = battary;
//                  usart_send[14] = button;
//                        usart_send[15] = firstpath_power;
//                  checksum = Checksum_u16(&usart_send[2],17);
//                  memcpy(&usart_send[19],&checksum,2);
//                  UART_PushFrame(usart_send,21);
               }   
               //   memcpy(&Modbus_HoldReg[tag_id_recv*2],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
               Modbus_HoldReg[tag_id_recv*2]=g_Tagdist[tag_id_recv-TAG_ID_START]>>16;
               Modbus_HoldReg[tag_id_recv*2+1]=g_Tagdist[tag_id_recv-TAG_ID_START];
               //dis_after_filter = LP_Frac_Update(p_Dis_Filter, dist_cm);
					
//               Modbus_HoldReg[tag_id_recv*2]=g_Tagdist[tag_id_recv-TAG_ID_START]>>16;
//               Modbus_HoldReg[tag_id_recv*2+1]=g_Tagdist[tag_id_recv-TAG_ID_START];
                     for(i=0;i<g_com_map[MAX_REPORT_ANC_NUM];i++)
                        {
                           Modbus_HoldReg[tag_id_recv*3*g_com_map[MAX_REPORT_ANC_NUM]+3*i] = ancrec_IDarray[i];
                           Modbus_HoldReg[tag_id_recv*3*g_com_map[MAX_REPORT_ANC_NUM]+3*i+1] = ancrec_distarray[i]>>16;
                           Modbus_HoldReg[tag_id_recv*3*g_com_map[MAX_REPORT_ANC_NUM]+3*i+2] = ancrec_distarray[i];
                        }
                    
                  }
               
               }