173 lines
3.8 KiB
C++
173 lines
3.8 KiB
C++
#include "rcprocess.h"
|
|
|
|
|
|
rcprocess::rcprocess(QObject *parent) : ThreadTemplet(parent)
|
|
{
|
|
setRunFrq(0.001);//20hz
|
|
|
|
|
|
rcin[0] = 1500;
|
|
rcin[1] = 1500;
|
|
rcin[2] = 1100;
|
|
rcin[3] = 1500;
|
|
rcin[4] = 1100;
|
|
rcin[5] = 1100;
|
|
rcin[6] = 1100;
|
|
rcin[7] = 1100;
|
|
rcin[8] = 1100;
|
|
rcin[9] = 1100;
|
|
rcin[10] = 1500;
|
|
rcin[11] = 1100;
|
|
rcin[12] = 1100;
|
|
for (int i=13; i<20; ++i)
|
|
{
|
|
rcin[i] = 1100;
|
|
}
|
|
|
|
isRCActive = 0;
|
|
|
|
}
|
|
|
|
rcprocess::~rcprocess()
|
|
{
|
|
qDebug() << "stop rc" << QThread::currentThreadId();
|
|
}
|
|
|
|
//这里一直在解码，一直检查双缓冲里面是否有数据，有就解码，没有就休息
|
|
void rcprocess::process()//线程函数
|
|
{
|
|
QThread::msleep(5000);//5s后再发送
|
|
uint64_t lastTime = 0;
|
|
uint8_t count = 0;
|
|
QByteArray datagram = nullptr;
|
|
while (true)
|
|
{
|
|
count ++;
|
|
|
|
QThread::msleep(1000/frq());
|
|
|
|
//解码从串口来的
|
|
datagram.clear();
|
|
datagram = readbuff(SourceType::s_port);//每次全部读取
|
|
|
|
|
|
if(datagram.size() > 0)
|
|
{
|
|
//QApplication::processEvents();
|
|
parse(SourceType::s_port,datagram);
|
|
}
|
|
|
|
quint64 currentTimestamp = (quint64)QDateTime::currentMSecsSinceEpoch();
|
|
|
|
if((currentTimestamp - lastTime) > 66)
|
|
{
|
|
lastTime = currentTimestamp;
|
|
update15Hz();
|
|
}
|
|
|
|
if(isInterruptionRequested())//退出
|
|
{
|
|
break;
|
|
}
|
|
//QThread::yieldCurrentThread();
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
void rcprocess::parse(quint32 src,QByteArray datagram)
|
|
{
|
|
for (QByteArray::const_iterator i = datagram.cbegin(); i != datagram.cend(); ++i)
|
|
{
|
|
if(sbus.parse_char((uint8_t)*i))
|
|
{
|
|
rcin_update = true;
|
|
|
|
if(isRCActive == 1)
|
|
{
|
|
for (int i=0; i<18; ++i)
|
|
{
|
|
rcin[i] = sbus.Channel[i+1];
|
|
}
|
|
|
|
rcin_cnt = 13;
|
|
}
|
|
|
|
|
|
|
|
sbuslist.clear();
|
|
for (int i=0; i<13; ++i)
|
|
{
|
|
sbuslist.append(sbus.Channel[i]);
|
|
}
|
|
|
|
emit new_remote_ctrl(sbuslist);
|
|
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void rcprocess::update15Hz(void)
|
|
{
|
|
uint8_t buff[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)];
|
|
quint64 currentTimestamp = ((quint64)QDateTime::currentMSecsSinceEpoch()) * 1000;
|
|
qToBigEndian(currentTimestamp, buff);
|
|
mavlink_message_t msg;
|
|
|
|
if (rcin_update)
|
|
{
|
|
if ((isRCActive != 0) || rcin_cnt > 0)
|
|
{
|
|
rcin_cnt--;
|
|
mavlink_rc_channels_override_t command;
|
|
command.chan1_raw = rcin[0];
|
|
command.chan2_raw = rcin[1];
|
|
command.chan3_raw = rcin[2];
|
|
command.chan4_raw = rcin[3];
|
|
command.chan5_raw = rcin[4];
|
|
command.chan6_raw = rcin[5];
|
|
command.chan7_raw = rcin[6];
|
|
command.chan8_raw = rcin[7];
|
|
command.chan9_raw = rcin[8];
|
|
command.chan10_raw = rcin[9];
|
|
command.chan11_raw = rcin[10];
|
|
command.chan12_raw = rcin[11];
|
|
command.chan13_raw = rcin[12];
|
|
command.chan14_raw = rcin[13];
|
|
command.chan15_raw = rcin[14];
|
|
command.chan16_raw = rcin[15];
|
|
command.chan17_raw = rcin[16];
|
|
command.chan18_raw = rcin[17];
|
|
command.target_component = (uint8_t)isRCActive;
|
|
command.target_system = sysid;
|
|
mavlink_msg_rc_channels_override_encode(GCS_SysID,GCS_CompID,&msg,&command);
|
|
|
|
|
|
Send(msg);
|
|
rcin_update = false;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
void rcprocess::SoftRCUpdate(QList<uint16_t> rc)
|
|
{
|
|
if(isRCActive == 2)
|
|
{
|
|
rcin_update = true;
|
|
for (int i=0; i<18; ++i)
|
|
{
|
|
rcin[i] = rc.at(i+1);
|
|
|
|
}
|
|
|
|
rcin_cnt = 10;
|
|
}
|
|
}
|
|
|
|
|