未改动版本

chen

2024-11-12 cb6f4698ce2a52378d26249028ee33094b3db49f

 keil/simple_ss_twr_dw_init.c

¶Ô±ÈÐÂÎļþ
@@ -0,0 +1,401 @@
#include "mk_trace.h"
#include "mk_uwb.h"
#include "mk_phy.h"
#include "mk_misc.h"
#include "mk_power.h"
#include "mk_sleep_timer.h"
#include "lib_ranging.h"
#include "dw_app.h"
#include "board.h"

#if defined(MK_SS_TWR_DW_INIT)

extern int simple_main(void);

/* Ranging period: 1s */
#define RANGING_PERIOD_MS (1000)

/* This is the delay from the end of the poll frame transmission to the enable of the receiver */
#define POLL_TX_TO_RESP_RX_DLY_US 700U

#define RESP_RX_TO_FINAL_TX_DLY_US 550U

/* Receive response timeout */
#define RESP_RX_TIMEOUT_US 500U

/* Field index in frame */
#define MSG_SEQ_NUM_IDX 2
#define FINAL_MSG_POLL_TX_TS_IDX 10
#define FINAL_MSG_RESP_RX_TS_IDX 14
#define FINAL_MSG_FINAL_TX_TS_IDX 18
uint16_t CmpTagInList(uint16_t tagid);
uint16_t AddNewTagIntoList(uint16_t tagid);
void AnchorListUpdate(void);
void Rank_ANchor_list_by_dis(void);
struct mk_uwb_configure
{
    uint8_t phy_work_mode; /* PHY_TX / PHY_RX / PHT_TX|PHY_RX */
    struct UWB_CONFIG_T phy_cfg;
};
extern uint32_t dev_id;
extern uint8_t group_id;
/* Default communication configuration. */
static struct mk_uwb_configure config = {
    .phy_work_mode = (uint8_t)(PHY_TX | PHY_RX),
    .phy_cfg.ch_num = 5,                      /* Channel number.                           */
    .phy_cfg.code_index = 9,                  /* TRX preamble code                         */
    .phy_cfg.mean_prf = MEAN_PRF_64M,         /* Mean prf 64/128/256M                      */
    .phy_cfg.data_bit_rate = DATA_BR_6M8,     /* Data rate 6.8M                            */
    .phy_cfg.sync_sym = PREAM_LEN_128,        /* Preamble duration, length of preamble 128 */
    .phy_cfg.sfd_sym = BPRF_NSFD2_8,          /* Identifier for SFD sequence               */
    .phy_cfg.ranging_bit = 1,                 /* ranging bit set 1                         */
    .phy_cfg.trx_mode = TRX_MODE_15_4Z_BPRF,  /* IEEE802.15.4z - BPRF mode                 */
    .phy_cfg.sts_pkt_cfg = STS_PKT_CFG_0,     /* SP0 Frame                                 */
    .phy_cfg.sts_segnum = STS_SEGNUM_BPRF_1,  /* Number of STS segments in the frame       */
    .phy_cfg.sts_seglen = STS_SEGLEN_BPRF_64, /* Number of symbols in an STS segment       */
    .phy_cfg.rx_ant_id = UWB_RX_ANTENNA_3,    /* UWB RX antenna port                       */
};

/* Buffer to store received frame */
static uint8_t rx_buf[128];
static uint8_t uwb_sendbuffer[150];
/*     Frames used in the ranging process
 *     Poll message:
 *     - byte 0 - 1: 0x8841 to indicate a data frame using 16-bit addressing.
 *     - byte 2: sequence number, incremented for each new frame.
 *     - byte 3 - 4: PAN Id 0x4B4d
 *     - byte 5 - 6: Destination address
 *     - byte 7 - 8: Source address
 *     - byte 9: Message type (0x02 RANGING_POLL / 0x03 RANGING_RESPONSE / 0x04 RANGING_FINAL)
 *     Response message:
 *     - byte 10: activity code (0x07 to tell the initiator to go on with the ranging exchange)
 *     Final message:
 *     - byte 10 - 13: poll message transmission timestamp.
 *     - byte 14 - 17: response message reception timestamp.
 *     - byte 18 - 21: final message transmission timestamp.
 */
