405 lines
12 KiB
C++
405 lines
12 KiB
C++
#include "commandprocess.h"
|
|
|
|
|
|
//留出一个接口给界面去注册指令
|
|
//收到ACK后发送一个信号
|
|
|
|
commandprocess::commandprocess(QObject *parent) : ThreadTemplet(parent)
|
|
{
|
|
setRunFrq(10);
|
|
|
|
status.m_Mode = Nop_Mode;
|
|
|
|
}
|
|
|
|
commandprocess::~commandprocess()
|
|
{
|
|
qDebug() << "stop commmand" << QThread::currentThreadId();
|
|
}
|
|
|
|
void commandprocess::process()//线程函数
|
|
{
|
|
QThread::msleep(5000);//5s后再发送
|
|
uint8_t count = 0;
|
|
|
|
qDebug() << "command" << QThread::currentThreadId();
|
|
|
|
while (true)
|
|
{
|
|
//QThread::msleep((status.m_Mode != 0)?(0):(-1));
|
|
count ++;
|
|
switch(status.m_Mode)
|
|
{
|
|
default:
|
|
case Nop_Mode : QThread::msleep(1000.0/frq());break;
|
|
case RecieveMode : ReadStateMachine();break;//QApplication::processEvents();break;
|
|
case TransmitMode : WriteStateMachine();break;//QApplication::processEvents();break;
|
|
}
|
|
|
|
if(isInterruptionRequested())//退出
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
//发送函数
|
|
void commandprocess::WriteCmd_int(uint8_t m_sysid, uint8_t m_compid ,
|
|
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)
|
|
{
|
|
if(status.m_Mode == Nop_Mode)
|
|
{
|
|
//给指令赋值
|
|
cmd_int.param1 = param1;
|
|
cmd_int.param2 = param2;
|
|
cmd_int.param3 = param3;
|
|
cmd_int.param4 = param4;
|
|
cmd_int.x = x;
|
|
cmd_int.y = y;
|
|
cmd_int.z = z;
|
|
|
|
cmd_int.command = command;
|
|
cmd_int.frame = frame;
|
|
cmd_int.current = current;
|
|
cmd_int.autocontinue = autocontinue;
|
|
|
|
//开启线程开始传输
|
|
status.transmit.type = 0;
|
|
status.m_Mode = TransmitMode;//发送模式
|
|
|
|
}
|
|
else
|
|
{
|
|
emit commandAccepted(true,command,0xFF);//广播指令反馈
|
|
//emit showMessage("Please do not operate too fast");
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void commandprocess::WriteCmd_long(float param1, float param2, float param3, float param4, float param5, float param6, float param7, uint16_t command, uint8_t confirmation)
|
|
{
|
|
if(status.m_Mode == Nop_Mode)
|
|
{
|
|
//给指令赋值
|
|
cmd_long.param1 = param1;
|
|
cmd_long.param2 = param2;
|
|
cmd_long.param3 = param3;
|
|
cmd_long.param4 = param4;
|
|
cmd_long.param5 = param5;
|
|
cmd_long.param6 = param6;
|
|
cmd_long.param7 = param7;
|
|
|
|
cmd_long.command = command;
|
|
cmd_long.confirmation = confirmation;
|
|
|
|
//开启线程开始传输
|
|
status.m_Mode = TransmitMode;//发送模式
|
|
status.transmit.type = 1;
|
|
|
|
}
|
|
else
|
|
{
|
|
emit commandAccepted(true,command,0xFF);//广播指令反馈
|
|
//emit showMessage("Please do not operate too fast");
|
|
}
|
|
}
|
|
|
|
//这个函数类似中断，专门处理接收到的状态
|
|
void commandprocess::Parse(mavlink_message_t msg)
|
|
{
|
|
mavlink_command_int_t command_int;
|
|
mavlink_command_long_t command_long;
|
|
mavlink_command_ack_t command_ack;
|
|
|
|
switch (msg.msgid) {
|
|
case MAVLINK_MSG_ID_COMMAND_INT://几乎不可能收到
|
|
mavlink_msg_command_int_decode(&msg,&command_int);
|
|
break;
|
|
case MAVLINK_MSG_ID_COMMAND_LONG://几乎不可能收到
|
|
mavlink_msg_command_long_decode(&msg,&command_long);
|
|
break;
|
|
case MAVLINK_MSG_ID_COMMAND_ACK:
|
|
mavlink_msg_command_ack_decode(&msg,&command_ack);
|
|
|
|
if(command_ack.target_system != GCS_SysID)
|
|
{
|
|
qDebug() << command_ack.target_system << "this msg is'nt mine";
|
|
//break;//如果目标系统不是自己，那么就抛弃该指令
|
|
}
|
|
|
|
//qDebug() << "command_ack.result" << command_ack.result;
|
|
|
|
qDebug() << "recieve cmd"
|
|
<< command_ack.target_system
|
|
<< command_ack.result
|
|
<< cmd_long.command
|
|
<< command_ack.command;
|
|
|
|
if(command_ack.result == MAV_RESULT_ACCEPTED)
|
|
{
|
|
if(cmd_long.command != command_ack.command)
|
|
{
|
|
break;
|
|
}
|
|
|
|
status.transmit.isWaitingforACK = false;
|
|
emit commandAccepted(true,command_ack.command,command_ack.result);//广播指令反馈
|
|
}
|
|
else
|
|
{
|
|
//拒绝该指令
|
|
emit commandAccepted(false,command_ack.command,command_ack.result);//广播指令反馈
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
//读状态机
|
|
void commandprocess::ReadStateMachine(void)//没有读指令这个说法
|
|
{
|
|
static uint8_t step = 0;
|
|
static int time = 0;
|
|
|
|
if(step == 0)
|
|
{
|
|
qDebug() << "start reading parameter";
|
|
status.recieve.isWaitingforValue = true;
|
|
|
|
step++;
|
|
time = QTime::currentTime().msecsSinceStartOfDay();
|
|
}
|
|
|
|
}
|
|
|
|
//写状态机
|
|
void commandprocess::WriteStateMachine(void)
|
|
{
|
|
static uint8_t step = 0;
|
|
static uint8_t timeout_count = 0;
|
|
static int time = 0;
|
|
|
|
if(step == 0)
|
|
{
|
|
status.transmit.isWaitingforACK = true;
|
|
|
|
if(status.transmit.type == 0)
|
|
{
|
|
qDebug() << "send cmd int" << cmd_int.command;
|
|
_int(cmd_int.param1,cmd_int.param2,cmd_int.param3,cmd_int.param4,
|
|
cmd_int.x,cmd_int.y,cmd_int.z,
|
|
cmd_int.command,cmd_int.frame,cmd_int.current,cmd_int.autocontinue);//发送一个指令
|
|
}
|
|
else if(status.transmit.type == 1)
|
|
{
|
|
qDebug() << "send cmd long" << cmd_long.command;
|
|
_long(cmd_long.param1,cmd_long.param2,cmd_long.param3,cmd_long.param4,
|
|
cmd_long.param5,cmd_long.param6,cmd_long.param7,
|
|
cmd_long.command,cmd_long.confirmation);//发送一个指令
|
|
}
|
|
|
|
time = QTime::currentTime().msecsSinceStartOfDay();
|
|
step++;//下一个阶段
|
|
}
|
|
else if(step == 1)//等待返回
|
|
{
|
|
if((QTime::currentTime().msecsSinceStartOfDay() - time) > sendTimeout)
|
|
{
|
|
timeout_count ++;
|
|
step = 0;//返回上一个步骤
|
|
if(timeout_count > 0)//如果指令发1次还没发出去，那么结束发送
|
|
{
|
|
qDebug() << "command set value time out fail";
|
|
step = 0;
|
|
status.m_Mode = Nop_Mode;
|
|
status.transmit.isWaitingforACK = false;
|
|
timeout_count = 0;
|
|
|
|
//清除指令
|
|
if(status.transmit.type == 0)
|
|
{
|
|
emit commandAccepted(false,cmd_int.command,0xFE);//超时
|
|
|
|
cmd_int.param1 = 0;
|
|
cmd_int.param2 = 0;
|
|
cmd_int.param3 = 0;
|
|
cmd_int.param4 = 0;
|
|
cmd_int.x = 0;
|
|
cmd_int.y = 0;
|
|
cmd_int.z = 0;
|
|
cmd_int.command = 0;
|
|
cmd_int.frame = 0;
|
|
cmd_int.current = 0;
|
|
cmd_int.autocontinue = 0;
|
|
}
|
|
else if(status.transmit.type == 1)
|
|
{
|
|
emit commandAccepted(false,cmd_long.command,0xFE);//超时
|
|
|
|
cmd_long.param1 = 0;
|
|
cmd_long.param2 = 0;
|
|
cmd_long.param3 = 0;
|
|
cmd_long.param4 = 0;
|
|
cmd_long.param5 = 0;
|
|
cmd_long.param6 = 0;
|
|
cmd_long.param7 = 0;
|
|
cmd_long.command = 0;
|
|
cmd_long.confirmation = 0;
|
|
}
|
|
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//收到Value
|
|
if(status.transmit.isWaitingforACK == false)
|
|
{
|
|
qDebug() << "command set value ok";
|
|
step = 0;
|
|
status.m_Mode = Nop_Mode;
|
|
status.transmit.isWaitingforACK = false;
|
|
timeout_count = 0;
|
|
|
|
//清除指令
|
|
if(status.transmit.type == 0)
|
|
{
|
|
cmd_int.param1 = 0;
|
|
cmd_int.param2 = 0;
|
|
cmd_int.param3 = 0;
|
|
cmd_int.param4 = 0;
|
|
cmd_int.x = 0;
|
|
cmd_int.y = 0;
|
|
cmd_int.z = 0;
|
|
cmd_int.command = 0;
|
|
cmd_int.frame = 0;
|
|
cmd_int.current = 0;
|
|
cmd_int.autocontinue = 0;
|
|
}
|
|
else if(status.transmit.type == 1)
|
|
{
|
|
cmd_long.param1 = 0;
|
|
cmd_long.param2 = 0;
|
|
cmd_long.param3 = 0;
|
|
cmd_long.param4 = 0;
|
|
cmd_long.param5 = 0;
|
|
cmd_long.param6 = 0;
|
|
cmd_long.param7 = 0;
|
|
cmd_long.command = 0;
|
|
cmd_long.confirmation = 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(GCS_SysID,GCS_CompID, &msg,&command_int);
|
|
Send(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(GCS_SysID,GCS_CompID, &msg,&command_long);
|
|
Send(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(uint16_t command,uint8_t result,uint8_t progress,int32_t result_param2)//地面站几乎不可能放这个函数
|
|
{
|
|
static mavlink_message_t msg;
|
|
static mavlink_command_ack_t command_ack;
|
|
|
|
command_ack.target_system = sysid;
|
|
command_ack.target_component = compid;
|
|
|
|
command_ack.command = command;
|
|
command_ack.result = result;
|
|
command_ack.progress = progress;
|
|
command_ack.result_param2 = result_param2;
|
|
|
|
mavlink_msg_command_ack_encode(GCS_SysID,GCS_CompID, &msg,&command_ack);
|
|
Send(msg);
|
|
}
|
|
|
|
|