BpHero-UWB上位机源码修改过程记录

BpHero-UWB上位机是从DECARANGERTLS PC 端源码修改来的,蓝点开放出来的代码,最基本的几个需求发现不能满足,比方:

基站ID修改为非0,1,2,自定义为其他的基站ID,程序就奔溃了;

修改后的效果图如下:

呱牛笔记

出现这个问题的主要原因是基站ID作为了数组的下标会用,一大就越界了,所以需要一个基站ID和下标值得映射关系:

class GraphicsWidget : public QWidget
{    QList<Anchor*> _anchorsList;    
    //返回anchor_id的序号
    int findAnchor(int anchId);
    
    }
void GraphicsWidget::addNewAnchor(quint64 anchId, bool show)    {
    //放到list中
    anc->idx = _anchorsList.length();
    anc->id = anchId;
    _anchorsList.append(anc);
}


int GraphicsWidget::findAnchor(int anchId){
    Anchor *anc = _anchors.value(anchId, NULL);
    if (!anc){
        return -1;
    }
    return anc->idx;
}

所以从这个出发点修改了一些代码,顺便对QT的语法有了基本的熟悉,大部分代码都是类C,所以还比较好懂,主要是slot和singal机制,看起来有点费劲,理解了就是一个回调函数的机制,例如:

void RTLSClient::processTagRangeReport(int aidx, int tid, int range, int lnum, int seq)
这个方法是处理标签通过串口上报的数据,然后处理完了就会通知界面绘制,这里就是通过emit事件回调通知的方式:

    emit tagRange(tid, aid, (range_corrected * 0.001)); //convert to meters
    
界面的代码:

void GraphicsWidget::tagRange(quint64 tagId, quint64 aId, double range)

建立关联是在:RTLSDisplayApplication::RTLSDisplayApplication 方法中:
    QObject::connect(_client, SIGNAL(tagRange(quint64,quint64,double)), graphicsWidget(), SLOT(tagRange(quint64,quint64,double)));

所有关联到基站ID和基站序号的地方,都对应需要做简单的调整:

