938 lines
31 KiB
C++
938 lines
31 KiB
C++
#include "missionprocess.h"
|
||
|
||
|
||
MissionProcess::MissionProcess(QObject *parent) : ThreadTemplet(parent)
|
||
{
|
||
setRunFrq(20);//默认20Hz频率运行
|
||
|
||
mission_status.m_Mode = Nop_Mode;
|
||
}
|
||
|
||
MissionProcess::~MissionProcess()
|
||
{
|
||
qDebug() << "stop mission" << QThread::currentThreadId();
|
||
}
|
||
|
||
void MissionProcess::process()//线程函数
|
||
{
|
||
QThread::msleep(5000);//5s后再发送
|
||
uint8_t count = 0;
|
||
while (true)
|
||
{
|
||
count ++;
|
||
|
||
//QThread::msleep(1000/frq());
|
||
|
||
switch(mission_status.m_Mode)
|
||
{
|
||
default:
|
||
case Nop_Mode : QThread::msleep(1000/frq());break;
|
||
case RecieveMode : ReadStateMachine();break;//QApplication::processEvents();break;
|
||
case TransmitMode :
|
||
{
|
||
//QApplication::processEvents();
|
||
if(mission_status.transmit.type == 0)//航线传输
|
||
{
|
||
WriteStateMachine();
|
||
}
|
||
else if(mission_status.transmit.type == 1)//当前点设置
|
||
{
|
||
setcurrent(mission_status.transmit.seq);
|
||
mission_status.m_Mode = Nop_Mode;
|
||
qDebug() << "set current point" << mission_status.transmit.seq;
|
||
}
|
||
}break;
|
||
}
|
||
|
||
if(isInterruptionRequested())//退出
|
||
{
|
||
break;
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
|
||
void MissionProcess::checkoutVehicle(int sysid)
|
||
{
|
||
QMap<int,QMap<int,mavlink_mission_item_int_t >> missionGroup = missions.value(sysid);//读出一个飞机的所有航线
|
||
|
||
QMap<int,mavlink_mission_item_int_t> group1 = missionGroup.value(1);
|
||
QMap<int,mavlink_mission_item_int_t> group3 = missionGroup.value(3);
|
||
QMap<int,mavlink_mission_item_int_t> group4 = missionGroup.value(4);
|
||
QMap<int,mavlink_mission_item_int_t> group5 = missionGroup.value(5);
|
||
QMap<int,mavlink_mission_item_int_t> group6 = missionGroup.value(6);
|
||
|
||
|
||
foreach (mavlink_mission_item_int_t item, group1) {
|
||
//把航点发出去
|
||
emit vehicleChanged(item.param1,item.param2,item.param3,item.param4,
|
||
item.x,item.y,item.z,
|
||
item.seq,item.mission_type,
|
||
item.command,
|
||
item.target_system,
|
||
item.target_component,
|
||
item.frame,
|
||
item.current,
|
||
item.autocontinue,
|
||
item.mission_type);
|
||
}
|
||
|
||
foreach (mavlink_mission_item_int_t item, group3) {
|
||
//把航点发出去
|
||
emit vehicleChanged(item.param1,item.param2,item.param3,item.param4,
|
||
item.x,item.y,item.z,
|
||
item.seq,item.mission_type,
|
||
item.command,
|
||
item.target_system,
|
||
item.target_component,
|
||
item.frame,
|
||
item.current,
|
||
item.autocontinue,
|
||
item.mission_type);
|
||
}
|
||
|
||
foreach (mavlink_mission_item_int_t item, group4) {
|
||
//把航点发出去
|
||
emit vehicleChanged(item.param1,item.param2,item.param3,item.param4,
|
||
item.x,item.y,item.z,
|
||
item.seq,item.mission_type,
|
||
item.command,
|
||
item.target_system,
|
||
item.target_component,
|
||
item.frame,
|
||
item.current,
|
||
item.autocontinue,
|
||
item.mission_type);
|
||
}
|
||
|
||
foreach (mavlink_mission_item_int_t item, group5) {
|
||
//把航点发出去
|
||
emit vehicleChanged(item.param1,item.param2,item.param3,item.param4,
|
||
item.x,item.y,item.z,
|
||
item.seq,item.mission_type,
|
||
item.command,
|
||
item.target_system,
|
||
item.target_component,
|
||
item.frame,
|
||
item.current,
|
||
item.autocontinue,
|
||
item.mission_type);
|
||
}
|
||
|
||
foreach (mavlink_mission_item_int_t item, group6) {
|
||
//把航点发出去
|
||
emit vehicleChanged(item.param1,item.param2,item.param3,item.param4,
|
||
item.x,item.y,item.z,
|
||
item.seq,item.mission_type,
|
||
item.command,
|
||
item.target_system,
|
||
item.target_component,
|
||
item.frame,
|
||
item.current,
|
||
item.autocontinue,
|
||
item.mission_type);
|
||
}
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
void MissionProcess::ReadCmd(uint8_t m_sysid, uint8_t m_compid,int m_group)
|
||
{
|
||
if(mission_status.m_Mode == Nop_Mode)//没有任务正在下载
|
||
{
|
||
mission_status.m_Mode = RecieveMode;//接收模式
|
||
|
||
mission_status.recieve.group = m_group;
|
||
qDebug() << "read mission" << sysid << compid << m_group;
|
||
|
||
}
|
||
}
|
||
|
||
void MissionProcess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,uint32_t count ,int m_group)
|
||
{
|
||
if(mission_status.m_Mode == Nop_Mode)//没有任务在上传
|
||
{
|
||
mission_status.transmit.type = 0;
|
||
mission_status.transmit.count = count;
|
||
mission_status.transmit.group = m_group;
|
||
mission_status.m_Mode = TransmitMode;//发送模式
|
||
|
||
qDebug() << "write mission" << m_sysid << m_compid << m_group;
|
||
}
|
||
}
|
||
|
||
|
||
void MissionProcess::SetCurrentPoint(int seq)
|
||
{
|
||
if(mission_status.m_Mode == Nop_Mode)//没有任务在上传
|
||
{
|
||
mission_status.transmit.type = 1;
|
||
mission_status.m_Mode = TransmitMode;//发送模式
|
||
mission_status.transmit.seq = seq - 1;
|
||
}
|
||
}
|
||
|
||
void MissionProcess::sendFence(qreal Vertex,
|
||
qreal group,
|
||
qreal Lat,
|
||
qreal Lng,
|
||
uint16_t FecneSeq,
|
||
uint16_t command,
|
||
uint16_t sysid,
|
||
uint16_t compid,
|
||
uint16_t mission_type)
|
||
{
|
||
|
||
mapcontrol::WayPointItem::_property item;
|
||
|
||
|
||
|
||
item.param1 = Vertex;
|
||
item.param2 = group;
|
||
item.param3 = 0;
|
||
item.param4 = 0;
|
||
item.x = Lat * 10e6;
|
||
item.y = Lng * 10e6;
|
||
item.z = 0;
|
||
item.seq = FecneSeq;
|
||
item.group = 0;
|
||
item.command = command;
|
||
item.target_system = sysid;
|
||
item.target_component = compid;
|
||
item.frame = 0;
|
||
item.current = 0;
|
||
item.autocontinue = 0;
|
||
item.mission_type = mission_type;
|
||
|
||
items.insert(item.seq,item);
|
||
|
||
qDebug() << Vertex
|
||
<< group
|
||
<< Lat
|
||
<< Lng
|
||
<< FecneSeq
|
||
<< command
|
||
<< sysid
|
||
<< compid
|
||
<< mission_type
|
||
<< items.size();
|
||
|
||
}
|
||
|
||
|
||
|
||
void MissionProcess::transmitPoint(float param1,float param2,float param3,float param4,
|
||
int32_t x,int32_t y,float z,
|
||
uint16_t seq,
|
||
uint16_t group,
|
||
uint16_t command,
|
||
uint8_t target_system,
|
||
uint8_t target_component,
|
||
uint8_t frame,
|
||
uint8_t current,
|
||
uint8_t autocontinue,
|
||
uint8_t mission_type)
|
||
{
|
||
|
||
mapcontrol::WayPointItem::_property item;
|
||
|
||
qDebug() << x << y << z;
|
||
|
||
item.param1 = param1;
|
||
item.param2 = param2;
|
||
item.param3 = param3;
|
||
item.param4 = param4;
|
||
item.x = x;
|
||
item.y = y;
|
||
item.z = z;
|
||
item.seq = seq-1;
|
||
item.group = group;
|
||
item.command = command;
|
||
item.target_system = target_system;
|
||
item.target_component = target_component;
|
||
item.frame = frame;
|
||
item.current = current;
|
||
item.autocontinue = autocontinue;
|
||
item.mission_type = mission_type;
|
||
|
||
items.insert(item.seq,item);
|
||
|
||
|
||
/*
|
||
foreach (mapcontrol::WayPointItem::_property item, items) {
|
||
qDebug() << "Mission Thread: transmit seq"
|
||
<< item.seq
|
||
<< item.param1
|
||
<< item.param2
|
||
<< item.param3
|
||
<< item.param4
|
||
<< item.x
|
||
<< item.y
|
||
<< item.z
|
||
<< item.seq
|
||
<< item.group
|
||
<< item.command
|
||
<< item.target_system
|
||
<< item.target_component
|
||
<< item.frame
|
||
<< item.current
|
||
<< item.autocontinue
|
||
<< item.mission_type;
|
||
}
|
||
*/
|
||
|
||
|
||
|
||
}
|
||
|
||
|
||
|
||
//这个函数类似中断，专门处理接收到的状态
|
||
void MissionProcess::Parse(mavlink_message_t msg)
|
||
{
|
||
switch (msg.msgid) {
|
||
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
|
||
mavlink_msg_mission_request_list_decode(&msg,&mission_request_list);
|
||
}break;
|
||
case MAVLINK_MSG_ID_MISSION_COUNT: {
|
||
mavlink_msg_mission_count_decode(&msg,&mission_count);
|
||
|
||
qDebug() << "mission_count" << mission_count.count;
|
||
mission_status.recieve.isWaitingforCount = false;
|
||
}break;
|
||
case MAVLINK_MSG_ID_MISSION_REQUEST_INT: {
|
||
mavlink_msg_mission_request_int_decode(&msg,&mission_request_int);
|
||
|
||
qDebug() << "recieve mission_request_int " << "mision type" << mission_request_int.mission_type << mission_item_int.seq;
|
||
mission_item_int.seq ++;
|
||
|
||
emit sendItemOK(mission_item_int.seq,true);
|
||
mission_status.transmit.isWaiteforRequest = false;
|
||
}break;
|
||
case MAVLINK_MSG_ID_MISSION_REQUEST: {
|
||
mavlink_msg_mission_request_decode(&msg,&mission_request);
|
||
|
||
qDebug() << "recieve mission_request " << "mission type" << mission_request.mission_type;
|
||
|
||
mission_item_int.seq ++;
|
||
|
||
emit sendItemOK(mission_item_int.seq,true);
|
||
mission_status.transmit.isWaiteforRequest = false;
|
||
}break;
|
||
case MAVLINK_MSG_ID_MISSION_ITEM_INT: {
|
||
mavlink_msg_mission_item_int_decode(&msg,&mission_item_int);
|
||
|
||
qDebug() << "recieve mission " << mission_item_int.seq;
|
||
|
||
if(mission_item_int.seq == 0)
|
||
{
|
||
emit clearWaypoint();
|
||
|
||
QMap<int,QMap<int,mavlink_mission_item_int_t >> missionGroup = missions.value(msg.sysid);//读出一个飞机的所有航线
|
||
QMap<int,mavlink_mission_item_int_t > points = missionGroup.value(mission_item_int.mission_type);//读出一组
|
||
|
||
points.clear();
|
||
missionGroup.insert(mission_item_int.mission_type,points);
|
||
missions.insert(msg.sysid,missionGroup);
|
||
|
||
}
|
||
|
||
//把航点发出去
|
||
emit receivedPoint(mission_item_int.param1,mission_item_int.param2,mission_item_int.param3,mission_item_int.param4,
|
||
mission_item_int.x,mission_item_int.y,mission_item_int.z,
|
||
mission_item_int.seq,mission_item_int.mission_type,
|
||
mission_item_int.command,
|
||
mission_item_int.target_system,
|
||
mission_item_int.target_component,
|
||
mission_item_int.frame,
|
||
mission_item_int.current,
|
||
mission_item_int.autocontinue,
|
||
mission_item_int.mission_type);
|
||
|
||
qDebug() << "recieve mission type" << mission_item_int.mission_type;
|
||
|
||
|
||
QMap<int,QMap<int,mavlink_mission_item_int_t >> missionGroup = missions.value(msg.sysid);//读出一个飞机的所有航线
|
||
QMap<int,mavlink_mission_item_int_t > points = missionGroup.value(mission_item_int.mission_type);//读出一组
|
||
|
||
points.insert(mission_item_int.seq,mission_item_int);
|
||
missionGroup.insert(mission_item_int.mission_type,points);
|
||
missions.insert(msg.sysid,missionGroup);
|
||
|
||
|
||
mission_status.recieve.isWaitingforItem = false;//已经收到航点，不用再等待
|
||
}break;
|
||
case MAVLINK_MSG_ID_MISSION_ITEM: {
|
||
mavlink_msg_mission_item_decode(&msg,&mission_item);
|
||
|
||
mission_status.recieve.isWaitingforItem = false;
|
||
}break;
|
||
case MAVLINK_MSG_ID_MISSION_ACK: {
|
||
mavlink_msg_mission_ack_decode(&msg,&mission_ack);
|
||
|
||
mission_status.transmit.isWaiteforACK = false;
|
||
}break;
|
||
case MAVLINK_MSG_ID_MISSION_CURRENT: {
|
||
mavlink_msg_mission_current_decode(&msg,&mission_current);
|
||
|
||
int group = mission_current.seq /1000;
|
||
int seq = mission_current.seq %1000;
|
||
|
||
//emit currentGroup(group);
|
||
// emit currentPoint(seq + 1);
|
||
emit currentPoint(mission_current.seq + 1);
|
||
|
||
}break;
|
||
case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
|
||
mavlink_msg_mission_set_current_decode(&msg,&mission_set_current);
|
||
}break;
|
||
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: {
|
||
mavlink_msg_mission_clear_all_decode(&msg,&mission_clear_all);
|
||
}break;
|
||
case MAVLINK_MSG_ID_MISSION_ITEM_REACHED: {
|
||
mavlink_msg_mission_item_reached_decode(&msg,&mission_item_reached);
|
||
}break;
|
||
case MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST: {
|
||
mavlink_msg_mission_request_partial_list_decode(&msg,&mission_request_partial_list);
|
||
}break;
|
||
case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: {
|
||
mavlink_msg_mission_write_partial_list_decode(&msg,&mission_write_partial_list);
|
||
}break;
|
||
|
||
}
|
||
}
|
||
|
||
|
||
//读航线状态机
|
||
void MissionProcess::ReadStateMachine(void)
|
||
{
|
||
static uint8_t step = 0;
|
||
static uint8_t timeout_count = 0;
|
||
static int time = 0;
|
||
|
||
if(step == 0)
|
||
{
|
||
qDebug() << "start reading mission";
|
||
mission_status.recieve.isWaitingforCount = true;
|
||
request_list(mission_status.recieve.group);
|
||
step++;
|
||
time = QTime::currentTime().msecsSinceStartOfDay();
|
||
}
|
||
else if(step == 1)//等待收到Count
|
||
{
|
||
//如果超时，那么重新发一次指令
|
||
if((QTime::currentTime().msecsSinceStartOfDay() - time) > readTimeout)
|
||
{
|
||
step = 0;//如果超时没有收到就返回上一个步骤
|
||
timeout_count ++;
|
||
qDebug() << "mission count time out " << timeout_count << "times, retry again";
|
||
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
|
||
{
|
||
step = 0;
|
||
mission_status.m_Mode = Nop_Mode;//切换到什么都不做
|
||
timeout_count = 0;
|
||
qDebug() << "mission read fail";
|
||
}
|
||
}
|
||
else//如果没有超时，那么就进入下一个阶段
|
||
{
|
||
if(mission_status.recieve.isWaitingforCount == false)//收到count
|
||
{
|
||
qDebug() << "mission count reccieved";
|
||
timeout_count = 0;
|
||
|
||
if(mission_count.count > 0)
|
||
{
|
||
request_int(0,mission_status.recieve.group);//读取0点
|
||
time = QTime::currentTime().msecsSinceStartOfDay();
|
||
mission_status.recieve.isWaitingforItem = true;
|
||
step ++;
|
||
|
||
qDebug() << "group" << mission_status.recieve.group;
|
||
}
|
||
else
|
||
{
|
||
step = 0;
|
||
mission_status.m_Mode = Nop_Mode;//切换到什么都不做
|
||
timeout_count = 0;
|
||
qDebug() << "no mission to read";
|
||
emit showMessage(tr("this mission group is empty"));
|
||
}
|
||
}
|
||
}
|
||
|
||
}
|
||
else if(step ==2)//请求航点第0个航点
|
||
{
|
||
//超过时间
|
||
if((QTime::currentTime().msecsSinceStartOfDay() - time) > readTimeout)
|
||
{
|
||
timeout_count ++;
|
||
qDebug() << "mission request 0 time out " << timeout_count << "times, retry again";
|
||
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
|
||
{
|
||
step = 0;
|
||
mission_status.m_Mode= Nop_Mode;//切换到什么都不做
|
||
timeout_count = 0;
|
||
qDebug() << "mission read fail";
|
||
}
|
||
step = 1;//返回上一个步骤重新请求一次
|
||
}
|
||
else
|
||
{
|
||
if(mission_status.recieve.isWaitingforItem == false)
|
||
{
|
||
timeout_count = 0;
|
||
time = QTime::currentTime().msecsSinceStartOfDay();
|
||
mission_status.recieve.isWaitingforItem = false;
|
||
step ++;
|
||
}
|
||
}
|
||
}
|
||
else if(step ==3)//请求航点
|
||
{
|
||
//超过时间
|
||
if((QTime::currentTime().msecsSinceStartOfDay() - time) > readTimeout)
|
||
{
|
||
timeout_count ++;
|
||
qDebug() << "mission request " << (mission_item_int.seq+1) << " time out " << timeout_count << "times, retry again";
|
||
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
|
||
{
|
||
step = 0;
|
||
mission_item_int.seq = 0;
|
||
mission_status.m_Mode= Nop_Mode;//切换到什么都不做
|
||
timeout_count = 0;
|
||
qDebug() << "mission read fail";
|
||
}
|
||
mission_status.recieve.isWaitingforItem = false;//如果超时了，那么就重新请求一次
|
||
}
|
||
else
|
||
{
|
||
if(mission_status.recieve.isWaitingforItem == false)
|
||
{
|
||
//分成两类，int和不带int
|
||
if((mission_item_int.seq+1) < mission_count.count)
|
||
{
|
||
qDebug() << "request mission_item_int.seq+1" << (mission_item_int.seq+1);
|
||
|
||
mission_status.recieve.isWaitingforItem = true;
|
||
request_int(mission_item_int.seq+1,mission_status.recieve.group);
|
||
timeout_count = 0;
|
||
time = QTime::currentTime().msecsSinceStartOfDay();
|
||
|
||
}
|
||
else
|
||
{
|
||
qDebug() << "mission recieve all";
|
||
step++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
else if(step == 4)//航线传输结束，发送ack
|
||
{
|
||
qDebug() << "step" << step << "transmit ok";
|
||
|
||
ack();
|
||
step = 0;
|
||
mission_status.m_Mode = Nop_Mode;//切换到什么都不做
|
||
timeout_count = 0;
|
||
mission_item_int.seq = 0;
|
||
mission_status.recieve.isWaitingforCount = true;
|
||
mission_status.recieve.isWaitingforItem = true;
|
||
}
|
||
}
|
||
|
||
//写航线状态机
|
||
void MissionProcess::WriteStateMachine(void)
|
||
{
|
||
static uint8_t step = 0;
|
||
static uint8_t timeout_count = 0;
|
||
static uint32_t time = 0;
|
||
|
||
if(step == 0)
|
||
{
|
||
//向其他线程或者自己读取航点的数量
|
||
qDebug() << "start send count" << mission_status.transmit.count;
|
||
count(mission_status.transmit.count,mission_status.transmit.group);//发送count
|
||
mission_status.transmit.isWaiteforRequest = true;
|
||
time = QTime::currentTime().msecsSinceStartOfDay();
|
||
step++;//下一个阶段
|
||
}
|
||
else if(step == 1)//等待收到Request
|
||
{
|
||
//如果超时，那么重新发一次指令
|
||
if((QTime::currentTime().msecsSinceStartOfDay() - time) > sendTimeout)
|
||
{
|
||
step = 0;//返回上一个阶段
|
||
timeout_count ++;
|
||
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
|
||
{
|
||
emit sendItemOK(mission_item_int.seq,false);
|
||
mission_item_int.seq = 0;
|
||
step = 0;
|
||
mission_status.m_Mode = Nop_Mode;
|
||
timeout_count = 0;
|
||
qDebug() << "waitfor request time out,abort transmit";
|
||
|
||
}
|
||
qDebug() << "waitfor request time out" << timeout_count;
|
||
|
||
}
|
||
else//如果没有超时，那么就进入下一个阶段
|
||
{
|
||
if(mission_status.transmit.isWaiteforRequest == false)
|
||
{
|
||
qDebug() << "recieve item Request";
|
||
mission_item_int.seq = 0;
|
||
step ++;
|
||
time = QTime::currentTime().msecsSinceStartOfDay();
|
||
}
|
||
}
|
||
|
||
}
|
||
else if(step == 2)
|
||
{
|
||
if((QTime::currentTime().msecsSinceStartOfDay() - time) > sendTimeout)
|
||
{
|
||
timeout_count ++;
|
||
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
|
||
{
|
||
emit sendItemOK(mission_item_int.seq,false);
|
||
mission_item_int.seq = 0;
|
||
step = 0;
|
||
mission_status.m_Mode = Nop_Mode;
|
||
timeout_count = 0;
|
||
|
||
qDebug() << "send item time out,abort transmit";
|
||
}
|
||
|
||
time = QTime::currentTime().msecsSinceStartOfDay();
|
||
//如果超时了，重新发一次当前航点
|
||
qDebug() << "send item time out" << timeout_count;
|
||
mission_status.transmit.isWaiteforRequest = false;
|
||
|
||
}
|
||
else
|
||
{
|
||
if(mission_status.transmit.isWaiteforRequest == false)//收到Request
|
||
{
|
||
//分成两类，int和不带int
|
||
if(mission_item_int.seq < mission_status.transmit.count)
|
||
{
|
||
qDebug() << "send item" << mission_item_int.seq;
|
||
|
||
mapcontrol::WayPointItem::_property i;
|
||
|
||
i = *items.find(mission_item_int.seq);
|
||
|
||
|
||
item_int(i.param1,i.param2,i.param3,i.param4,
|
||
i.x,i.y,i.z,
|
||
i.seq,i.command,i.frame,
|
||
i.current,i.autocontinue,i.mission_type);//发送航点
|
||
|
||
qDebug() << i.param1
|
||
<< i.param2
|
||
<< i.param3
|
||
<< i.param4
|
||
<< i.x
|
||
<< i.y
|
||
<< i.z
|
||
<< i.seq
|
||
<< i.mission_type;
|
||
|
||
|
||
|
||
/*
|
||
item(i.param1,i.param2,i.param3,i.param4,
|
||
i.x * 10e-7,i.y * 10e-7,i.z * 10e-4,
|
||
i.seq,i.command,i.frame,
|
||
i.current,i.autocontinue,i.mission_type);//发送航点
|
||
*/
|
||
|
||
|
||
if(mission_item_int.seq < (mission_status.transmit.count - 1))
|
||
{
|
||
mission_status.transmit.isWaiteforRequest = true;
|
||
}
|
||
else
|
||
{
|
||
step++;
|
||
mission_status.transmit.isWaiteforACK = true;
|
||
}
|
||
|
||
time = QTime::currentTime().msecsSinceStartOfDay();
|
||
}
|
||
else
|
||
{
|
||
step++;
|
||
mission_status.transmit.isWaiteforACK = true;
|
||
time = QTime::currentTime().msecsSinceStartOfDay();
|
||
}
|
||
}
|
||
}
|
||
}
|
||
else if(step == 3)//航线传输结束，等待ack，如果没接收到就算了，超时也结束
|
||
{
|
||
if((QTime::currentTime().msecsSinceStartOfDay() - time) > sendTimeout)
|
||
{
|
||
timeout_count ++;
|
||
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取
|
||
{
|
||
emit sendItemOK(mission_item_int.seq,false);
|
||
qDebug() << "wait for ack time out ,transmit compelet";
|
||
step = 0;
|
||
mission_item_int.seq = 0;
|
||
mission_status.m_Mode = Nop_Mode;
|
||
mission_status.transmit.isWaiteforACK = false;
|
||
timeout_count = 0;
|
||
//清除存储的航线
|
||
items.clear();
|
||
}
|
||
qDebug() << "wait for ack time out";
|
||
}
|
||
else
|
||
{
|
||
//收到ACK
|
||
if(mission_status.transmit.isWaiteforACK == false)
|
||
{
|
||
emit sendItemOK(mission_item_int.seq+1,true);
|
||
qDebug() << "recieve ack ,transmit compelet";
|
||
step = 0;
|
||
mission_item_int.seq = 0;
|
||
mission_status.m_Mode = Nop_Mode;
|
||
mission_status.transmit.isWaiteforRequest = false;
|
||
|
||
//清除存储的航线
|
||
items.clear();
|
||
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
/*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::request_list(int missiontype)//读取整列请求
|
||
{
|
||
static mavlink_message_t msg;
|
||
static mavlink_mission_request_list_t mission_request_list;
|
||
|
||
mission_request_list.mission_type = missiontype;//3，4，5，6对应 1，2，3，4
|
||
mission_request_list.target_system = sysid;
|
||
mission_request_list.target_component = compid;
|
||
|
||
mavlink_msg_mission_request_list_encode(GCS_SysID,GCS_CompID, &msg,&mission_request_list);
|
||
Send(msg);
|
||
}
|
||
|
||
void MissionProcess::count(uint16_t count,int missiontype)//计数值
|
||
{
|
||
static mavlink_message_t msg;
|
||
static mavlink_mission_count_t mission_count;
|
||
|
||
mission_count.count = count;
|
||
mission_count.mission_type = missiontype;
|
||
mission_count.target_system = sysid;
|
||
mission_count.target_component = compid;
|
||
|
||
mavlink_msg_mission_count_encode(GCS_SysID,GCS_CompID, &msg,&mission_count);
|
||
Send(msg);
|
||
}
|
||
|
||
void MissionProcess::request_int(uint16_t seq,int missiontype)//读取请求
|
||
{
|
||
static mavlink_message_t msg;
|
||
static mavlink_mission_request_int_t mission_request_int;
|
||
|
||
mission_request_int.seq = seq;
|
||
mission_request_int.mission_type = missiontype;
|
||
mission_request_int.target_system = sysid;
|
||
mission_request_int.target_component = compid;
|
||
|
||
mavlink_msg_mission_request_int_encode(GCS_SysID,GCS_CompID, &msg,&mission_request_int);
|
||
Send(msg);
|
||
}
|
||
|
||
void MissionProcess::request(uint16_t seq,int missiontype)//读取请求
|
||
{
|
||
static mavlink_message_t msg;
|
||
static mavlink_mission_request_t mission_request;
|
||
|
||
mission_request.mission_type = missiontype;
|
||
mission_request.seq = seq;
|
||
mission_request.target_system = sysid;
|
||
mission_request.target_component = compid;
|
||
mavlink_msg_mission_request_encode(GCS_SysID,GCS_CompID, &msg,&mission_request);
|
||
Send(msg);
|
||
}
|
||
|
||
void MissionProcess::item_int(float param1,float param2,float param3,float param4,
|
||
int32_t x,int32_t 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 msg;
|
||
static mavlink_mission_item_int_t mission_item_int;
|
||
|
||
mission_item_int.target_system = sysid;
|
||
mission_item_int.target_component = compid;
|
||
|
||
mission_item_int.param1 = param1;
|
||
mission_item_int.param2 = param2;
|
||
mission_item_int.param3 = param3;
|
||
mission_item_int.param4 = param4;
|
||
|
||
mission_item_int.x = x;
|
||
mission_item_int.y = y;
|
||
mission_item_int.z = z;
|
||
|
||
mission_item_int.seq = seq;
|
||
mission_item_int.command = command;
|
||
mission_item_int.frame = frame;
|
||
mission_item_int.current = current;
|
||
mission_item_int.autocontinue = autocontinue;
|
||
mission_item_int.mission_type = mission_type;
|
||
|
||
mavlink_msg_mission_item_int_encode(GCS_SysID,GCS_CompID, &msg,&mission_item_int);
|
||
Send(msg);
|
||
|
||
//emit showMessage(tr("发送航点 %1").arg(seq + 1));
|
||
}
|
||
|
||
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 msg;
|
||
static mavlink_mission_item_t mission_item;
|
||
|
||
mission_item.target_system = sysid;
|
||
mission_item.target_component = 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_mission_item_encode(GCS_SysID,GCS_CompID, &msg,&mission_item);
|
||
Send(msg);
|
||
}
|
||
|
||
void MissionProcess::ack(void)
|
||
{
|
||
static mavlink_message_t msg;
|
||
static mavlink_mission_ack_t mission_ack;
|
||
|
||
mission_ack.target_system = sysid;
|
||
mission_ack.target_component = compid;
|
||
mission_ack.mission_type = 0;
|
||
mission_ack.type = 0;
|
||
|
||
mavlink_msg_mission_ack_encode(GCS_SysID,GCS_CompID, &msg,&mission_ack);
|
||
Send(msg);
|
||
|
||
|
||
}
|
||
|
||
void MissionProcess::current(void)
|
||
{
|
||
static mavlink_message_t msg;
|
||
static mavlink_mission_current_t mission_current;
|
||
|
||
mavlink_msg_mission_current_encode(GCS_SysID,GCS_CompID, &msg,&mission_current);
|
||
Send(msg);
|
||
}
|
||
|
||
void MissionProcess::setcurrent(uint16_t seq)
|
||
{
|
||
static mavlink_message_t msg;
|
||
static mavlink_mission_set_current_t mission_set_current;
|
||
|
||
mission_set_current.target_system = sysid;
|
||
mission_set_current.target_component = compid;
|
||
|
||
mission_set_current.seq = seq;
|
||
|
||
mavlink_msg_mission_set_current_encode(GCS_SysID,GCS_CompID, &msg,&mission_set_current);
|
||
Send(msg);
|
||
}
|
||
|
||
void MissionProcess::clear_all(void)
|
||
{
|
||
static mavlink_message_t msg;
|
||
static mavlink_mission_clear_all_t mission_clear_all;
|
||
|
||
mission_clear_all.target_system = sysid;
|
||
mission_clear_all.target_component = compid;
|
||
mission_clear_all.mission_type = 0;
|
||
|
||
mavlink_msg_mission_clear_all_encode(GCS_SysID,GCS_CompID, &msg,&mission_clear_all);
|
||
Send(msg);
|
||
}
|
||
|
||
void MissionProcess::item_reached(void)
|
||
{
|
||
static mavlink_message_t msg;
|
||
static mavlink_mission_item_reached_t mission_item_reached;
|
||
|
||
mavlink_msg_mission_item_reached_encode(GCS_SysID,GCS_CompID, &msg,&mission_item_reached);
|
||
Send(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_request_partial_list_encode(GCS_SysID,GCS_CompID, &msg,&mission_request_partial_list);
|
||
Send(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_write_partial_list_encode(GCS_SysID,GCS_CompID, &msg,&mission_write_partial_list);
|
||
Send(msg);
|
||
}
|