129 lines
2.9 KiB
C++
129 lines
2.9 KiB
C++
#include "statusprocess.h"
|
|
|
|
//留出一个接口给界面去注册指令
|
|
//收到ACK后发送一个信号
|
|
|
|
statusprocess::statusprocess(QObject *parent) : ThreadTemplet(parent)
|
|
{
|
|
setRunFrq(1);
|
|
status.m_Mode = _modetype::Nop_Mode;
|
|
}
|
|
|
|
statusprocess::~statusprocess()
|
|
{
|
|
qDebug() << "stop status" << QThread::currentThreadId();
|
|
status.m_Mode = _modetype::Nop_Mode;
|
|
|
|
}
|
|
|
|
|
|
void statusprocess::process()//线程函数
|
|
{
|
|
uint8_t count = 0;
|
|
static int time = 0;
|
|
|
|
QThread::msleep(5000);//5s后再发送
|
|
|
|
while (true)
|
|
{
|
|
count ++;
|
|
|
|
|
|
switch(status.m_Mode)
|
|
{
|
|
default:
|
|
case Nop_Mode : QThread::msleep(1000.0/frq());break;
|
|
case RecieveMode : ReadStateMachine();break;//QApplication::processEvents();break;
|
|
case TransmitMode :
|
|
{
|
|
//QApplication::processEvents();
|
|
if((QTime::currentTime().msecsSinceStartOfDay() - time) > (1000.0/running_frq))
|
|
{
|
|
heartbeat(m_heartbeat.custom_mode,
|
|
m_heartbeat.type,
|
|
m_heartbeat.autopilot,
|
|
m_heartbeat.base_mode,
|
|
m_heartbeat.system_status,
|
|
m_heartbeat.mavlink_version);
|
|
time = QTime::currentTime().msecsSinceStartOfDay();
|
|
}
|
|
else
|
|
{
|
|
QThread::msleep(1000.0/frq());
|
|
//QThread::yieldCurrentThread();
|
|
}
|
|
}
|
|
}
|
|
|
|
if(isInterruptionRequested())//退出
|
|
{
|
|
break;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
//发送函数
|
|
void statusprocess::setHeartbeat(QVariant state,QVariant frq)
|
|
{
|
|
//给指令赋值
|
|
|
|
//开启线程开始传输
|
|
status.transmit.type = 0;
|
|
|
|
if(state.toBool() == true)
|
|
{
|
|
status.m_Mode = _modetype::TransmitMode;//发送模式
|
|
}
|
|
else
|
|
{
|
|
status.m_Mode = _modetype::Nop_Mode;//空闲模式
|
|
}
|
|
|
|
setRunFrq(frq.toFloat());
|
|
}
|
|
|
|
|
|
|
|
|
|
//读状态机
|
|
void statusprocess::ReadStateMachine(void)//没有读指令这个说法
|
|
{
|
|
|
|
}
|
|
|
|
//写状态机
|
|
void statusprocess::WriteStateMachine(void)
|
|
{
|
|
|
|
|
|
}
|
|
|
|
|
|
/*all msg
|
|
heartbeat
|
|
*/
|
|
|
|
|
|
|
|
void statusprocess::heartbeat(uint32_t custom_mode,uint8_t type,uint8_t autopilot,uint8_t base_mode,uint8_t system_status,uint8_t mavlink_version)
|
|
{
|
|
static mavlink_message_t msg;
|
|
static mavlink_heartbeat_t heartbeat;
|
|
|
|
heartbeat.custom_mode = custom_mode;
|
|
heartbeat.type = type;
|
|
|
|
heartbeat.autopilot = autopilot;
|
|
heartbeat.base_mode = base_mode;
|
|
heartbeat.system_status = system_status;
|
|
heartbeat.mavlink_version = mavlink_version;
|
|
|
|
mavlink_msg_heartbeat_encode(GCS_SysID,GCS_CompID, &msg,&heartbeat);
|
|
Send(msg);
|
|
}
|
|
|
|
|
|
|
|
|