181 lines
3.9 KiB
C++
181 lines
3.9 KiB
C++
#include "statusprocess.h"
|
|
|
|
//留出一个接口给界面去注册指令
|
|
//收到ACK后发送一个信号
|
|
|
|
statusprocess::statusprocess(QObject *parent) : QObject(parent)
|
|
{
|
|
setRunFrq(1);
|
|
|
|
status.m_Mode = _modetype::Nop_Mode;
|
|
|
|
}
|
|
|
|
|
|
void statusprocess::setRunFrq(uint32_t frq)
|
|
{
|
|
if((frq != 0)||(frq <= 1000))
|
|
{
|
|
running_frq = frq;
|
|
qDebug() << "set command thread running frquency:" <<frq <<"Hz";
|
|
}
|
|
}
|
|
|
|
void statusprocess::start()
|
|
{
|
|
if(thread == nullptr)
|
|
{
|
|
thread = new QThread();
|
|
this->moveToThread(thread);
|
|
connect(thread, &QThread::started, this, &statusprocess::process);
|
|
}
|
|
|
|
if(!thread->isRunning())
|
|
{
|
|
running_flag = true;
|
|
thread->start();
|
|
qDebug() << "thread start" << running_flag;
|
|
}
|
|
else
|
|
{
|
|
qDebug() << "thread has started";
|
|
}
|
|
}
|
|
|
|
void statusprocess::stop()
|
|
{
|
|
if(thread->isRunning())
|
|
{
|
|
running_flag = false;
|
|
qDebug() << "thread stop"
|
|
<< QThread::currentThreadId()
|
|
<< QThread::currentThread()
|
|
<< "running state:"
|
|
<< running_flag;
|
|
|
|
disconnect(thread, nullptr, nullptr, nullptr);
|
|
|
|
}
|
|
else
|
|
{
|
|
qDebug() << "thread is not running";
|
|
}
|
|
}
|
|
|
|
void statusprocess::process()//线程函数
|
|
{
|
|
uint8_t count = 0;
|
|
|
|
while (running_flag)
|
|
{
|
|
count ++;
|
|
QThread::msleep(1000.0/running_frq);
|
|
|
|
switch(status.m_Mode)
|
|
{
|
|
default:
|
|
case Nop_Mode :break;
|
|
case RecieveMode : ReadStateMachine();break;
|
|
case TransmitMode :
|
|
{
|
|
heartbeat(m_heartbeat.custom_mode,
|
|
m_heartbeat.type,
|
|
m_heartbeat.autopilot,
|
|
m_heartbeat.base_mode,
|
|
m_heartbeat.system_status,
|
|
m_heartbeat.mavlink_version);
|
|
|
|
//qDebug() << "heartbeat";
|
|
}
|
|
}
|
|
}
|
|
//退出线程
|
|
disconnect(thread, nullptr, nullptr, nullptr);
|
|
thread->quit();
|
|
//thread->wait(200);//等待结束
|
|
thread->deleteLater();
|
|
thread = nullptr;
|
|
}
|
|
|
|
void statusprocess::setID(int m_sysid,int m_compid)
|
|
{
|
|
sysid = (uint8_t)m_sysid;
|
|
compid = (uint8_t)m_compid;
|
|
}
|
|
|
|
void statusprocess::SendMessage(mavlink_message_t msg)
|
|
{
|
|
uint8_t buff[256+20];
|
|
uint16_t len = mavlink_msg_to_send_buffer(buff, &msg);
|
|
emit SendMessageTo(0,buff, len);//使用信号和槽
|
|
}
|
|
|
|
//发送函数
|
|
void statusprocess::setHeartbeat(QVariant state,QVariant frq)
|
|
{
|
|
//给指令赋值
|
|
|
|
qDebug() << state << frq;
|
|
|
|
//开启线程开始传输
|
|
status.transmit.type = 0;
|
|
|
|
if(state.toBool() == true)
|
|
{
|
|
status.m_Mode = _modetype::TransmitMode;//发送模式
|
|
}
|
|
else
|
|
{
|
|
status.m_Mode = _modetype::Nop_Mode;//发送模式
|
|
}
|
|
|
|
setRunFrq(frq.toInt());
|
|
|
|
start();//开启线程
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
//读状态机
|
|
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(Current_sysID,Current_CompID, &msg,&heartbeat);
|
|
SendMessage(msg);
|
|
}
|
|
|
|
|
|
|
|
|