解决MK 溢出导致联网错误问题，差分数据问题，开机关闭卫星GSV功能，准备迁移到MK-UBK-通用分支

yincheng.zhong

3 天以前 42764e1824563c32798d7bbf592a47dd2db8dce2

 keil/uwb_app.c

@@ -72,12 +72,12 @@
uint8_t uwb_rx_flag;
extern Operation_step UWB_work_state;
extern int16_t first_search_flag;
typedef enum 
typedef enum
{
      SEARCH,
    SEARCH,
    CLOSE,
    RANGE,
}enumwltagstate;
} enumwltagstate;
#define TAGFREQ_IDX                  7

/* Ranging period */
@@ -242,9 +242,9 @@
};
#endif

static struct anchor_id_car{
 uint16_t anchor_new_id;
 uint16_t change_num;
static struct anchor_id_car {
    uint16_t anchor_new_id;
    uint16_t change_num;
};
/* Buffer to store received frame */

@@ -335,14 +335,14 @@
uint8_t rxnumoknum,rxnumerrornum;
uint8_t resp_tx_flag;
static void rx_int_callback(struct MAC_HW_REPORT_T *rx_report)
{      uint8_t valid_sts=0;
{   uint8_t valid_sts=0;
    // Power off radio
    power_off_radio();
    rxnum++;
    

    /** UWB RX success */
    if (rx_report->err_code == UWB_RX_OK)
    {   
    {
        resp_tx_flag=0;
        rxnumoknum++;
        /* Received data does not contain FCS */
@@ -356,10 +356,10 @@
        poll_rx_num++;
        rssi = rx_report->rssi;
        receive_flag=1;
      Anchor_App();
        Anchor_App();
        if(resp_tx_flag==0)
        {
            OpenUWB();
            OpenUWB();
        }
//         #ifdef STS_MODE
//            valid_sts= sts_valid_check();
@@ -386,12 +386,12 @@
        /* UWB_TO_ERR      Receive timeout           */
        /* UWB_STS_ERR     STS error                 */
        rxnumerrornum++;
                 temp_count= phy_timer_count_get();
        temp_count= phy_timer_count_get();
        memcpy(&rx_rpt, rx_report, sizeof(struct MAC_HW_REPORT_T));
        rx_length = 0;
        receive_error_flag=1;
	
               OpenUWB();

        OpenUWB();

//      OpenUWB();//再次开启UWB接收
    }
@@ -414,7 +414,7 @@
        temp_internal=temp_count;
        resp_tx_num++;

            OpenUWB();
        OpenUWB();

//          OpenUWB();//再次开启UWB接收
        //LOG_INFO(TRACE_MODULE_APP, "poll_rx_num is %d,resp_tx_num is %d\r\n",poll_rx_num,resp_tx_num);
@@ -422,7 +422,7 @@
//    OpenUWB();//再次开启UWB接收
}
uint32_t start_receive_count,end_receive_count,poll_timeout,current_count,temp_resp;
 uint16_t anchordata_id[TAG_NUM_IN_SYS],anchordata_dist[TAG_NUM_IN_SYS];
uint16_t anchordata_id[TAG_NUM_IN_SYS],anchordata_dist[TAG_NUM_IN_SYS];
static uint8_t anchordata_bat[TAG_NUM_IN_SYS];
static int8_t anchor_rssi[TAG_NUM_IN_SYS];
uint8_t anchordata_num = 0;
@@ -449,20 +449,20 @@
            anchordata_num++;
    }
    anchordata_id[i] = ancid;//与之通信的标签id存入当前表中
      distance=dist*0.5+(int16_t)anchordata_dist[i]*0.5;
    distance=dist*0.5+(int16_t)anchordata_dist[i]*0.5;
    anchordata_dist[i] = dist;
    anchordata_bat[i] = battary;
    tagofflinetime[i]=0;//不断更新当前TAG对应离线时间
      dist_temp=dist;
    dist_temp=dist;
//    if(dist>0)
		

//      if(temp_recdist_before_offset!=0&&distance!=0x1ffff)
//      {
//    distance=dist*0.5+distance*0.5;
//      }else{
//         distance=dist;
//      }
		

}
//uint16_t CmpTagInList(uint16_t tagid)
//{   uint16_t i;
@@ -534,19 +534,19 @@
//}
void TagListUpdate(void)
{
uint16_t i,j=0,k=0;
    uint16_t i,j=0,k=0;
    for(i=0; i<taglist_num; i++)
    {
          if(tagofflinetime[i]++<TAG_KEEPTIMES)
        if(tagofflinetime[i]++<TAG_KEEPTIMES)
        {
            tagid_list[j]=tagid_list[i];
            tagdist_list[j] = tagdist_list[i];
                  anchordata_bat[j]=anchordata_bat[i];//电量随之更新
            anchordata_bat[j]=anchordata_bat[i];//电量随之更新
            anchor_rssi[j]=anchor_rssi[i];
            tagofflinetime[j++]=tagofflinetime[i];
        }
      }
       taglist_num=j;
    }
    taglist_num=j;
}

