1对6

zhyinch

2022-03-01 617d989d294c30b5f04f643b1ec7e5d9b4878a1a

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

@@ -15,6 +15,7 @@
 */

#include <string.h>
#include <math.h>
#include "dw_app.h"
#include "deca_device_api.h"
#include "deca_regs.h"
@@ -29,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
@@ -48,7 +51,7 @@
 * frame length of approximately 2.66 ms with above configuration. */
#define RESP_RX_TO_FINAL_TX_DLY_UUS 400
/* Receive response timeout. See NOTE 5 below. */
#define RESP_RX_TIMEOUT_UUS 600
#define RESP_RX_TIMEOUT_UUS 9600

#define POLL_RX_TO_RESP_TX_DLY_UUS 420
/* This is the delay from the end of the frame transmission to the enable of the receiver, as programmed for the DW1000's wait for response feature. */
@@ -81,12 +84,19 @@
#define ANCTIMEMS             14
#define ANCTIMEUS             16
#define ANCSEND_INTERVAL      18
#define SIGNALPOWER           20
//DATA POLL
#define CURENTPACKNUM_IDX          7
#define TOTALPACKNUM_IDX         8
#define DATA_IDX                      10


#define POLL                    0x01
#define RESPONSE                0x02
#define FINAL                  0x03
#define SYNC                     0x04

#define DATA_POLL                     0x21
#define DATA_RESPONSE                    0x22
/*------------------------------------ Variables ------------------------------------------*/
/* Default communication configuration. We use here EVK1000's default mode (mode 3). */
static dwt_config_t config = {
@@ -104,12 +114,12 @@

/* Frames used in the ranging process. See NOTE 2 below. */
static uint8_t tx_poll_msg[20] = {0};
static uint8_t tx_sync_msg[14] = {0};
static uint8_t tx_sync_msg[140] = {0};
//static uint8_t rx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0, 0, 0};
static uint8_t tx_final_msg[24] = {0};
   
//static uint8_t rx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0};
static uint8_t tx_resp_msg[22] = {0};
static uint8_t tx_resp_msg[23] = {0};
//static uint8_t rx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
   
/* Frame sequence number, incremented after each transmission. */
@@ -120,7 +130,7 @@
   
/* Buffer to store received response message.
 * Its size is adjusted to longest frame that this example code is supposed to handle. */
#define RX_BUF_LEN       24
#define RX_BUF_LEN       224
static uint8_t rx_buffer[RX_BUF_LEN];
   
/* Time-stamps of frames transmission/reception, expressed in device time units.
@@ -287,6 +297,7 @@
   memcpy(&tx_final_msg[TAG_ID_IDX], &dev_id, 4);
   memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &dev_id, 4);
   memcpy(&tx_sync_msg[ANCHOR_ID_IDX], &dev_id, 4);
   memcpy(&tx_sync_msg[TAG_ID_IDX], &dev_id, 4);
   
   memcpy(&tx_resp_msg[ANCSEND_INTERVAL], &g_com_map[COM_INTERVAL], 2);
}   
@@ -320,6 +331,53 @@
   dwt_writetxfctrl(sizeof(tx_sync_msg), 0);//设置超宽带发送数据长度
   dwt_starttx(DWT_START_TX_IMMEDIATE);
}
 double firstpath_power, rx_power,rec_firstpath_power;
  double f1, f2, r1, r2;
uint16_t F1,F2,F3,N,C;
double B = 131072;
double A = 121.74;
double min_power;
 dwt_rxdiag_t d1;
double LOS(dwt_rxdiag_t *dia) {
     F1 = dia->firstPathAmp1;
     F2 = dia->firstPathAmp2;
     F3 = dia->firstPathAmp3;
     N = dia->rxPreamCount;
     C = dia->maxGrowthCIR;
    
    firstpath_power=10* log10((F1*F1+F2*F2+F3*F3)/(N*N))-A;
//    rx_power=10*log10(C*B/(N*N))-A;

 //   min_power =  - 10 * log10((F1 *F1 + F2 * F2 + F3 * F3) / (C *B));
    return min_power;
  }
#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];
  }

uint16_t g_Resttimer;
uint8_t result;
u8 tag_succ_times=0;
@@ -327,14 +385,15 @@
u16 checksum;
int8_t tag_delaytime;
extern uint16_t sync_timer;
u16 tmp_time;
u16 tmp_time;	
u32 start_poll;
extern float dw_vbat;
extern u16 slottime,max_slotnum,current_slotpos,tyncpoll_time;
void Tag_App(void)//发送模式(TAG标签)
{
   uint32_t frame_len;
   uint32_t final_tx_time;
   u32 start_poll;

   u8 i,getsync_flag=0;
   u8 bat_percent;
   //LED0_ON;
@@ -414,6 +473,7 @@
         TIM3->CNT=tmp_time;
      }
         memcpy(&hex_dist2, &rx_buffer[DIST_IDX], 4);
            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);
         if(rec_com_interval>4&&rec_com_interval!=g_com_map[COM_INTERVAL])
@@ -468,6 +528,7 @@
               memcpy(&usart_send[9],&hex_dist2,4);
               usart_send[13] = bat_percent;
               usart_send[14] = button;
                    usart_send[15] = rec_firstpath_power;
               checksum = Checksum_u16(&usart_send[2],17);
               memcpy(&usart_send[19],&checksum,2);
               UART_PushFrame(usart_send,21);
@@ -513,12 +574,13 @@
   /* Execute a delay between ranging exchanges. */
   
}

