hm/gcs-nf
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

调通遥控器，但是dlink进入死锁

Browse Source
This commit is contained in:
hm
2021-01-13 21:51:51 +08:00
parent f56f004774
commit 222df390c6
21 changed files with 233 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
dlink/dlink.cpp
+16 -30
View File
@@ -15,7 +15,7 @@ DLink::DLink(QObject *parent) : QObject(parent)
//在当前线程运行
connect(mavlinknode,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
this,SLOT(SendMessageTo(quint8,quint8*,quint16)),Qt::BlockingQueuedConnection);//采用直连的方式，因为是多线程
this,SLOT(SendMessageTo(quint8,quint8*,quint16)),Qt::QueuedConnection);//BlockingQueuedConnection);//采用直连的方式，因为是多线程
//在当前线程运行
connect(mavlinknode,SIGNAL(showMessage(QString,int)),
@@ -25,6 +25,14 @@ DLink::DLink(QObject *parent) : QObject(parent)
connect(this,SIGNAL(recieveMessage(quint32,QByteArray)),
mavlinknode,SLOT(setbuff(quint32,QByteArray)),Qt::DirectConnection);
connect(this,SIGNAL(getRTK(QByteArray)),
mavlinknode,SLOT(readRTK(QByteArray)),Qt::DirectConnection);
connect(this,SIGNAL(getRemote(QByteArray)),
mavlinknode,SLOT(readRemote(QByteArray)),Qt::DirectConnection);
mavlinknode->start();
//其他协议节点。。。
}
@@ -491,28 +499,23 @@ void DLink::processPendingMulticastDatagrams(void)
void DLink::readRTKPendingDatagrams(void)
{
bool rtk_sending = true;
QObject *obj = sender();
QSerialPort *serialport = qobject_cast<QSerialPort *>(obj);
if(serialport)
{
QByteArray datagram = serialport->readAll();
if (rtk_sending)
if(datagram.size() > 0)
{
rtkrawdata.append(datagram);
if (rtkrawdata.size() > 2048)
{
rtkrawdata.mid(2048);
}
emit getRTK(datagram);
}
}
}
void DLink::readRemotePendingDatagrams(void)
{
bool remote_enabled = true;
QObject *obj = sender();
QSerialPort *serialport = qobject_cast<QSerialPort *>(obj);
@@ -520,30 +523,13 @@ void DLink::readRemotePendingDatagrams(void)
{
QByteArray datagram = serialport->readAll();
if (remote_enabled)
if(datagram.size() > 0)
{
for (QByteArray::const_iterator i = datagram.cbegin(); i != datagram.cend(); ++i)
{
if (sbus.parse_char(*i))
{
for (int i=0; i<18; ++i)
{
rcin[i] = sbus.Channel[i+1];
}
rcin_cnt = 10;
QList<uint16_t> list;
for (int i=0; i<10; ++i)
{
list.append(sbus.Channel[i]);
}
//emit new_remote_ctrl(list);
}
}
emit getRemote(datagram);
}
}
}
dlink/dlink.h
+6 -7
View File
@@ -7,7 +7,7 @@
#include <QUdpSocket>
#include "QDebug"
#include "mavlinknode.h"
#include "sbusparser.h"
QT_BEGIN_NAMESPACE
@@ -70,7 +70,8 @@ signals:
QVariant m_Param3,QVariant m_Param4,
QVariant m_Param5);
void getRemote(QByteArray);
void getRTK(QByteArray);
public slots:
@@ -110,10 +111,8 @@ protected:
};
SBusParser sbus;
uint16_t rcin[20];
int rcin_cnt = 0;
bool rcin_update = true;
QMap<uint32_t,unsigned> cnts_in;
QMap<uint32_t,unsigned> cnts_out;
@@ -134,7 +133,7 @@ protected:
qint32 multicast_transmit_port;
QByteArray rtkrawdata;
QMap<quint8,QIODevice *> Device;
dlink/dlink.pro
+2 -4
View File
@@ -45,12 +45,10 @@ DEFINES += QT_DEPRECATED_WARNINGS
HEADERS += \
$$PWD/dlink.h \
$$PWD/dlinkglobal.h \
sbusparser.h
$$PWD/dlinkglobal.h
SOURCES += \
$$PWD/dlink.cpp \
sbusparser.cpp
$$PWD/dlink.cpp
#加入第三方库
INCLUDEPATH += $$PWD/../thirdpart/include
dlink/sbusparser.cpp
-212
View File
@@ -1,212 +0,0 @@
#include "sbusparser.h"
#include "QDebug"
bool SBusParser::parse_char(uint8_t c)
{
bool rslt = false;
switch (status)
{
case 0u:
if (c == 0xEB)
{
++status;
}
break;
case 1u:
if (c == 0x90)
{
++status;
}
else
{
status = 0u;
}
break;
case 2u:
if (c == 0x0F)
{
Buf[0] = c;
idx = 1;
sum = c;
++status;
}
else
{
status = 0u;
}
break;
case 3u:
Buf[idx++] = c;
sum += c;
if (idx == 25u)
{
++status;
}
break;
case 4u:
seq = c;
sum += c;
++status;
break;
case 5u:
if (c == sum)
{
if (update())
{
rslt = true;
}
}
status = 0u;
break;
default:
status = 0u;
}
return rslt;
}
bool SBusParser::update(void)
{
uint16_t temp;
uint16_t ChannelTemp[17];
ChannelTemp[1] = ((Buf[2] & 0x07) << 8) | (Buf[1]);
ChannelTemp[2] = ((Buf[3] & 0x3F) << 5) | (Buf[2] >> 3);
ChannelTemp[3] = ((Buf[5] & 0x01) << 10)| (Buf[4] << 2) | (Buf[3] >> 6);
ChannelTemp[4] = ((Buf[6] & 0x0F) << 7) | (Buf[5] >> 1);
ChannelTemp[5] = ((Buf[7] & 0x7F) << 4) | (Buf[6] >> 4);
ChannelTemp[6] = ((Buf[9] & 0x03) << 9) | (Buf[8] << 1) | (Buf[7] >> 7);
ChannelTemp[7] = ((Buf[10] & 0x1F) << 6) | (Buf[9] >> 2);
ChannelTemp[8] = ((Buf[11] & 0xFF) << 3) | (Buf[10] >> 5);
ChannelTemp[9] = ((Buf[13] & 0x07) << 8) | (Buf[12]);
ChannelTemp[10] = ((Buf[14] & 0x3F) << 5) | (Buf[13] >> 3);
ChannelTemp[11] = ((Buf[16] & 0x01) << 10)| (Buf[15] << 2) | (Buf[14] >> 6);
ChannelTemp[12] = ((Buf[17] & 0x0F) << 7) | (Buf[16] >> 1);
ChannelTemp[13] = ((Buf[18] & 0x7F) << 4) | (Buf[17] >> 4);
ChannelTemp[14] = ((Buf[20] & 0x03) << 9) | (Buf[19] << 1) | (Buf[18] >> 7);
ChannelTemp[15] = ((Buf[21] & 0x1F) << 6) | (Buf[20] >> 2);
ChannelTemp[16] = ((Buf[22] & 0xFF) << 3) | (Buf[21] >> 5);
Channel[0] = seq;
for(int i = 1; i <= 16; ++i )
{
temp = (uint16_t)(0.606f * ChannelTemp[i] + 879.39f);
if (temp >=800 && temp <= 2200)
{
Channel_Old[FilterCount][i] = temp;
//qDebug() <<FilterFirstIni;
if(FilterFirstIni == 0)//先取5次数据再进行滤波处理
{
//去掉一个最大值和最小值
float ChannelSum = 0;
uint16_t MaxValue = 0,MinValue = 0xffff;
for(int j = 0;j<DeepOfFilter;j++)
{
MaxValue = (MaxValue>Channel_Old[j][i])?(MaxValue):(Channel_Old[j][i]);//fine(Channel_Old[j][i],MaxValue,0x00);//比较最大值
MinValue = (MinValue<Channel_Old[j][i])?(MinValue):(Channel_Old[j][i]);//fine(Channel_Old[j][i],MinValue,0x01);//比较最小值
ChannelSum += Channel_Old[j][i];
//qDebug() << j << FilterCount << i << MaxValue << MinValue <<ChannelSum;
}
ChannelSum = (ChannelSum - MaxValue - MinValue)/(DeepOfFilter - 2);
//如果存在野值，那么就用均值将野值代替掉
/*
if((MaxValue == Channel_Old[FilterCount][i])||(MinValue == Channel_Old[FilterCount][i]))
{
Channel_Old[FilterCount][i] = (uint16_t)ChannelSum;
}
*/
if (ChannelSum >=800 && ChannelSum <= 2200)
{
Channel[i] = ChannelSum;
}
}
else
{
if (temp >=800 && temp <= 2200)
{
Channel[i] = temp;
}
FilterFirstIni--;
if(FilterFirstIni <= 0)FilterFirstIni = 0;
}
//将计数值清零
FilterCount++;
if(FilterCount>=DeepOfFilter)
{
FilterCount = 0;
}
if(FilterFirstIni > 0)
{
return false;
}
}
else
{
return false;
}
}
if(Buf[23] & (1<<1))//bit 1
{
Channel[17] = 1900;
}
else
{
Channel[17] = 1100;
}
//DigiChannel 2
if(Buf[23] & (1))//bit 0
{
Channel[18] = 1900;
}
else
{
Channel[18] = 1100;
}
Channel[19] = (uint16_t)(Buf[23] & 0x0C);//signal is lost/fail/ok?
if (Channel[19])
{
return false;
}
return true;
}
//type == 0x00 比较大的值，type = 0x01,比较小的值
uint16_t SBusParser::fine(uint16_t data,uint16_t compare,uint8_t type)
{
uint16_t Max = 0,Min = 0xffff;
switch (type) {
case 0x00:
{
Max = (data >= compare)?(data):(compare);
}break;
case 0x01:
{
Min = (data <= compare)?(data):(compare);
}break;
}
return (type)?(Min):(Max);
}
dlink/sbusparser.h
-31
View File
@@ -1,31 +0,0 @@
#ifndef SBUSPARSER_H
#define SBUSPARSER_H
#include <stdint.h>
#include <stdbool.h>
#define DeepOfFilter 5
class SBusParser
{
public:
bool parse_char(uint8_t c);
uint16_t Channel[20];
private:
int16_t FilterFirstIni = DeepOfFilter;
uint16_t FilterCount = 0;
uint16_t Channel_Old[DeepOfFilter][20];//5次不跳跃才算做有效数据
uint8_t status = 0u;
unsigned int idx;
uint8_t Buf[25];
uint8_t sum;
uint8_t seq;
bool update(void);
uint16_t fine(uint16_t data,uint16_t compare,uint8_t type);
};
#endif // SBUSPARSER_H