add command
This commit is contained in:
@@ -0,0 +1,259 @@
|
||||
#include "commandprocess.h"
|
||||
|
||||
|
||||
//留出一个接口给界面去注册指令
|
||||
//收到ACK后发送一个信号
|
||||
|
||||
commandprocess::commandprocess(QObject *parent) : QObject(parent)
|
||||
{
|
||||
setRunFrq(500);
|
||||
}
|
||||
|
||||
|
||||
void commandprocess::setRunFrq(uint32_t frq)
|
||||
{
|
||||
if((frq != 0)||(frq <= 1000))
|
||||
{
|
||||
running_frq = frq;
|
||||
qDebug() << "set command thread running frquency:" <<frq <<"Hz";
|
||||
}
|
||||
}
|
||||
|
||||
void commandprocess::start()
|
||||
{
|
||||
if(thread == nullptr)
|
||||
{
|
||||
thread = new QThread();
|
||||
this->moveToThread(thread);
|
||||
connect(thread, &QThread::started, this, &commandprocess::process);
|
||||
}
|
||||
|
||||
if(!thread->isRunning())
|
||||
{
|
||||
running_flag = true;
|
||||
thread->start();
|
||||
qDebug() << "thread start" << running_flag;
|
||||
}
|
||||
else
|
||||
{
|
||||
qDebug() << "thread has started";
|
||||
}
|
||||
}
|
||||
|
||||
void commandprocess::stop()
|
||||
{
|
||||
if(thread->isRunning())
|
||||
{
|
||||
running_flag = false;
|
||||
qDebug() << "thread stop" << running_flag;
|
||||
}
|
||||
else
|
||||
{
|
||||
qDebug() << "thread is not running";
|
||||
}
|
||||
}
|
||||
|
||||
void commandprocess::process()//线程函数
|
||||
{
|
||||
uint8_t count = 0;
|
||||
|
||||
while (running_flag)
|
||||
{
|
||||
count ++;
|
||||
QThread::msleep(1000/running_frq);
|
||||
|
||||
switch(status.m_Mode)
|
||||
{
|
||||
default:
|
||||
case Nop_Mode :break;
|
||||
case RecieveMode : ReadStateMachine();break;
|
||||
case TransmitMode : WriteStateMachine();break;
|
||||
}
|
||||
|
||||
}
|
||||
//退出线程
|
||||
thread->quit();
|
||||
thread->wait(200);//等待结束
|
||||
thread->deleteLater();
|
||||
thread = nullptr;
|
||||
}
|
||||
|
||||
|
||||
void commandprocess::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 commandprocess::Parse(mavlink_message_t msg)
|
||||
{
|
||||
switch (msg.msgid) {
|
||||
case MAVLINK_MSG_ID_COMMAND_INT:
|
||||
mavlink_msg_command_int_decode(&msg,&command_int);
|
||||
case MAVLINK_MSG_ID_COMMAND_LONG:
|
||||
mavlink_msg_command_long_decode(&msg,&command_long);
|
||||
case MAVLINK_MSG_ID_COMMAND_ACK:
|
||||
mavlink_msg_command_ack_decode(&msg,&command_ack);
|
||||
}
|
||||
}
|
||||
|
||||
//读状态机
|
||||
void commandprocess::ReadStateMachine(void)//没有读指令这个说法
|
||||
{
|
||||
static uint8_t step = 0;
|
||||
static uint8_t timeout_count = 0;
|
||||
static uint32_t time = 0;
|
||||
|
||||
|
||||
if(step == 0)
|
||||
{
|
||||
qDebug() << "start reading parameter";
|
||||
status.recieve.isWaitingforValue = true;
|
||||
|
||||
//request_list();
|
||||
|
||||
step++;
|
||||
time = QTime::currentTime().msecsSinceStartOfDay();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//写状态机
|
||||
void commandprocess::WriteStateMachine(void)
|
||||
{
|
||||
static uint8_t step = 0;
|
||||
static uint8_t timeout_count = 0;
|
||||
static uint32_t time = 0;
|
||||
|
||||
if(step == 0)
|
||||
{
|
||||
|
||||
status.transmit.isWaitingforACK = true;
|
||||
|
||||
//set("id",0,1);//发送一个指令
|
||||
time = QTime::currentTime().msecsSinceStartOfDay();
|
||||
step++;//下一个阶段
|
||||
}
|
||||
else if(step == 1)//等待返回
|
||||
{
|
||||
if((QTime::currentTime().msecsSinceStartOfDay() - time) > 1000)
|
||||
{
|
||||
timeout_count ++;
|
||||
step = 0;//返回上一个步骤
|
||||
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束发送
|
||||
{
|
||||
qDebug() << "command set value fail";
|
||||
step = 0;
|
||||
status.m_Mode = Nop_Mode;
|
||||
status.transmit.isWaitingforACK = false;
|
||||
timeout_count = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//收到Value
|
||||
if(status.transmit.isWaitingforACK == false)
|
||||
{
|
||||
step = 0;
|
||||
status.m_Mode = Nop_Mode;
|
||||
status.transmit.isWaitingforACK = false;
|
||||
timeout_count = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
/*all msg
|
||||
|
||||
MAVLINK_MSG_ID_COMMAND_INT
|
||||
MAVLINK_MSG_ID_COMMAND_LONG
|
||||
MAVLINK_MSG_ID_COMMAND_ACK
|
||||
*/
|
||||
|
||||
|
||||
void commandprocess::_int(float param1,
|
||||
float param2,
|
||||
float param3,
|
||||
float param4,
|
||||
int32_t x,
|
||||
int32_t y,
|
||||
float z,
|
||||
uint16_t command,
|
||||
uint8_t frame,
|
||||
uint8_t current,
|
||||
uint8_t autocontinue)
|
||||
{
|
||||
static mavlink_message_t msg;
|
||||
static mavlink_command_int_t command_int;
|
||||
|
||||
command_int.target_system = sysid;
|
||||
command_int.target_component = compid;
|
||||
|
||||
|
||||
command_int.param1 = param1;
|
||||
command_int.param2 = param2;
|
||||
command_int.param3 = param3;
|
||||
command_int.param4 = param4;
|
||||
command_int.x = x;
|
||||
command_int.y = y;
|
||||
command_int.z = z;
|
||||
command_int.command = command;
|
||||
command_int.frame. = frame;
|
||||
command_int.current = current;
|
||||
command_int.autocontinue = autocontinue;
|
||||
|
||||
mavlink_msg_command_int_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&command_int);
|
||||
SendMessage(msg);
|
||||
}
|
||||
|
||||
void commandprocess::_long(float param1,
|
||||
float param2,
|
||||
float param3,
|
||||
float param4,
|
||||
float param5,
|
||||
float param6,
|
||||
float param7,
|
||||
uint16_t command,
|
||||
uint8_t confirmation)
|
||||
{
|
||||
static mavlink_message_t msg;
|
||||
static mavlink_command_long_t command_long;
|
||||
|
||||
command_long.target_system = sysid;
|
||||
command_long.target_component = compid;
|
||||
|
||||
command_long.param1 = param1;
|
||||
command_long.param2 = param2;
|
||||
command_long.param3 = param3;
|
||||
command_long.param4 = param4;
|
||||
command_long.param5 = param5;
|
||||
command_long.param6 = param6;
|
||||
command_long.param7 = param7;
|
||||
command_long.command = command;
|
||||
command_long.confirmation = confirmation;
|
||||
|
||||
mavlink_msg_command_long_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&command_long);
|
||||
SendMessage(msg);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
MAV_RESULT_ACCEPTED=0,
|
||||
MAV_RESULT_TEMPORARILY_REJECTED=1,
|
||||
MAV_RESULT_DENIED=2,
|
||||
MAV_RESULT_UNSUPPORTED=3,
|
||||
MAV_RESULT_FAILED=4,
|
||||
MAV_RESULT_IN_PROGRESS=5,
|
||||
**/
|
||||
|
||||
|
||||
void commandprocess::ack()//地面站几乎不可能放这个函数
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user