int8_t correction_time;
extern uint8_t sync_seq;
#define TDFILTER

//#define CHECK_UID
extern uint8_t UID_ERROR;
extern u16 dist_threshold;

u8 misdist_num[TAG_NUM_IN_SYS];
void Anchor_App(void)
{
@@ -585,7 +647,8 @@
         /* Write and send the response message. See NOTE 9 below.*/
         if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
         memcpy(&tx_resp_msg[DIST_IDX], &g_Tagdist[tag_id_recv], 4);
		
            tx_resp_msg[SIGNALPOWER] = firstpath_power;
            
         dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);//写入发送数据
         dwt_writetxfctrl(sizeof(tx_resp_msg), 0);//设定发送长度
         result = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//延迟发送，等待接收
@@ -655,7 +718,8 @@

               dist_cm = dist_no_bias * 1000; //dis ä¸ºå•ä½ä¸ºcm的距离
//               dist[TAG_ID] = LP(dis, TAG_ID); //LP ä¸ºä½Žé€šæ»¤æ³¢å™¨ï¼Œè®©æ•°æ®æ›´ç¨³å®š
					
               dwt_readdiagnostics(&d1);
               LOS(&d1);
               /*--------------------------以下为非测距逻辑------------------------*/
             #ifdef DEBUG_OUTPUT
            printf("收到FINAL包，标签ID: %d .\r\n",tag_id_recv);
@@ -678,9 +742,15 @@
                  #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])
@@ -694,6 +764,7 @@
                  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);