uint8_t position;
@@ -559,7 +559,7 @@
    memcpy(&send_buffer[ANCHOR_ID_IDX],&dev_id,2);//自身id
    memcpy(&send_buffer[TAG_ID_IDX],&tag_id_recv,2);//接受者id
    send_buffer[MESSAGE_TYPE_IDX]=MBX_RESPONSE;
     memcpy(&send_buffer[TAGFREQ_IDX],&tag_frequency,2);
    memcpy(&send_buffer[TAGFREQ_IDX],&tag_frequency,2);

    position=ancrec_nearbasepos;
    resp_tx_en_start_u32 = poll_rx_en_start_u32+US_TO_PHY_TIMER_COUNT(POLL_RX_TO_RESP_TX_DLY_US)+ancrec_nearbasepos*US_TO_PHY_TIMER_COUNT(DELAY_BETWEEN_TWO_FRAME_UUS);//加入帧间隔
@@ -572,7 +572,7 @@
    temp_count3= phy_timer_count_get();
    flag_temp2=uwb_tx(send_buffer, 40,1 ,resp_tx_en_start_u32);//立即发送测试size大小
    resp_tx_flag=1;
      tagofflinetime[taglist_pos] = 0;//更新标签通信
    tagofflinetime[taglist_pos] = 0;//更新标签通信