static uint8_t tx_poll_msg[] = {0x41, 0x88, 0, 0x4D, 0x4B, 0x53, 0x45, 0x4D, 0x49, 0x02};
static uint8_t rx_resp_msg[] = {0x41, 0x88, 0, 0x4D, 0x4B, 0x4D, 0x49, 0x53, 0x45, 0x03, 0x07};
static uint8_t tx_final_msg[] = {0x41, 0x88, 0, 0x4D, 0x4B, 0x53, 0x45, 0x4D, 0x49, 0x04, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

/* Count value of phy counter when transmitting and receiving frames */
static uint32_t poll_tx_en_start_u32;
static uint32_t resp_rx_en_start_u32;
static uint32_t final_tx_en_start_u32;

/* 41 bits timestamps of frames transmission/reception. */
static int64_t poll_tx_ts_i64;
static int64_t resp_rx_ts_i64;
static int64_t final_tx_ts_i64;

/* Frame sequence number, incremented after each transmission. */
static uint8_t frame_seq_nb = 0;

static volatile uint16_t rx_state;
static volatile uint16_t rx_length;
//DW
uint16_t rec_nearbaseid,temp_index;
uint8_t temp_tag_num;
int32_t test2;
static uint8_t receive_flag=0;
static uint8_t rec_tag_index,tag_num_tosend;
enum SIMPLE_FSM_T
{
    SIMPLE_IDLE = 0,
    SIMPLE_POLL = 1,
    SIMPLE_RESPONSE = 2,
    SIMPLE_FINAL = 3,
};
void uwb_poll_buffer_construct(void)
{

}

static enum SIMPLE_FSM_T state = SIMPLE_IDLE;

/**
 * @brief Correct TX timestamp of the ranging frame.
 *
 * @param[in] timestamp    PHY timer count of TX
 * @return TX timestamp (unit: 15.65ps)
 */
static int64_t ranging_tx_time_correct(uint32_t timestamp)
{
    int64_t tx_timestamp = ranging_tx_time(timestamp);

    // correct antenna delay (TX using the same antenna as RX)
    tx_timestamp += ranging_ant_delays_get(config.phy_cfg.rx_ant_id) / 2;

    return tx_timestamp;
}

/**
 * @brief Correct RX timestamp of the ranging frame.
 *
 * @param[in] ind    MAC RX report
 * @return RX timestamp (unit: 15.65ps)
 */
static int64_t ranging_rx_time_correct(const struct MAC_HW_REPORT_T *ind)
{
    int64_t rx_timestamp = ranging_rx_time(ind);

    // correct antenna delay
    rx_timestamp -= ranging_ant_delays_get(config.phy_cfg.rx_ant_id) / 2;

    return rx_timestamp;
}

/* RX done process handler. */
static void rx_int_callback(struct MAC_HW_REPORT_T *rx_report)
{
    // Power off radio
    power_off_radio();

    rx_state = rx_report->err_code;

    /** UWB RX success */
    if (rx_state == UWB_RX_OK)
    {
        /* Received data does not contain FCS */
        rx_length = rx_report->pkt_len;
        memcpy(rx_buf, rx_report->pkt_data, rx_length);
        /* Calculate rx timestamp */
        resp_rx_ts_i64 = ranging_rx_time_correct(rx_report);
        receive_flag=1;
    }
    else
    {
        /* UWB_PLD_ERR     payload error             */
        /* UWB_PHR_ERR     PHR error                 */
        /* UWB_SFD_ERR     Sfd error                 */
        /* UWB_BD_ERR      Preamble detection error  */
        /* UWB_TO_ERR      Receive timeout           */
        /* UWB_STS_ERR     STS error                 */
        rx_length = 0;
        receive_flag=2;
    }

}

/* TX done process handler. */
static void tx_int_callback(struct MAC_HW_REPORT_T *tx_report)
{
    // Power off radio
    power_off_radio();

    /** UWB TX success */
    if (tx_report->err_code == UWB_TX_OK)
    {
    }
}


uint32_t start_receive_count,end_receive_count,poll_timeout,current_count;
uint16_t  taglist_total_num,taglist_current_index;  //当前列表总数量
uint8_t  taglist_keeptime[MAX_TAG_LIST_NUM]; //每个标签存活时间
uint16_t taglist_id[MAX_TAG_LIST_NUM],taglist_dist[MAX_TAG_LIST_NUM]; //标签距离和标签的ID
uint8_t rec_tag_num,temp_tag_num,kp1;
int32_t mainbase_dist,nearbase_distlist[MAX_NEARBASE_NUM],true_nearbase_distlist[MAX_NEARBASE_NUM],true_exsistbase_list[MAX_NEARBASE_NUM],ancsync_time,nextpoll_delaytime,offsettimeus;
uint8_t anclost_times=0 , mainbase_lost_count=0,exsistbase_list[MAX_NEARBASE_NUM],get_newbase=0,nearbase_num,last_nearbase_num,next_nearbase_num;
uint16_t nearbaseid_list[MAX_NEARBASE_NUM],mainbase_id,true_nearbase_idlist[MAX_NEARBASE_NUM],rec_nearbaseid,rec_nearbasepos;

//注意：因为新注册的标签并不知道基站已经收到了，所以要在下一包立刻将新的标签ID发出，否则标签会一直抢占基站的poll包。
//方法是将新注册的标签添加到下次发送的标签列表头部，其他标签依次向后挪动。
uint16_t AddNewTagIntoList(uint16_t tagid)
{
    uint16_t newindex_cal = taglist_current_index+25;//当前索引+25，为下次索引要发出的值的最大值，如果总数小于这个值，即可放到最后面。
    if(newindex_cal>=taglist_total_num) //如果当前索引是最后一包，那么就加到总索引的后面，如果不是就加到下一次起始索引位置。
    {
        newindex_cal = taglist_total_num;

    } else {
        for(uint16_t i=taglist_total_num; i>newindex_cal; i--) //从后往前挪地方??就是插入
        {
            taglist_id[i]=taglist_id[i-1];
            taglist_dist[i]=taglist_dist[i-1];
            taglist_keeptime[i]=taglist_keeptime[i-1];
        }
    }
    taglist_total_num++;
    taglist_id[newindex_cal] = tagid;
    taglist_dist[newindex_cal] = 0;
    taglist_keeptime[newindex_cal] = TAG_KEEPTIMES;
    return newindex_cal;
}
uint8_t FindNearBasePos(uint16_t baseid)//寻找当前列表中的基站返回索引
{
    uint8_t i;
    for(i=0; i<nearbase_num; i++)
    {
        if(baseid==nearbaseid_list[i])
            return i;
    }
    if(i==nearbase_num)
        return nearbase_num;
}

void AnchorListUpdate(void)
{
    next_nearbase_num = 0;
    int j=0;
    for(int i=0; i<nearbase_num; i++)
    {
        if(exsistbase_list[i]>0)
        {
            exsistbase_list[i]--;
            next_nearbase_num++;
            true_exsistbase_list[j]=exsistbase_list[i];
            true_nearbase_idlist[j]=nearbaseid_list[i];
            true_nearbase_distlist[j++]=nearbase_distlist[i];

        }
        nearbase_num = next_nearbase_num;//更新现存基站数
        last_nearbase_num = next_nearbase_num;//更新当前基站数
    }
}

void Rank_ANchor_list_by_dis(void)//按照目前的基站距离进行冒泡排序用临时数组方式将最近的放到最前面，最远的放到最后面
{
    for(int i=0; i<last_nearbase_num-1; i++)
    {
        for(int j=0; j<last_nearbase_num-1; j++)
        {
            if(true_nearbase_distlist[j]>true_nearbase_distlist[j+1])
            {
                uint32_t temp_dist,temp_id,temp_exsis;
                temp_dist=true_nearbase_distlist[j];
                temp_id = true_nearbase_idlist[j];
                temp_exsis=true_exsistbase_list[j];
                true_nearbase_distlist[j]=true_nearbase_distlist[j+1];
                true_nearbase_idlist[j]=true_nearbase_idlist[j+1];
                true_exsistbase_list[j]=true_exsistbase_list[j+1];

                true_nearbase_distlist[j+1]=temp_dist;
                true_nearbase_idlist[j+1]=temp_id;
                true_exsistbase_list[j+1]=temp_exsis;
            }
        }
    }
    for (int i=0; i<last_nearbase_num; i++)
    {
        nearbaseid_list[i]=true_nearbase_idlist[i];
        nearbase_distlist[i]=true_nearbase_distlist[i];
        exsistbase_list[i] = true_exsistbase_list[i];
    }
}
uint16_t CmpTagInList(uint16_t tagid)//找这个新一包的基站在不在现存的taglist中在的话对应位置更新存活时间，不在就增加（确保有效数量小于最大设置数）
{
    uint16_t temp;
    for(uint8_t i=0; i<taglist_total_num; i++)
    {
        if(memcmp(&tagid,&taglist_id[i],2)==0)
        {
            taglist_keeptime[i] = TAG_KEEPTIMES;
            return i;
        }
    }
    if(taglist_total_num<MAX_TAG_LIST_NUM)
        temp = AddNewTagIntoList(tagid);
    return temp;
}
int simple_main(void)
{
    // The following peripherals will be initialized in the uwb_open function
    // phy/mac/aes/lsp/phy timers initialized
    uwb_open();

    // Set calibration parameters
    uwb_calibration_params_set(config.phy_cfg.ch_num);

    // set advanced parameters
    struct PHY_ADV_CONFIG_T adv_config = {
        .thres_fap_detect = 40,
        .nth_scale_factor = 4,
        .ranging_performance_mode = 0,
        .dynamic_port_sel_en = 0,
    };
    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);

    ranging_frame_type_set(config.phy_cfg.sts_pkt_cfg);

    // Register rx interrupt callback function
    mac_register_process_handler(tx_int_callback, rx_int_callback);

//#if LOW_POWER_EN
//    // Initialize low power mode
//    power_init();
//    // Enable sleep timer
//    sleep_timer_open(true, SLEEP_TIMER_MODE_ONESHOT, NULL);
//#endif
    uwb_poll_buffer_construct();
    temp_tag_num=0;//临时数量为0
    poll_tx_en_start_u32 = phy_timer_count_get();
    uwb_tx(uwb_sendbuffer,sizeof(uwb_sendbuffer),0,0);//立即发送
    poll_tx_ts_i64 = ranging_tx_time_correct(poll_tx_en_start_u32 + phy_start_time_gap());//修正时间戳
    while(mac_is_busy());//等待发送完成

    resp_rx_en_start_u32 = poll_tx_en_start_u32 + US_TO_PHY_TIMER_COUNT(POLL_TX_TO_RESP_RX_DLY_US);//设置发送多少s后接收按target独立时间起点算+ US_TO_PHY_TIMER_COUNT(POLL_TX_TO_RESP_RX_DLY_US为发送后要多久开启接收时间
    start_receive_count=phy_timer_count_get();
    poll_timeout=tag_num_tosend*50000+50000;//多一个多0.4ms默认0.4ms计算为0.125*4*100000
    end_receive_count=start_receive_count+poll_timeout;
    if(end_receive_count>=124800000)
    {
        end_receive_count-=124800000;
    }
    current_count=phy_timer_count_get();
    uwb_rx(1, resp_rx_en_start_u32, RESP_RX_TIMEOUT_US);//开启接受并设置接收超时
    while(current_count<end_receive_count||current_count>end_receive_count+62400000)//循环接受包体,若为124.8K则是+62400000
    {
        current_count=phy_timer_count_get();//不断刷新当前计数器值
        if(receive_flag==1)//成功接收数据
        {
            receive_flag=0;
            //获取发射端时钟偏差
            test2=phy_freq_offset_get();
            //接收成功则判断是否为同一组
            if (rx_buf[MESSAGE_TYPE_IDX] == MBX_RESPONSE&&!memcmp(&rx_buf[TAG_ID_IDX],&dev_id,2)&&rx_buf[GROUP_ID_IDX]==group_id) //判断接收到的数据是否是response数据
            {
                memcpy(&rec_nearbaseid,&rx_buf[ANCHOR_ID_IDX],2);
                if(last_nearbase_num==0)
                {
                    nearbaseid_list[0]=rec_nearbaseid;
                    nearbase_num=1;
                    exsistbase_list[0]=KEEP_TIMES;
                    SetANCTimestap(0,&rx_buf[RESP_MSG_POLL_RX_TS_IDX],&rx_buf[RESP_MSG_RESP_TX_TS_IDX],(uint32_t)resp_rx_ts_i64,&rx_buf[RESP_MSG_ANC_DISTOFFSET],test2);//这个是改过的参数
                }
//                                                if(rec_nearbaseid==nearbaseid_list[0])
//                                                   {
//                                     exsistbase_list[0]=KEEP_TIMES;
//                                   SetANCTimestap(0,&rx_buf[RESP_MSG_POLL_RX_TS_IDX],&rx_buf[RESP_MSG_RESP_TX_TS_IDX],(uint32_t)resp_rx_ts_i64,&rx_buf[RESP_MSG_ANC_DISTOFFSET],test2);//这个是改过的参数
//                                                   }
                else
                {
                    rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
                    SetANCTimestap(rec_nearbasepos,&rx_buf[RESP_MSG_POLL_RX_TS_IDX],&rx_buf[RESP_MSG_RESP_TX_TS_IDX],(uint32_t)resp_rx_ts_i64,&rx_buf[RESP_MSG_ANC_DISTOFFSET],test2);
                    if(rec_nearbasepos>=last_nearbase_num)          //发现新的基站
                    {
                        //get_newbase=1;
                        nearbase_num++;
                        nearbaseid_list[rec_nearbasepos] = rec_nearbaseid;
                    }
                    exsistbase_list[rec_nearbasepos]=KEEP_TIMES;//更新存活时间
                }
                memset(rx_buf, 0, sizeof(rx_buf));//清空接收到的用完的数组
                rx_length=0;//清0长度
                uwb_rx(0, 0, 0);//立即开启接受并设置0超时
            } else if(receive_flag==2) { //接收出错
                receive_flag=0;
                uwb_rx(0, 0, 0);//立即开启接受并设置0超时
            }
        }
        //关闭UWBapi
        uwb_close();
        CalculateDists(poll_tx_ts_i64); //计算距离
        AnchorListUpdate();//更新存活基站列表
        Rank_ANchor_list_by_dis();//冒泡排序重新按距离重新排列基站列表最小距离存货基站放到最前面
    }
}
#endif