void GraphicsWidget::anchorTableChanged(int r, int c)
{
    if(!_ignore)
    {
        _ignore = true;

        Anchor *anc = NULL;
        bool ok;
#if 0//for more than 1 anchor
        anc = _anchors.value(r, NULL);
#else
        if (_anchorsList.length() <= r){
            return;
        }
        anc = _anchorsList.value(r);
#endif//add end.
        anchPos(anc->id, xn, yn, zn, true, false);
                    

                    
}

void GraphicsWidget::anchorTableClicked(int r, int c)
{
    Anchor *anc = NULL;

#if 0
    anc = _anchors.value(r, NULL);
#else
    if (_anchorsList.length() <= r){
        return;
    }
    anc = _anchorsList.value(r);
#endif

void GraphicsWidget::insertAnchor(int ridx, double x, double y, double z,int *array, bool show)
{
    int i = 0;
    _ignore = true;

    //temp code add start.
    if (array == NULL){
        return;
    }
    Anchor *anc = _anchorsList.at(ridx);
    if (anc == NULL){
        return;
    }
    //add end.
    
    //显示出基站ID值 而不是序号
    pItem->setText(QString::number(anc->id));
    //.....
}

void GraphicsWidget::centerOnAnchors(void)
{
#if 0//for more then 3 anchors.
    Anchor *a1 = this->.value(0, NULL);
    Anchor *a2 = this->_anchors.value(1, NULL);
    Anchor *a3 = this->_anchors.value(2, NULL);
    //Anchor *a4 = anc = this->anchors.value(0, NULL);
    QPolygonF p1 = QPolygonF() << QPointF(a1->a->pos()) << QPointF(a2->a->pos()) << QPointF(a3->a->pos()) ;
#else
    QPolygonF p1 = QPolygonF();
    QMap<quint64, Anchor *>::iterator iter = _anchors.begin();
    int index = 0;
    while (iter != _anchors.end())
    {
        Anchor *a1 = iter.value();
        p1 << QPointF(a1->a->pos());

        iter++;
        index++;
    }
#endif


    emit centerRect(p1.boundingRect());

}


void GraphicsWidget::showGeoFencingMode(bool set)
{
    Anchor *anc;
    int i;

    if(set)
    {
        //place anchor ID 0 at 0,0
        Anchor *anc = _anchorsList.at(0);
        if (anc){
            anchPos(anc->id, a1x, a1y, a1z, true, false);
        }
        
    
    
        for(i=0; i<_anchorsList.length(); i++)
        {
            anc = this->_anchorsList.value(i);

            if(anc)
            {
                if(anc->show)
                {
                    anc->a->setOpacity(1);
                    anc->ancLabel->setOpacity(1);
                }
            }
        }    
}

RTLSClient.cpp

void RTLSClient::processAnchRangeReport(int aidx, int tid, int range, int lnum, int seq)
{
    QDateTime now = QDateTime::currentDateTime();
    QString nowstr = now.toString("T:hhmmsszzz:");
    //qDebug() << "a and t " << aid << tid << "correction = " << (_ancArray[aid].tagRangeCorection[tid] * 0.01);


#if 1
    if (aidx < 0 || aidx > MAX_NUM_ANCS){
        //找不到对应的基站位置数据
        return;
    }
    int aid = aidx;//
#else
    int aid = findAnchorIndex(origin_aid);
    if (aid == -1){
        //找不到对应的基站位置数据
        return;
    }
#endif

}

void RTLSClient::processTagRangeReport(int aidx, int tid, int range, int lnum, int seq)
{
#if 0
    if (aidx < 0 || aidx > MAX_NUM_ANCS){
        //找不到对应的基站位置数据
        return;
    }
    int aid = aidx;//
    int tid_inner = tid & 0x7;
#else
    int aid = findAnchorIndex(aidx);
    if (aid == -1){
        //找不到对应的基站位置数据
        return;
    }
    int tid_inner = tid & 0x7;
#endif
    int range_corrected = range + (_ancArray[aid].tagRangeCorection[tid_inner] * 10); //range correction is in cm (range is in mm)
    int idx = 0;
 
}
void RTLSClient::updateAnchorXYZ(int aidx, int x, double value)
{
#if 0
    int id = findAnchorIndex(origin_id);
    if (id == -1){
        //找不到对应的基站位置数据
        return;
    }
#else
    if (aidx < 0 || aidx > MAX_NUM_ANCS){
        //找不到对应的基站位置数据
        return;
    }
    int id = aidx;//
#endif//


}

int RTLSClient::findAnchorIndex(int origin_id){
    int i = 0;
#if 0
    //这里默认基站ID要大于基站个数的序号
    if (origin_id >= 0 && origin_id < MAX_NUM_ANCS){
        return origin_id;
    }
#endif//
    while (i < MAX_NUM_ANCS)
    {
        if (origin_id == _ancArray[i].id){
            return i;
        }
        i++;
    }
    return -1;
}

void RTLSClient::updateTagCorrection(int aid, int tid, int value)
{
    int id = findAnchorIndex(aid);
    if (id == -1){
        //找不到对应的基站位置数据
        return;
    }
    tid &= 0x7;
    _ancArray[id].tagRangeCorection[tid] = value;
}

int* RTLSClient::getTagCorrections(int anchID)
{
    int id = findAnchorIndex(anchID);
    if (id == -1){
        //找不到对应的基站位置数据
        return NULL;
    }
    return &_ancArray[id].tagRangeCorection[0];
}


修改串口上报的报文,出发点是,原来报文结构是标签距离0号基站多远,1号基站多远,2号基站多远,如果基站ID变化了,就不能适应,所以报文修改为了,基站Id(16位)+距离 基站ID+距离的格式:

#define frame_length 29//14+10
uint8_t frame[frame_length];
int ra[3];
void RTLSClient::ProcessData(void)
{
    uint8_t frame_type,TAG_ID,seq;
    uint16  range[4],lnum, anchor_id[4];
    frame_type = frame[0];
    TAG_ID = frame[1]|frame[2]<<8;
    lnum = frame[3];
    seq = lnum&0xff;

    char temp_str[1024] = "\0";
    int index = 0;
#if 0
    memset(temp_str, 0x00, sizeof(temp_str));
    for(int i=0; i<frame_length; i++){
        sprintf(temp_str+index, "%d ", frame[i]);
        index = strlen(temp_str);
    }
    qDebug() <<"test:"+QString(temp_str);
#endif
    if(frame_type==1)
    {
        range[0]=frame[6]|frame[7]<<8;
        range[1]=frame[8]|frame[9]<<8;
        range[2]=frame[10]|frame[11]<<8;
        range[0] = range[0]*10;
        range[1] = range[1]*10;
        range[2] = range[2]*10;
        if(_useAutoPos) //if Anchor auto positioning is enabled then process Anchor-Anchor TWR data
        {
            processAnchRangeReport(0, 1, range[0], lnum, seq);
            processAnchRangeReport(0, 2, range[1], lnum, seq);
            processAnchRangeReport(1, 2, range[2], lnum, seq);
        }
    }
    else if(frame_type == 2)
    {
        anchor_id[0] = frame[4]|frame[5]<<8;
        range[0]=frame[6]|frame[7]<<8;
        anchor_id[1] = frame[8]|frame[9]<<8;
        range[1]=frame[10]|frame[11]<<8;
        anchor_id[2] = frame[12]|frame[13]<<8;
        range[2]=frame[14]|frame[15]<<8;
        anchor_id[3] = frame[16]|frame[17]<<8;
        range[3]=frame[18]|frame[19]<<8;
        ra[0] = range[0]*10;
        ra[1] = range[1]*10;
        ra[2] = range[2]*10;
        ra[3] = range[3]*10;
        //下面这么处理是为了保准ra中的顺序对应是第0个基站、第1个基站、第2个基站这么个顺序
        for (int i=0; i<3; i++){
            if (anchor_id[i] == _ancArray[0].id){
                ra[0] = range[i]*10;
                ra[3] = range[i]*10;
                anchor_id[3] = _ancArray[0].id;
                range[3] = range[i];
            }else if (anchor_id[i] == _ancArray[1].id){
                ra[1] = range[i]*10;
            }else if (anchor_id[i] == _ancArray[2].id){
                ra[2] = range[i]*10;
            }
        }
        memset(temp_str, 0x00, sizeof(temp_str));
        index = 0;
        for(int i=0; i<3; i++){
            sprintf(temp_str+index, "%d:", anchor_id[i]);
            index = strlen(temp_str);
            sprintf(temp_str+index, "%d ", range[i]);
            index = strlen(temp_str);
        }

        qDebug() <<"distance:"+QString(temp_str);
        processTagRangeReport(anchor_id[0], TAG_ID, range[0]*10, lnum, seq);
        processTagRangeReport(anchor_id[1], TAG_ID, range[1]*10, lnum, seq);
        processTagRangeReport(anchor_id[2], TAG_ID, range[2]*10, lnum, seq);
        processTagRangeReport(anchor_id[3], TAG_ID, range[3]*10, lnum, seq);
        trilaterateTag(TAG_ID, seq);
    }
}


呱牛笔记

请先登录后发表评论
  • 最新评论
  • 总共0条评论