858 lines
27 KiB
C++
858 lines
27 KiB
C++
#include "missionprocess.h"
|
|
|
|
|
|
MissionProcess::MissionProcess(QObject *parent) : QObject(parent)
|
|
{
|
|
setRunFrq(20);//默认50Hz频率运行
|
|
}
|
|
|
|
|
|
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(thread == nullptr)
|
|
{
|
|
thread = new QThread();
|
|
this->moveToThread(thread);
|
|
connect(thread, &QThread::started, this, &MissionProcess::process);
|
|
}
|
|
|
|
if(!thread->isRunning())
|
|
{
|
|
running_flag = true;
|
|
thread->start();
|
|
qDebug() << "thread start" << running_flag;
|
|
}
|
|
else
|
|
{
|
|
qDebug() << "thread has started";
|
|
}
|
|
}
|
|
|
|
void MissionProcess::stop()
|
|
{
|
|
if(thread->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.0/running_frq);
|
|
|
|
switch(mission_status.m_Mode)
|
|
{
|
|
default:
|
|
case Nop_Mode :break;
|
|
case RecieveMode : ReadStateMachine();break;
|
|
case TransmitMode :
|
|
{
|
|
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;
|
|
}
|
|
//qDebug() << count;
|
|
}
|
|
//退出线程
|
|
disconnect(thread, nullptr, nullptr, nullptr);
|
|
thread->quit();
|
|
thread->wait();//等待结束
|
|
thread->deleteLater();
|
|
thread = nullptr;
|
|
}
|
|
|
|
void MissionProcess::setID(int m_sysid,int m_compid)
|
|
{
|
|
sysid = (uint8_t)m_sysid;
|
|
compid = (uint8_t)m_compid;
|
|
}
|
|
|
|
|
|
|
|
void MissionProcess::SendMessage(mavlink_message_t msg)
|
|
{
|
|
uint8_t buff[256+20];
|
|
uint16_t len = mavlink_msg_to_send_buffer(buff, &msg);
|
|
|
|
/*
|
|
QString num;
|
|
for (int i = 0; i < len; ++i) {
|
|
num.append(QString::number(buff[i],16).toUpper());
|
|
num.append(" ");
|
|
}
|
|
qDebug() << num;
|
|
*/
|
|
|
|
emit SendMessageTo(0,buff, len);//使用信号和槽
|
|
}
|
|
|
|
|
|
void MissionProcess::ReadCmd(uint8_t m_sysid, uint8_t m_compid)
|
|
{
|
|
if(mission_status.m_Mode != RecieveMode)//没有任务正在下载
|
|
{
|
|
mission_status.m_Mode = RecieveMode;//接收模式
|
|
|
|
qDebug() << "read mission" << sysid << compid;
|
|
|
|
start();//开启线程
|
|
}
|
|
}
|
|
|
|
void MissionProcess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,uint32_t count )
|
|
{
|
|
if(mission_status.m_Mode != TransmitMode)//没有任务在上传
|
|
{
|
|
mission_status.transmit.type = 0;
|
|
mission_status.transmit.count = count;
|
|
mission_status.m_Mode = TransmitMode;//发送模式
|
|
qDebug() << "write mission" << sysid << compid;
|
|
start();//开启线程
|
|
}
|
|
}
|
|
|
|
|
|
void MissionProcess::SetCurrentPoint(int seq)
|
|
{
|
|
if(mission_status.m_Mode != TransmitMode)//没有任务在上传
|
|
{
|
|
mission_status.transmit.type = 1;
|
|
mission_status.m_Mode = TransmitMode;//发送模式
|
|
mission_status.transmit.seq = seq - 1;
|
|
start();//开启线程
|
|
}
|
|
}
|
|
|
|
|
|
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() << "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);
|
|
|
|
if(mission_count.target_system != Current_sysID)
|
|
{
|
|
qDebug() << mission_count.target_system << "this msg is'nt mine";
|
|
//break;//如果目标系统不是自己，那么就抛弃该指令
|
|
}
|
|
|
|
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);
|
|
|
|
if(mission_count.target_system != Current_sysID)
|
|
{
|
|
qDebug() << mission_count.target_system << "this msg is'nt mine";
|
|
//break;//如果目标系统不是自己，那么就抛弃该指令
|
|
}
|
|
|
|
qDebug() << "recieve mission_request_int ";
|
|
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);
|
|
|
|
if(mission_request.target_system != Current_sysID)
|
|
{
|
|
qDebug() << mission_request.target_system << "this msg is'nt mine";
|
|
//break;//如果目标系统不是自己，那么就抛弃该指令
|
|
}
|
|
|
|
qDebug() << "recieve mission_request ";
|
|
|
|
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);
|
|
|
|
if(mission_item_int.target_system != Current_sysID)
|
|
{
|
|
qDebug() << mission_item_int.target_system << "this msg is'nt mine";
|
|
//break;//如果目标系统不是自己，那么就抛弃该指令
|
|
}
|
|
|
|
qDebug() << "recieve mission " << mission_item_int.seq;
|
|
|
|
if(mission_item_int.seq == 0)
|
|
{
|
|
emit clearWaypoint();
|
|
}
|
|
|
|
//把航点发出去
|
|
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,0,
|
|
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);
|
|
|
|
mission_status.recieve.isWaitingforItem = false;//已经收到航点，不用再等待
|
|
}break;
|
|
case MAVLINK_MSG_ID_MISSION_ITEM: {
|
|
mavlink_msg_mission_item_decode(&msg,&mission_item);
|
|
|
|
if(mission_item.target_system != Current_sysID)
|
|
{
|
|
qDebug() << mission_item.target_system << "this msg is'nt mine";
|
|
//break;//如果目标系统不是自己，那么就抛弃该指令
|
|
}
|
|
|
|
mission_status.recieve.isWaitingforItem = false;
|
|
}break;
|
|
case MAVLINK_MSG_ID_MISSION_ACK: {
|
|
mavlink_msg_mission_ack_decode(&msg,&mission_ack);
|
|
|
|
if(mission_ack.target_system != Current_sysID)
|
|
{
|
|
qDebug() << mission_ack.target_system << "this msg is'nt mine";
|
|
//break;//如果目标系统不是自己，那么就抛弃该指令
|
|
}
|
|
|
|
mission_status.transmit.isWaiteforACK = false;
|
|
}break;
|
|
case MAVLINK_MSG_ID_MISSION_CURRENT: {
|
|
mavlink_msg_mission_current_decode(&msg,&mission_current);
|
|
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();
|
|
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;
|
|
|
|
request_int(0);//读取0点
|
|
time = QTime::currentTime().msecsSinceStartOfDay();
|
|
mission_status.recieve.isWaitingforItem = true;
|
|
step ++;
|
|
}
|
|
}
|
|
|
|
}
|
|
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);
|
|
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);//发送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;
|
|
|
|
|
|
|
|
/*
|
|
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(void)//读取整列请求
|
|
{
|
|
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 = sysid;
|
|
mission_request_list.target_component = compid;
|
|
|
|
mavlink_msg_mission_request_list_encode(Current_sysID,Current_CompID, &msg,&mission_request_list);
|
|
SendMessage(msg);
|
|
}
|
|
|
|
void MissionProcess::count(uint16_t count)//计数值
|
|
{
|
|
static mavlink_message_t msg;
|
|
static mavlink_mission_count_t mission_count;
|
|
|
|
mission_count.count = count;
|
|
mission_count.mission_type = 0;
|
|
mission_count.target_system = sysid;
|
|
mission_count.target_component = compid;
|
|
|
|
mavlink_msg_mission_count_encode(Current_sysID,Current_CompID, &msg,&mission_count);
|
|
SendMessage(msg);
|
|
}
|
|
|
|
void MissionProcess::request_int(uint16_t seq)//读取请求
|
|
{
|
|
static mavlink_message_t msg;
|
|
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 = sysid;
|
|
mission_request_int.target_component = compid;
|
|
|
|
mavlink_msg_mission_request_int_encode(Current_sysID, Current_CompID, &msg,&mission_request_int);
|
|
SendMessage(msg);
|
|
}
|
|
|
|
void MissionProcess::request(uint16_t seq)//读取请求
|
|
{
|
|
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 = sysid;
|
|
mission_request.target_component = compid;
|
|
mavlink_msg_mission_request_encode(Current_sysID, Current_CompID, &msg,&mission_request);
|
|
SendMessage(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(Current_sysID,Current_CompID, &msg,&mission_item_int);
|
|
SendMessage(msg);
|
|
}
|
|
|
|
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(Current_sysID,Current_CompID, &msg,&mission_item);
|
|
SendMessage(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(Current_sysID,Current_CompID, &msg,&mission_ack);
|
|
SendMessage(msg);
|
|
|
|
|
|
}
|
|
|
|
void MissionProcess::current(void)
|
|
{
|
|
static mavlink_message_t msg;
|
|
static mavlink_mission_current_t mission_current;
|
|
|
|
mavlink_msg_mission_current_encode(Current_sysID,Current_CompID, &msg,&mission_current);
|
|
SendMessage(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(Current_sysID,Current_CompID, &msg,&mission_set_current);
|
|
SendMessage(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(Current_sysID,Current_CompID, &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_item_reached_encode(Current_sysID,Current_CompID, &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_request_partial_list_encode(Current_sysID,Current_CompID, &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_write_partial_list_encode(Current_sysID,Current_CompID, &msg,&mission_write_partial_list);
|
|
SendMessage(msg);
|
|
}
|