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);
}
}
本文为呱牛笔记原创文章,转载无需和我联系,但请注明来自呱牛笔记 ,it3q.com
请先登录后发表评论
- 最新评论
- 总共0条评论