@@ -737,4 +808,302 @@
      dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
   }
}
u8 frame_len,jumptime;
u8 rec_gpsdata[1000];
extern u8 RTCMdata[2000];
u16 recgpsdata_i,recdata_len;
void UWBSendOnePackData(u8* data_addr,u8 len)
{
   delay_ms(jumptime);
   g_Resttimer = 0;
   LED0_BLINK;
   g_start_sync_flag=1;
   dwt_forcetrxoff();
   dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
  dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);		
   tx_sync_msg[MESSAGE_TYPE_IDX]=DATA_POLL;
   memcpy(&tx_sync_msg[DATA_IDX],data_addr,len);
   dwt_writetxdata(len+14, tx_sync_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
   dwt_writetxfctrl(len+14, 0);//设置超宽带发送数据长度
   dwt_starttx(DWT_START_TX_IMMEDIATE| DWT_RESPONSE_EXPECTED);
   start_poll = time32_incr;
   recgpsdata_i = 0;
   while(time32_incr - start_poll<100)
   {	
      while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到成功接收或者发生错误
   { 	
   };
   if(status_reg==0xffffffff)
   {
      NVIC_SystemReset();
   }

   if (status_reg & SYS_STATUS_RXFCG)//如果成功接收
   {
      /* Clear good RX frame event and TX frame sent in the DW1000 status register. */
      dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//清楚寄存器标志位

      /* A frame has been received, read it into the local buffer. */
      frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;   //获得接收到的数据长度

      dwt_readrxdata(rx_buffer, frame_len, 0);   //读取接收数据

      dwt_rxenable(0);
      /* 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[MESSAGE_TYPE_IDX] == DATA_RESPONSE ) //判断接收到的数据是否是response数据
      {
         recdata_len = frame_len-14;		
         memcpy(&rec_gpsdata[recgpsdata_i],&rx_buffer[DATA_IDX],recdata_len);
         recgpsdata_i+=recdata_len;
         if(rx_buffer[CURENTPACKNUM_IDX]==1)
         {
            USART_puts(rec_gpsdata,recgpsdata_i);
            break;
         }
      }else{
         jumptime=time32_incr%20;
      }
   }else{
      jumptime=time32_incr%20;
   }
}
}

char *HIDO_UtilStrnchr(const char *_pcStr, char _cChr, HIDO_UINT32 u32N)
{
    int32_t i = 0;
    for (i = 0; i < u32N; i++)
    {
        if (_pcStr[i] == _cChr)
        {
            return (char *) (_pcStr + i);
        }
    }

    return NULL;
}

HIDO_UINT8 HIDO_UtilCharToHex(HIDO_CHAR _cCh)
{
    return (_cCh >= '0' && _cCh <= '9') ?
            (_cCh - '0') :
            (_cCh >= 'A' && _cCh <= 'F' ?
                    (_cCh - 'A' + 0x0A) :
                    (_cCh >= 'a' && _cCh <= 'f' ? _cCh - 'a' + 0x0A : 0));
}

HIDO_UINT32 HIDO_UtilStrBufToInt(HIDO_CHAR *_pcStringBuf, HIDO_UINT32 _u32BufLen)
 {
    HIDO_UINT32 u32Temp = 0;

    while (_u32BufLen != 0)
    {
        u32Temp = u32Temp * 10 + HIDO_UtilCharToHex(*_pcStringBuf);
        _pcStringBuf++;
        _u32BufLen--;
    }

    return u32Temp;
}

/******************************************************************************
 Description    : Get gps latitude from gps data
 parameter      : data: gps data pointer, len: gps data length, gps: ST_GPS pointer
 Return         : TRUE on success, FALSE on fail
 author         : DuJian
 history        : 2012-7-27 new
 ******************************************************************************/
static int32_t GPS_ParseLat(HIDO_DataStruct *_pstLatData, double *lat)
{
    uint32_t u32Len = _pstLatData->m_u32Len;
    char *pcStart = (char *) _pstLatData->m_pData;
    char *pcDot = NULL;
    uint32_t u32TempLen = 0;
    double dd;
    double mmmm;
    double mm;

    if (u32Len < 9)
    {
        return HIDO_ERR;
    }

    pcDot = HIDO_UtilStrnchr(pcStart, '.', u32Len);
    if (NULL == pcDot || (pcDot - pcStart) != 4)
    {
        return HIDO_ERR;
    }

    dd = HIDO_UtilStrBufToInt(pcStart, 2);
    mm = HIDO_UtilStrBufToInt(pcStart + 2, 2);
    u32TempLen = u32Len - (pcDot + 1 - pcStart);
    mmmm = HIDO_UtilStrBufToInt(pcDot + 1, u32TempLen);
    while(u32TempLen != 0)
    {
        mmmm /= 10.0;
        u32TempLen--;
    }

    mm = mm + mmmm;
    *lat = dd + (mm / 60.0);

    return HIDO_OK;
}
static int32_t GPS_ParseHeight(HIDO_DataStruct *_pstLatData, double *height)
{
      uint32_t u32Len = _pstLatData->m_u32Len;
    char *pcStart = (char *) _pstLatData->m_pData;
    char *pcDot = NULL;
      uint32_t u32TempLen = 0;
      double dd;
    double mmmm;
    double mm;
	
      pcDot = HIDO_UtilStrnchr(pcStart, '.', u32Len);
      u32TempLen = pcDot - pcStart;
      mm = HIDO_UtilStrBufToInt(pcStart, u32TempLen);
     u32TempLen = u32Len - (pcDot + 1 - pcStart);
    mmmm = HIDO_UtilStrBufToInt(pcDot + 1, u32TempLen);
    while(u32TempLen != 0)
    {
        mmmm /= 10.0;
        u32TempLen--;
    }
      *height = mm + mmmm;
      return HIDO_OK;
}
/******************************************************************************
 Description    : Get gps longitude from gps data
 parameter      : data: gps data pointer, len: gps data length, gps: ST_GPS pointer
 Return         : TRUE on success, FALSE on fail
 author         : DuJian
 history        : 2012-7-27 new
 ******************************************************************************/
static int32_t GPS_ParseLon(HIDO_DataStruct *_pstLonData, double *lon)
{
    uint32_t u32Len = _pstLonData->m_u32Len;
    char *pcStart = (char *) _pstLonData->m_pData;
    char *pcDot = NULL;
    uint32_t u32TempLen = 0;
    double ddd;
    double mmmm;
    double mm;

    if (u32Len < 10)
    {
        return HIDO_ERR;
    }

    pcDot = HIDO_UtilStrnchr(pcStart, '.', u32Len);
    if (NULL == pcDot || (pcDot - pcStart) != 5)
    {
        return HIDO_ERR;
    }

    ddd = HIDO_UtilStrBufToInt(pcStart, 3);
    mm = HIDO_UtilStrBufToInt(pcStart + 3, 2);
    u32TempLen = u32Len - (pcDot + 1 - pcStart);
    mmmm = HIDO_UtilStrBufToInt(pcDot + 1, u32TempLen);
    while(u32TempLen != 0)
    {
        mmmm /= 10.0;
        u32TempLen--;
    }

    mm = mm + mmmm;
    *lon = ddd + (mm / 60.0);

    return HIDO_OK;
}

extern u8 gpsdataready_flag;
extern u16 gps_packlen;
u8 totalpack_num,currentpack_num;
u16 sendtimes;
void RecOnePackData(void)
{
   dwt_setrxtimeout(0);//设定接收超时时间，0位没有超时时间

   /* Activate reception immediately. */
   dwt_rxenable(0);//打开接收

   /* Poll for reception of a frame or error/timeout. See NOTE 7 below. */
   while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))&&!g_start_send_flag&&!g_start_sync_flag)//不断查询芯片状态直到接收成功或者出现错误
   { 
      IdleTask();
   };

   if (status_reg & SYS_STATUS_RXFCG)//成功接收
   { 
      u16 tag_recv_interval,send_i=0,remain_i=0;
      /* Clear good RX frame event in the DW1000 status register. */
      dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//清除标志位

      /* A frame has been received, read it into the local buffer. */
      frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//获得接收数据长度
      g_Resttimer = 0;
      dwt_readrxdata(rx_buffer, frame_len, 0);//读取接收数据
      if (rx_buffer[MESSAGE_TYPE_IDX] == DATA_POLL ) //判断接收到的数据是否是response数据
      {
         recdata_len = frame_len-14;	
         memcpy(rec_gpsdata,&rx_buffer[DATA_IDX],recdata_len);
      //   if(gpsdataready_flag)
         {
            gpsdataready_flag = 0;
            tx_sync_msg[MESSAGE_TYPE_IDX]=DATA_RESPONSE;
            remain_i = gps_packlen;
            currentpack_num = 0;
            totalpack_num = gps_packlen/110;
            while(remain_i>0)
            {
               if(remain_i>=110)
               {
                  tx_sync_msg[CURENTPACKNUM_IDX] = 0;
                  memcpy(&tx_sync_msg[DATA_IDX],&RTCMdata[send_i],110);
                  send_i+=110;
                  remain_i-=110;
                  dwt_writetxdata(110+14, tx_sync_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
                  dwt_writetxfctrl(110+14, 0);//设置超宽带发送数据长度
                  dwt_starttx(DWT_START_TX_IMMEDIATE);
                  sendtimes++;
               }else{
                  tx_sync_msg[CURENTPACKNUM_IDX] = 1;
                  memcpy(&tx_sync_msg[DATA_IDX],&RTCMdata[send_i],remain_i);
                  dwt_writetxdata(remain_i+14, tx_sync_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
                  dwt_writetxfctrl(remain_i+14, 0);//设置超宽带发送数据长度
                  dwt_starttx(DWT_START_TX_IMMEDIATE);
                  remain_i = 0;
                  sendtimes++;						
               }
               delay_us(1000);
            }
         }
         LED0_BLINK;
			
         extern HIDO_DataStruct stPosState[4];
			
         GPS_ParseGGA(rec_gpsdata,recdata_len);
			
         const char *fmt = "{\"battery\":4.2,\"dev_type\":\"11\",\"device_sn\":\"15625394\",\"gps_type\":%d,\"high\":%.8lf,\"lat\":%.8lf,\"lng\":%.8lf}";
			
         double lat = 0;
         double lon = 0;
         double high = 0;
         uint8_t gps_type;
         gps_type = HIDO_UtilStrBufToInt(stPosState[2].m_pData,1);
         if(gps_type!=0)
         {
         GPS_ParseLon(&stPosState[1], &lon);
         GPS_ParseLat(&stPosState[0], &lat);
         GPS_ParseHeight(&stPosState[3], &high);
         }
         printf(fmt,gps_type, high, lat, lon);
   //USART_puts(rec_gpsdata,recdata_len);
      }
   }else{
      dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
   }
	
}