//    temp_count1=phy_timer_count_get();
//    temp_endcount1=phy_timer_count_get()+MS_TO_PHY_TIMER_COUNT(10);
//    while(mac_is_busy())
@@ -615,33 +615,33 @@
    return i;
}
void Calibration_Time(void)
{	
   current_count_calibration=phy_timer_count_get();
   if((current_count_calibration-start_receive_count_calibration>=MS_TO_PHY_TIMER_COUNT(500)||current_count_calibration+(UINT32_MAX-start_receive_count_calibration)>=MS_TO_PHY_TIMER_COUNT(500))
      &&!((end_receive_count-current_count_calibration<=MS_TO_PHY_TIMER_COUNT(200))||(end_receive_count+(UINT32_MAX-current_count_calibration)<=MS_TO_PHY_TIMER_COUNT(200))))
   {
      start_receive_count_calibration=current_count_calibration;//更新开始时间

		
      if(secondtask_search_count++%2==0)
{
    current_count_calibration=phy_timer_count_get();
    if((current_count_calibration-start_receive_count_calibration>=MS_TO_PHY_TIMER_COUNT(500)||current_count_calibration+(UINT32_MAX-start_receive_count_calibration)>=MS_TO_PHY_TIMER_COUNT(500))
            &&!((end_receive_count-current_count_calibration<=MS_TO_PHY_TIMER_COUNT(200))||(end_receive_count+(UINT32_MAX-current_count_calibration)<=MS_TO_PHY_TIMER_COUNT(200))))
    {
        secondtask_search_flag = 1;
    }else{
        secondtask_search_flag = 0;
    }
      if(secondtask_search_flag)//更新S时间TICK
      {
      HIDO_TimerTick();
      TagListUpdate();
        start_receive_count_calibration=current_count_calibration;//更新开始时间


        if(secondtask_search_count++%2==0)
        {
            secondtask_search_flag = 1;
        } else {
            secondtask_search_flag = 0;
        }
        if(secondtask_search_flag)//更新S时间TICK
        {
            HIDO_TimerTick();
            TagListUpdate();
//    GPS_Poll();
//      if(nomove_count<=g_com_map[NOMOVESLEEP_TIME])//防止溢出
//    nomove_count++;
//      else{
//      nomove_count=g_com_map[NOMOVESLEEP_TIME]+1;
//      }
      }
        }
//      update_led_power_state();//更新等状态防止震动卡死在搜索
   }
    }
}
uint16_t g_com_receive_id;
uint8_t usart_send[400];
@@ -650,34 +650,34 @@
uint16_t tagseq;
void CloseUWB(void)
{
      uwb_rx_force_off(1);
      uwb_rx_flag=0;
   //LOG_INFO(TRACE_MODULE_APP,"关闭uwb_rx\r\n");
    uwb_rx_force_off(1);
    uwb_rx_flag=0;
    //LOG_INFO(TRACE_MODULE_APP,"关闭uwb_rx\r\n");
}
void OpenUWB(void)
{
//        while(!mac_is_busy())
//        {
      flag_temp1=uwb_rx(0, 0,range_timeout_us);//提交接收请求
      uwb_rx_flag=1;
    flag_temp1=uwb_rx(0, 0,range_timeout_us);//提交接收请求
    uwb_rx_flag=1;
//        }
      //LOG_INFO(TRACE_MODULE_APP,"打开uwb_rx\r\n");
}	
    //LOG_INFO(TRACE_MODULE_APP,"打开uwb_rx\r\n");
}
void UWBOneSecondTask(void)
{
 TagListUpdate();//更新标签数
 uwb_losttimer++;
 if(uwb_losttimer>g_com_map[UWB_RNAGE_TIME])
 uwb_losttimer=g_com_map[UWB_RNAGE_TIME]+1;//防止溢出
    TagListUpdate();//更新标签数
    uwb_losttimer++;
    if(uwb_losttimer>g_com_map[UWB_RNAGE_TIME])
        uwb_losttimer=g_com_map[UWB_RNAGE_TIME]+1;//防止溢出
}
extern uint16_t gaodu;
void UWBSendUDPTask(void)
{
   uint16_t checksum,tempdistarray[20];
    uint16_t checksum,tempdistarray[20];
//    UDP_TEST();
    for(uint16_t i=0;i<taglist_num-1;i++)
    for(uint16_t i=0; i<taglist_num-1; i++)
    {
        for(uint16_t j=0;j<taglist_num-1-i;j++)
        for(uint16_t j=0; j<taglist_num-1-i; j++)
        {
            if(tagdist_list[j]>tagdist_list[j+1])
            {
@@ -698,7 +698,7 @@
                anchor_rssi[j+1] = rssi1;
            }
        }
    

    }
//    if(taglist_num>0)
//    {
@@ -710,34 +710,34 @@
//        }
//    }
//    Set4LEDColor(uwbled,rtkled,led4g,powerled);
    

    if(taglist_num>14)       //最多上传20个基站数据，距离最近的20个
        taglist_num = 14;
                    usart_send[0]= 0x55;
                    usart_send[1]= 0xaa;
               usart_send[2] = 0x12;//正常模式 
               usart_send[3] = 15+5*(taglist_num);//数据段长度
               memcpy(&usart_send[4],&dev_id,2);
               usart_send[6] = tagseq;
               usart_send[7] = (tagseq++)>>8;
               usart_send[8] = bat_percent;
    usart_send[0]= 0x55;
    usart_send[1]= 0xaa;
    usart_send[2] = 0x12;//正常模式
    usart_send[3] = 15+5*(taglist_num);//数据段长度
    memcpy(&usart_send[4],&dev_id,2);
    usart_send[6] = tagseq;
    usart_send[7] = (tagseq++)>>8;
    usart_send[8] = bat_percent;
//                    usart_send[8] = rxnum;
               usart_send[9] = userkey_state|0<<1|0<<2|0<<5|uwb_state<<6;
            //   memcpy(&usart_send[10],&rec_tagheight,2);
                    memcpy(&usart_send[12],gaodu,2);
    usart_send[9] = userkey_state|0<<1|0<<2|0<<5|uwb_state<<6;
    //   memcpy(&usart_send[10],&rec_tagheight,2);
    memcpy(&usart_send[12],gaodu,2);
//               usart_send[12] = 0;
//               usart_send[13] = 0;
               usart_send[14] = 0;
               usart_send[15] = 0;
               usart_send[16] = taglist_num;
				 
               memcpy(&usart_send[17],&tagid_list,2*taglist_num);
               memcpy(&usart_send[17+taglist_num*2],&tagdist_list,2*taglist_num);
                    memcpy(&usart_send[17+taglist_num*4],&anchordata_bat,taglist_num);
               checksum = Checksum_u16(&usart_send[2],15+5*taglist_num);
               memcpy(&usart_send[17+5*taglist_num],&checksum,2);
               HexToAsciiSendUDP(usart_send,19+5*taglist_num);
//                    taglist_num = 0;                    
    usart_send[14] = 0;
    usart_send[15] = 0;
    usart_send[16] = taglist_num;

    memcpy(&usart_send[17],&tagid_list,2*taglist_num);
    memcpy(&usart_send[17+taglist_num*2],&tagdist_list,2*taglist_num);
    memcpy(&usart_send[17+taglist_num*4],&anchordata_bat,taglist_num);
    checksum = Checksum_u16(&usart_send[2],15+5*taglist_num);
    memcpy(&usart_send[17+5*taglist_num],&checksum,2);
    HexToAsciiSendUDP(usart_send,19+5*taglist_num);
//                    taglist_num = 0;
//                    rxnum=0;
}
extern uint32_t step_count;
@@ -755,46 +755,46 @@
//    receive_error_flag=0;
//    OpenUWB();
//    }
   if(HIDO_TimerGetTick()-uwbtasktimer >=1)
   {
      step_count = mir3da_get_step(); // 获取步数
      uwbtasktimer = HIDO_TimerGetTick();
      UWBOneSecondTask();
      if(g_com_map[UWBFrequency]==1&&uwb_OpenClose_flag==1)
    if(HIDO_TimerGetTick()-uwbtasktimer >=1)
    {
        step_count = mir3da_get_step(); // 获取步数
        uwbtasktimer = HIDO_TimerGetTick();
        UWBOneSecondTask();
        if(g_com_map[UWBFrequency]==1&&uwb_OpenClose_flag==1)
        {
      UWBSendUDPTask();
            UWBSendUDPTask();
        }
   }
    }
}
int uwb_app_poll(void)
{
    UWBIdleTask();
    UWBIdleTask();
}
extern uint8_t uwb_offtime_count;
int Anchor_App(void)
{
           uint8_t i;
         uint16_t tempid;
          uwb_offtime_count=0;
   #ifdef  CEJU_ZIUMIAN
    current_state = STATE_NORMAL;
   #endif
         if(group_id==rx_buf[GROUP_ID_IDX]&&rx_buf[MESSAGE_TYPE_IDX] == MBX_POLL)//判断是否是和自己是同一组通讯的且为poll包
        {    flag_recsuccess = 1;
                  wltag_state=RANGE;//成功后从search切换为range
                  memcpy(&g_com_receive_id,&rx_buf[TAG_ID_IDX],2);
                 //temp_count2=phy_timer_count_get();
                  uwb_losttimer=0;//成功测距清0 lost时间
            frame_seq_nb2 = rx_buf[SEQUENCE_IDX];//获取包序
            battary = rx_buf[BATTARY_IDX];
            rec_nearbase_num=rx_buf[FZ_NEARBASENUM_INDEX];  //标签传过来的他与基站交互的基站id数目
            memcpy(&tag_id_recv,&rx_buf[TAG_ID_IDX],2);
            memcpy(rec_ancidlist,&rx_buf[FZ_NEARBASEID_INDEX],2*rec_nearbase_num);
            memcpy(rec_ancdistlist,&rx_buf[FZ_NEARBASEID_INDEX+2*rec_nearbase_num],2*rec_nearbase_num);
            memcpy(&rec_antdelay,&rx_buf[FZ_NEARBASEID_INDEX+rec_nearbase_num*4],2);
            uwbled=BLUE;
					  
					  
    uint8_t i;
    uint16_t tempid;
    uwb_offtime_count=0;
#ifdef  CEJU_ZIUMIAN
    current_state = STATE_NORMAL;
#endif
    if(group_id==rx_buf[GROUP_ID_IDX]&&rx_buf[MESSAGE_TYPE_IDX] == MBX_POLL)//判断是否是和自己是同一组通讯的且为poll包
    {   flag_recsuccess = 1;
        wltag_state=RANGE;//成功后从search切换为range
        memcpy(&g_com_receive_id,&rx_buf[TAG_ID_IDX],2);
        //temp_count2=phy_timer_count_get();
        uwb_losttimer=0;//成功测距清0 lost时间
        frame_seq_nb2 = rx_buf[SEQUENCE_IDX];//获取包序
        battary = rx_buf[BATTARY_IDX];
        rec_nearbase_num=rx_buf[FZ_NEARBASENUM_INDEX];  //标签传过来的他与基站交互的基站id数目
        memcpy(&tag_id_recv,&rx_buf[TAG_ID_IDX],2);
        memcpy(rec_ancidlist,&rx_buf[FZ_NEARBASEID_INDEX],2*rec_nearbase_num);
        memcpy(rec_ancdistlist,&rx_buf[FZ_NEARBASEID_INDEX+2*rec_nearbase_num],2*rec_nearbase_num);
        memcpy(&rec_antdelay,&rx_buf[FZ_NEARBASEID_INDEX+rec_nearbase_num*4],2);
        uwbled=BLUE;


//            for(i=0; i<rec_nearbase_num; i++)
//            {
//                if(rec_ancidlist[i] == dev_id)
@@ -819,85 +819,85 @@
//                seize_anchor=1;   //抢占anchor
//                Anchor_RecNearPoll(i);
//            }
                        taglist_pos=CmpTagInList(tag_id_recv);
                if(taglist_num>=ANC_MAX_NUM)
               return 0;
                         for(i=0; i<rec_nearbase_num; i++)
                        {
                              memcpy(&tempid,&rx_buf[FZ_NEARBASEID_INDEX+i*2],2);
                              if(tempid==dev_id)
                              {
                                    Anchor_RecNearPoll(i);
                                    tagdist_list[taglist_pos]=rec_ancdistlist[i];
                                    anchordata_bat[taglist_pos] = battary;//保存该基站电量
                        anchor_rssi[taglist_pos]=rssi;
                                    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)||(rec_nearbase_num==0&&!ExistInTagList(tag_id_recv)))//当基站不在标签列表中，标签也不在基站列表中时，随机找个时间片回复基站。
                {
                       Anchor_RecNearPoll(rec_nearbase_num);
        taglist_pos=CmpTagInList(tag_id_recv);
        if(taglist_num>=TAG_NUM_IN_SYS)
            return 0;
        for(i=0; i<rec_nearbase_num; i++)
        {
            memcpy(&tempid,&rx_buf[FZ_NEARBASEID_INDEX+i*2],2);
            if(tempid==dev_id)
            {
                Anchor_RecNearPoll(i);
                tagdist_list[taglist_pos]=rec_ancdistlist[i];
                anchordata_bat[taglist_pos] = battary;//保存该基站电量
                anchor_rssi[taglist_pos]=rssi;
                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)||(rec_nearbase_num==0&&!ExistInTagList(tag_id_recv)))//当基站不在标签列表中，标签也不在基站列表中时，随机找个时间片回复基站。
        {
            Anchor_RecNearPoll(rec_nearbase_num);
        }
    }
//                else
//                {
//                OpenUWB();
//                }
				
				


}

void Uwb_init(void)
{
      uwb_open();
   #ifdef STS_MODE
 // Set STS key and IV
    uwb_open();
#ifdef STS_MODE
// Set STS key and IV
    phy_sts_key_configure(&sts_iv_key);
	
   // which RX ports will be used for AoA/PDoA

    // which RX ports will be used for AoA/PDoA
    phy_rx_ant_mode_set(RX_ANT_PORTS_COMBINATION);
   #endif
#endif
    // Set calibration parameters
    uwb_calibration_params_set(config.phy_cfg.ch_num);
   #ifndef STS_MODE
   // set advanced parameters
#ifndef STS_MODE
    // set advanced parameters
    struct PHY_ADV_CONFIG_T adv_config = {
        .thres_fap_detect = 40,
        .nth_scale_factor = 4,
        .ranging_performance_mode = 0,
        .skip_weakest_port_en = 0,
    };
   #else
    // set advanced parameters
#else
    // set advanced parameters
    struct PHY_ADV_CONFIG_T adv_config = {
        .thres_fap_detect = 40,
        .nth_scale_factor = 4,
        .ranging_performance_mode = 3,
        .skip_weakest_port_en = 0,
    };
   #endif
#endif
    phy_adv_params_configure(&adv_config);

    // uwb configure
    uwb_configure(config.phy_work_mode, board_param.tx_power_fcc[CALIB_CH(config.phy_cfg.ch_num)], &config.phy_cfg);
      #if (defined STS_MODE) || (defined MK_MODE)
      ranging_lib_init();
      #endif
#if (defined STS_MODE) || (defined MK_MODE)
    ranging_lib_init();
#endif
    ranging_frame_type_set(config.phy_cfg.sts_pkt_cfg);
      #ifdef STS_MODE
      aoa_aux_info_set(AOA_AUX_ANT_IQ_RSSI_PDOA_AOA_FOM);
#ifdef STS_MODE
    aoa_aux_info_set(AOA_AUX_ANT_IQ_RSSI_PDOA_AOA_FOM);
    aoa_steering_vector_set((const float *)((uint32_t)((config.phy_cfg.ch_num == 9) ? svec_ch9_ptr : svec_ch5_ptr) | SRAM_BASE));

    aoa_param_config();
		
      phy_rx_sts_switch_mode_set(config.phy_cfg.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
      #endif
      mac_register_process_handler(tx_int_callback, rx_int_callback);//初始化回调

    phy_rx_sts_switch_mode_set(config.phy_cfg.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
#endif
    mac_register_process_handler(tx_int_callback, rx_int_callback);//初始化回调
}