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

b

Browse Source
This commit is contained in:
hm
2020-03-12 16:00:08 +08:00
parent 18d30c1e0e
commit 4839cd046e
3 changed files with 129 additions and 609 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MavLinkNode/missionprocess.cpp
+111 -142
View File
@@ -5,6 +5,22 @@ MissionProcess::MissionProcess(QObject *parent) : QObject(parent)
mTimer = new QTimer;
}
void MissionProcess::mTimerOut()
{
ismTimerTimeOut = true;
}
void MissionProcess::mTimerReset(uint32_t time)
{
mTimer->start(time);
ismTimerTimeOut = false;
}
void MissionProcess::mTimerStop()
{
mTimer->stop();
}
void MissionProcess::setRunFrq(uint32_t frq)
{
if((frq != 0)||(frq <= 1000))
@@ -74,6 +90,29 @@ void MissionProcess::process()//线程函数
Missionthread = nullptr;
}
void MissionProcess::SendMessage(mavlink_message_t msg)
{
uint8_t MAVLink_Buf[256+20];
uint16_t MAVLink_Len;
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &msg);
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
}
void MissionProcess::ReadCmd(uint8_t m_sysid, uint8_t m_compid)
{
sysid = m_sysid;
compid = m_compid;
isReadMission = true;
start();//开启线程
}
@@ -143,40 +182,32 @@ void MissionProcess::ReadStateMachine(void)
if(isReadMission == true) //如果收到读取航线的指令
{
if(step == 0)//第一步 发送请求
if(step == 0)
{
//MISSION_REQUEST_LIST
request_list();//发送请求
//启动定时器，等待超时
mTimer->start(10000);//10秒，如果超时，那么重新发送一次指令
request_list(0,0);//请求读取某一个设备的航线
mTimerReset(1000);//1秒，如果超时，那么重新发送一次指令
step++;//下一个阶段
}
else if(step == 1)//等待收到Count
{
//如果超时，那么重新发一次指令
if(timeout)
if(ismTimerTimeOut)
{
step = 1;
timeout_count ++;
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
{
mTimerStop();
isReadMission = false;
}
}
else//如果没有超时，那么就进入下一个阶段
{
//如果收到MISSION_COUNT
if(isRecieveCount == true)//收到count
{
mTimerStop();//停止定时器
step ++;
}
}
}
@@ -194,69 +225,33 @@ void MissionProcess::ReadStateMachine(void)
}
else if(step == 4)//航线传输结束，发送ack
{
//发送 MISSION_ACK
ack(0,0);
step = 0;
isReadMission = false;
isRecieveCount = false;
mTimerStop();//停止定时器
}
}
}
void MissionProcess::request_list(void)//读取整列请求
void MissionProcess::request_list(uint8_t sysid,uint8_t compid)//读取整列请求
{
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
uint16_t MAVLink_Len; //发送的长度
static mavlink_message_t msg;
static mavlink_mission_request_list_t mission_request_list;
mission_request_list.mission_type = 0;
mission_request_list.target_system = vehicle.sysid;
mission_request_list.target_component = vehicle.compid;
//打包消息(到MAVLink_msg)
MAVLink_Len = mavlink_msg_mission_request_list_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_request_list);
//根据消息得到Buf和Len
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
//通过串口发送
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
mission_request_list.target_system = sysid;
mission_request_list.target_component = compid;
qDebug() << "mission request list";
mavlink_msg_mission_request_list_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_request_list);
SendMessage(msg);
}
void MissionProcess::count(uint16_t count)//计数值
{
static mavlink_message_t MAVLink_Msg;
uint8_t MAVLink_Buf[256+20];
uint16_t MAVLink_Len;
static mavlink_message_t msg;
static mavlink_mission_count_t mission_count;
mission_count.count = count;
@@ -264,17 +259,13 @@ void MissionProcess::count(uint16_t count)//计数值
mission_count.target_system = vehicle.sysid;
mission_count.target_component = vehicle.compid;
MAVLink_Len = mavlink_msg_mission_count_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_count);
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
mavlink_msg_mission_count_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_count);
SendMessage(msg);
}
void MissionProcess::request_int(uint16_t seq)//读取请求
{
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
uint16_t MAVLink_Len; //发送的长度
static mavlink_message_t msg;
static mavlink_mission_request_int_t mission_request_int;
mission_request_int.seq = seq;
@@ -282,26 +273,21 @@ void MissionProcess::request_int(uint16_t seq)//读取请求
mission_request_int.target_system = vehicle.sysid;
mission_request_int.target_component = vehicle.compid;
MAVLink_Len = mavlink_msg_mission_request_int_encode(2, MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_request_int);
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
mavlink_msg_mission_request_int_encode(2, MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_request_int);
SendMessage(msg);
}
void MissionProcess::request(uint16_t seq)//读取请求
{
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
uint16_t MAVLink_Len; //发送的长度
static mavlink_message_t msg;
static mavlink_mission_request_t mission_request;
mission_request.mission_type = 0;
mission_request.seq = seq;
mission_request.target_system = vehicle.sysid;
mission_request.target_component = vehicle.compid;
MAVLink_Len = mavlink_msg_mission_request_encode(2, MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_request);
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
mavlink_msg_mission_request_encode(2, MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_request);
SendMessage(msg);
}
void MissionProcess::item_int(float param1,float param2,float param3,float param4,
@@ -310,10 +296,7 @@ void MissionProcess::item_int(float param1,float param2,float param3,float param
uint8_t frame,uint8_t current,
uint8_t autocontinue,uint8_t mission_type)
{
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
uint16_t MAVLink_Len; //发送的长度
static mavlink_message_t msg;
static mavlink_mission_item_t mission_item;
mission_item.target_system = vehicle.sysid;
@@ -335,9 +318,8 @@ void MissionProcess::item_int(float param1,float param2,float param3,float param
mission_item.autocontinue = autocontinue;
mission_item.mission_type = mission_type;
MAVLink_Len = mavlink_msg_mission_item_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_item);
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
mavlink_msg_mission_item_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_item);
SendMessage(msg);
}
void MissionProcess::item(float param1,float param2,float param3,float param4,
@@ -346,10 +328,7 @@ void MissionProcess::item(float param1,float param2,float param3,float param4,
uint8_t frame,uint8_t current,
uint8_t autocontinue,uint8_t mission_type)
{
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
uint16_t MAVLink_Len; //发送的长度
static mavlink_message_t msg;
static mavlink_mission_item_t mission_item;
mission_item.target_system = vehicle.sysid;
@@ -371,90 +350,80 @@ void MissionProcess::item(float param1,float param2,float param3,float param4,
mission_item.autocontinue = autocontinue;
mission_item.mission_type = mission_type;
//打包消息(到MAVLink_msg)
MAVLink_Len = mavlink_msg_mission_item_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_item);
//根据消息得到Buf和Len
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
//通过串口发送
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
mavlink_msg_mission_item_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_item);
SendMessage(msg);
}
void MissionProcess::ack(void)
void MissionProcess::ack(uint8_t sysid,uint8_t compid)
{
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
uint16_t MAVLink_Len; //发送的长度
static mavlink_message_t msg;
static mavlink_mission_ack_t mission_ack;
mission_ack.target_system = vehicle.sysid;
mission_ack.target_component = vehicle.compid;
mission_ack.target_system = sysid;
mission_ack.target_component = compid;
mission_ack.mission_type = 0;
mission_ack.type = 0;
//打包消息(到MAVLink_msg)
MAVLink_Len = mavlink_msg_mission_ack_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_ack);
//根据消息得到Buf和Len
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
//通过串口发送
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
mavlink_msg_mission_ack_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_ack);
SendMessage(msg);
}
void MissionProcess::current(void)
{
static mavlink_message_t msg; //MAVLink协议信息(MSG)
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
uint16_t MAVLink_Len; //发送的长度
static mavlink_message_t msg;
static mavlink_mission_set_current_t mission_current;
//打包消息(到MAVLink_msg)
MAVLink_Len = mavlink_msg_mission_current_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_current);
//根据消息得到Buf和Len
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &msg);
//通过串口发送
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
mavlink_msg_mission_current_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_current);
SendMessage(msg);
}
void MissionProcess::setcurrent(void)
{
static mavlink_message_t msg; //MAVLink协议信息(MSG)
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
uint16_t MAVLink_Len; //发送的长度
static mavlink_message_t msg;
static mavlink_mission_set_current_t mission_set_current;
//打包消息(到MAVLink_msg)
MAVLink_Len = mavlink_msg_mission_set_current_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_set_current);
//根据消息得到Buf和Len
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &msg);
//通过串口发送
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
mavlink_msg_mission_set_current_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_set_current);
SendMessage(msg);
}
void MissionProcess::clear_all(void)
{
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
uint16_t MAVLink_Len; //发送的长度
static mavlink_message_t msg;
static mavlink_mission_clear_all_t mission_clear_all;
mission_clear_all.target_system = vehicle.sysid;
mission_clear_all.target_component = vehicle.compid;
mission_clear_all.mission_type = 0;
//打包消息(到MAVLink_msg)
MAVLink_Len = mavlink_msg_mission_clear_all_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_clear_all);
//根据消息得到Buf和Len
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
//通过串口发送
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
mavlink_msg_mission_clear_all_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_clear_all);
SendMessage(msg);
}
void MissionProcess::item_reached(void)
{
static mavlink_message_t msg;
static mavlink_mission_item_reached_t mission_item_reached;
mavlink_msg_mission_clear_all_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_item_reached);
SendMessage(msg);
}
void MissionProcess::request_partial_list(void)
{
static mavlink_message_t msg;
static mavlink_mission_request_partial_list_t mission_request_partial_list;
mavlink_msg_mission_clear_all_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_request_partial_list);
SendMessage(msg);
}
void MissionProcess::write_partial_list(void)
{
static mavlink_message_t msg;
static mavlink_mission_write_partial_list_t mission_write_partial_list;
mavlink_msg_mission_clear_all_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_write_partial_list);
SendMessage(msg);
}
MavLinkNode/missionprocess.cpp.autosave
-460
View File
@@ -1,460 +0,0 @@
#include "missionprocess.h"
MissionProcess::MissionProcess(QObject *parent) : QObject(parent)
{
mTimer = new QTimer;
}
void MissionProcess::setRunFrq(uint32_t frq)
{
if((frq != 0)||(frq <= 1000))
{
running_frq = frq;
qDebug() << "set mission thread running frquency:" <<frq <<"Hz";
}
}
void MissionProcess::start()
{
if(Missionthread == nullptr)
{
Missionthread = new QThread();
this->moveToThread(Missionthread);
connect(Missionthread, &QThread::started, this, &MissionProcess::process);
}
if(!Missionthread->isRunning())
{
running_flag = true;
Missionthread->start();
qDebug() << "thread start" << running_flag;
}
else
{
qDebug() << "thread has started";
}
}
void MissionProcess::stop()
{
if(Missionthread->isRunning())
{
running_flag = false;
qDebug() << "thread stop" << running_flag;
}
else
{
qDebug() << "thread is not running";
}
}
void MissionProcess::process()//线程函数
{
uint8_t count = 0;
while (running_flag)
{
count ++;
QThread::msleep(1000/running_frq);
//状态机
//读取航线
//如果要读取航线，线发送读取指令，然后等待返回，如果返回，那么启动线程进入循环接收
//如果要写入，那么先发，然后等待，如果反馈，那么继续发送，如果不反馈，发送10次，无响应超时
}
//退出线程
Missionthread->quit();
Missionthread->deleteLater();
Missionthread = nullptr;
}
void MissionProcess::MissionParse(mavlink_message_t msg)
{
switch (msg.msgid) {
//航线部分
case MAVLINK_MSG_ID_MISSION_COUNT: {//飞控发来计数值，然后开始读取
mavlink_msg_mission_count_decode(&msg,&mission_count);
isRecieveCount = true;//收到了计数，那可以开始读取第一个
}break;
case MAVLINK_MSG_ID_MISSION_ITEM: {
mavlink_msg_mission_item_decode(&msg,&mission_item);
}break;
case MAVLINK_MSG_ID_MISSION_ITEM_INT: {
mavlink_msg_mission_item_int_decode(&msg,&mission_item_int);
//emit mission_recieve_item(vehicle.mission_item_int);
if((mission_count.count-1) >= (mission_item_int.seq+1))
{
//Mavlink_msg_mission_request_int(vehicle.mission_item_int.seq + 1);
}
else
{
//Mavlink_msg_mission_ack();
}
}break;
case MAVLINK_MSG_ID_MISSION_REQUEST: {
mavlink_msg_mission_request_decode(&msg,&mission_request);
//emit mission_item_request(vehicle.mission_request.seq);
}break;
case MAVLINK_MSG_ID_MISSION_REQUEST_INT: {
mavlink_msg_mission_request_int_decode(&msg,&mission_request_int);
//emit mission_item_request_int(vehicle.mission_request_int.seq);
}break;
}
}
/*All msg
*
* MISSION_REQUEST_LIST
* MISSION_COUNT
* MISSION_REQUEST_INT
* MISSION_REQUEST
* MISSION_ITEM_INT
* MISSION_ITEM
* MISSION_ACK
* MISSION_CURRENT
* MISSION_SET_CURRENT
* STATUSTEXT
* MISSION_CLEAR_ALL
* MISSION_ITEM_REACHED
* MISSION_REQUEST_PARTIAL_LIST
* MISSION_WRITE_PARTIAL_LIST
*
*
**/
//读航线状态机
void MissionProcess::ReadStateMachine(void)
{
static uint8_t step = 0;
if(isReadMission == true) //如果收到读取航线的指令
{
if(step == 0)//第一步 发送请求
{
//MISSION_REQUEST_LIST
request_list();//发送请求
//启动定时器，等待超时
mTimer->start(10000);//10秒，如果超时，那么重新发送一次指令
step++;//下一个阶段
}
else if(step == 1)//等待收到Count
{
//如果超时，那么重新发一次指令
if(timeout)
{
step = 1;
timeout_count ++;
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
{
isReadMission = false;
}
}
else//如果没有超时，那么就进入下一个阶段
{
//如果收到MISSION_COUNT
step ++;
}
}
else if(step ==3)//请求航点
{
//
//如果当前航点没有是第（n-1）航点，就正常传输MISSION_ITEM_INT，MISSION_ITEM
//如果当前已经是第（n-1）个，那么下一步step++,
//分两个部分，分别是发送和等待
}
else if(step == 4)//航线传输结束，发送ack
{
//发送 MISSION_ACK
}
}
}
void MissionProcess::request_list(void)//读取整列请求
{
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
uint16_t MAVLink_Len; //发送的长度
static mavlink_mission_request_list_t mission_request_list;
mission_request_list.mission_type = 0;
mission_request_list.target_system = vehicle.sysid;
mission_request_list.target_component = vehicle.compid;
//打包消息(到MAVLink_msg)
MAVLink_Len = mavlink_msg_mission_request_list_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_request_list);
//根据消息得到Buf和Len
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
//通过串口发送
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
qDebug() << "mission request list";
}
void MissionProcess::count(uint16_t count)//计数值
{
static mavlink_message_t MAVLink_Msg;
uint8_t MAVLink_Buf[256+20];
uint16_t MAVLink_Len;
static mavlink_mission_count_t mission_count;
mission_count.count = count;
mission_count.mission_type = 0;
mission_count.target_system = vehicle.sysid;
mission_count.target_component = vehicle.compid;
MAVLink_Len = mavlink_msg_mission_count_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_count);
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
}
void MissionProcess::request_int(uint16_t seq)//读取请求
{
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
uint16_t MAVLink_Len; //发送的长度
static mavlink_mission_request_int_t mission_request_int;
mission_request_int.seq = seq;
mission_request_int.mission_type = 0;
mission_request_int.target_system = vehicle.sysid;
mission_request_int.target_component = vehicle.compid;
MAVLink_Len = mavlink_msg_mission_request_int_encode(2, MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_request_int);
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
}
void MissionProcess::request(uint16_t seq)//读取请求
{
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
uint16_t MAVLink_Len; //发送的长度
static mavlink_mission_request_t mission_request;
mission_request.mission_type = 0;
mission_request.seq = seq;
mission_request.target_system = vehicle.sysid;
mission_request.target_component = vehicle.compid;
MAVLink_Len = mavlink_msg_mission_request_encode(2, MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_request);
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
}
void MissionProcess::item_int(float param1,float param2,float param3,float param4,
float x,float y,float z,
uint16_t seq,uint16_t command,
uint8_t frame,uint8_t current,
uint8_t autocontinue,uint8_t mission_type)
{
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
uint16_t MAVLink_Len; //发送的长度
static mavlink_mission_item_t mission_item;
mission_item.target_system = vehicle.sysid;
mission_item.target_component = vehicle.compid;
mission_item.param1 = param1;
mission_item.param2 = param2;
mission_item.param3 = param3;
mission_item.param4 = param4;
mission_item.x = x;
mission_item.y = y;
mission_item.z = z;
mission_item.seq = seq;
mission_item.command = command;
mission_item.frame = frame;
mission_item.current = current;
mission_item.autocontinue = autocontinue;
mission_item.mission_type = mission_type;
MAVLink_Len = mavlink_msg_mission_item_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_item);
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
}
void MissionProcess::item(float param1,float param2,float param3,float param4,
float x,float y,float z,
uint16_t seq,uint16_t command,
uint8_t frame,uint8_t current,
uint8_t autocontinue,uint8_t mission_type)
{
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
uint16_t MAVLink_Len; //发送的长度
static mavlink_mission_item_t mission_item;
mission_item.target_system = vehicle.sysid;
mission_item.target_component = vehicle.compid;
mission_item.param1 = param1;
mission_item.param2 = param2;
mission_item.param3 = param3;
mission_item.param4 = param4;
mission_item.x = x;
mission_item.y = y;
mission_item.z = z;
mission_item.seq = seq;
mission_item.command = command;
mission_item.frame = frame;
mission_item.current = current;
mission_item.autocontinue = autocontinue;
mission_item.mission_type = mission_type;
//打包消息(到MAVLink_msg)
MAVLink_Len = mavlink_msg_mission_item_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_item);
//根据消息得到Buf和Len
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
//通过串口发送
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
}
void MissionProcess::ack(void)
{
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
uint16_t MAVLink_Len; //发送的长度
static mavlink_mission_ack_t mission_ack;
mission_ack.target_system = vehicle.sysid;
mission_ack.target_component = vehicle.compid;
mission_ack.mission_type = 0;
mission_ack.type = 0;
//打包消息(到MAVLink_msg)
MAVLink_Len = mavlink_msg_mission_ack_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_ack);
//根据消息得到Buf和Len
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
//通过串口发送
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
}
void MissionProcess::current(void)
{
static mavlink_message_t msg; //MAVLink协议信息(MSG)
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
uint16_t MAVLink_Len; //发送的长度
static mavlink_mission_set_current_t mission_current;
//打包消息(到MAVLink_msg)
MAVLink_Len = mavlink_msg_mission_current_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_current);
//根据消息得到Buf和Len
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &msg);
//通过串口发送
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
}
void MissionProcess::setcurrent(void)
{
static mavlink_message_t msg; //MAVLink协议信息(MSG)
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
uint16_t MAVLink_Len; //发送的长度
static mavlink_mission_set_current_t mission_set_current;
//打包消息(到MAVLink_msg)
MAVLink_Len = mavlink_msg_mission_set_current_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_set_current);
//根据消息得到Buf和Len
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &msg);
//通过串口发送
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
}
void MissionProcess::clear_all(void)
{
static mavlink_message_t MAVLink_Msg; //MAVLink协议信息(MSG)
uint8_t MAVLink_Buf[256+20]; //发送的缓存(注意大小, 3代表两个参数的长度)
uint16_t MAVLink_Len; //发送的长度
static mavlink_mission_clear_all_t mission_clear_all;
mission_clear_all.target_system = vehicle.sysid;
mission_clear_all.target_component = vehicle.compid;
mission_clear_all.mission_type = 0;
//打包消息(到MAVLink_msg)
MAVLink_Len = mavlink_msg_mission_clear_all_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_clear_all);
//根据消息得到Buf和Len
MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
//通过串口发送
SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
}
MavLinkNode/missionprocess.h
+15 -4
View File
@@ -30,17 +30,28 @@ public slots:
void MissionParse(mavlink_message_t msg);
//读取航线的指令
void ReadCmd(uint8_t m_sysid, uint8_t m_compid);
private slots:
//线程私有接口
void SendMessage(mavlink_message_t msg);
void process();
//定时器
void mTimerOut();
void mTimerReset(uint32_t time);
void mTimerStop();
//航线读取传输
void ReadMissionRequest();
void request_list(void);
void request_list(uint8_t sysid, uint8_t compid);
void count(uint16_t count);
void request_int(uint16_t seq);
void request(uint16_t seq);
@@ -54,7 +65,7 @@ private slots:
uint16_t seq,uint16_t command,
uint8_t frame,uint8_t current,
uint8_t autocontinue,uint8_t mission_type);
void ack(void);
void ack(uint8_t sysid, uint8_t compid);
void current(void);
void setcurrent(void);
void clear_all(void);
@@ -93,7 +104,7 @@ private:
QTimer *mTimer = nullptr;
bool ismTimerTimeOut = false;
};