hm/gcs-nf
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修正姿态显示和指令发送

Browse Source
This commit is contained in:
Company
2020-06-03 18:09:41 +08:00
parent d20e39a987
commit 72aa91dfb8
12 changed files with 354 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MavLinkNode/commandprocess.cpp
+91 -45
View File
@@ -93,7 +93,7 @@ void commandprocess::SendMessage(mavlink_message_t msg)
}
//发送函数
void commandprocess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,
void commandprocess::WriteCmd_int(uint8_t m_sysid, uint8_t m_compid ,
float param1,
float param2,
float param3,
@@ -102,9 +102,6 @@ void commandprocess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,
int32_t y,
float z, uint16_t command, uint8_t frame, uint8_t current, uint8_t autocontinue)
{
//sysid = m_sysid;
//compid = m_compid;
//给指令赋值
cmd_int.param1 = param1;
cmd_int.param2 = param2;
@@ -122,24 +119,31 @@ void commandprocess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,
//开启线程开始传输
status.m_Mode = TransmitMode;//发送模式
status.transmit.type = 0;
start();//开启线程
}
void commandprocess::WriteCmd_long(float param1, float param2, float param3, float param4, float param5, float param6, float param7)
void commandprocess::WriteCmd_long(float param1, float param2, float param3, float param4, float param5, float param6, float param7, uint16_t command, uint8_t confirmation)
{
WriteCmd(0,0,
param1,
param2,
param3,
param4,
param5,
param6,
param7,
0,0,0,0);
//给指令赋值
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;
start();//开启线程
}
@@ -167,7 +171,7 @@ void commandprocess::Parse(mavlink_message_t msg)
{
status.transmit.isWaitingforACK = false;
emit commandAccepted(command_ack.command);//广播指令已经接受的反馈
emit commandAccepted(true,command_ack.command);//广播指令已经接受的反馈
}
else
{
@@ -181,17 +185,13 @@ void commandprocess::Parse(mavlink_message_t msg)
void commandprocess::ReadStateMachine(void)//没有读指令这个说法
{
static uint8_t step = 0;
//static uint8_t timeout_count = 0;
static int time = 0;
if(step == 0)
{
qDebug() << "start reading parameter";
status.recieve.isWaitingforValue = true;
//request_list();
step++;
time = QTime::currentTime().msecsSinceStartOfDay();
}
@@ -209,9 +209,21 @@ void commandprocess::WriteStateMachine(void)
{
status.transmit.isWaitingforACK = true;
_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);//发送一个指令
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++;//下一个阶段
}
@@ -230,17 +242,36 @@ void commandprocess::WriteStateMachine(void)
timeout_count = 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;
if(status.transmit.type == 0)
{
emit commandAccepted(false,cmd_int.command);
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);
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;
}
}
}
@@ -256,17 +287,32 @@ void commandprocess::WriteStateMachine(void)
timeout_count = 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;
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;
}
}
}
}