599 lines
19 KiB
C++
599 lines
19 KiB
C++
#include "missionprocess.h"
|
|
|
|
|
|
MissionProcess::MissionProcess(QObject *parent) : QObject(parent)
|
|
{
|
|
setRunFrq(200);//默认200Hz频率运行
|
|
}
|
|
|
|
|
|
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);
|
|
|
|
switch(mission_status.m_Mode)
|
|
{
|
|
default:
|
|
case Nop_Mode :break;
|
|
case RecieveMode : ReadStateMachine();break;
|
|
case TransmitMode : WriteStateMachine();break;
|
|
}
|
|
|
|
}
|
|
//退出线程
|
|
Missionthread->quit();
|
|
Missionthread->wait(200);//等待结束
|
|
Missionthread->deleteLater();
|
|
Missionthread = nullptr;
|
|
}
|
|
|
|
void MissionProcess::SendMessage(mavlink_message_t msg)
|
|
{
|
|
uint8_t buff[256+20];
|
|
uint16_t len = mavlink_msg_to_send_buffer(buff, &msg);
|
|
emit SendMessageTo(0,buff, len);//使用信号和槽
|
|
}
|
|
|
|
|
|
void MissionProcess::ReadCmd(uint8_t m_sysid, uint8_t m_compid)
|
|
{
|
|
sysid = m_sysid;
|
|
compid = m_compid;
|
|
mission_status.m_Mode = RecieveMode;//接收模式
|
|
|
|
start();//开启线程
|
|
}
|
|
|
|
void MissionProcess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,uint32_t count )
|
|
{
|
|
sysid = m_sysid;
|
|
compid = m_compid;
|
|
mission_status.transmit.count = count;
|
|
mission_status.m_Mode = TransmitMode;//发送模式
|
|
|
|
start();//开启线程
|
|
}
|
|
|
|
|
|
//这个函数类似中断，专门处理接收到的状态
|
|
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);
|
|
mission_status.transmit.isWaiteforRequest = false;
|
|
}break;
|
|
case MAVLINK_MSG_ID_MISSION_REQUEST: {
|
|
mavlink_msg_mission_request_decode(&msg,&mission_request);
|
|
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;
|
|
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);
|
|
}break;
|
|
case MAVLINK_MSG_ID_MISSION_CURRENT: {
|
|
mavlink_msg_mission_current_decode(&msg,&mission_current);
|
|
}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 uint32_t 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) > 1000)
|
|
{
|
|
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) > 1000)
|
|
{
|
|
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) > 1000)
|
|
{
|
|
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_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_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)
|
|
{
|
|
//向其他线程或者自己读取航点的数量
|
|
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) > 1000)
|
|
{
|
|
step = 0;//返回上一个阶段
|
|
timeout_count ++;
|
|
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
|
|
{
|
|
step = 0;
|
|
mission_status.m_Mode = Nop_Mode;
|
|
timeout_count = 0;
|
|
}
|
|
}
|
|
else//如果没有超时，那么就进入下一个阶段
|
|
{
|
|
if(mission_status.transmit.isWaiteforRequest == false)
|
|
{
|
|
step ++;
|
|
time = QTime::currentTime().msecsSinceStartOfDay();
|
|
}
|
|
}
|
|
|
|
}
|
|
else if(step == 2)
|
|
{
|
|
if((QTime::currentTime().msecsSinceStartOfDay() - time) > 1000)
|
|
{
|
|
timeout_count ++;
|
|
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
|
|
{
|
|
step = 0;
|
|
mission_status.m_Mode = Nop_Mode;
|
|
timeout_count = 0;
|
|
}
|
|
//如果超时了，重新发一次当前航点
|
|
mission_status.transmit.isWaiteforRequest = false;
|
|
|
|
}
|
|
else
|
|
{
|
|
if(mission_status.transmit.isWaiteforRequest == false)//收到Request
|
|
{
|
|
//分成两类，int和不带int
|
|
if(mission_item_int.seq < mission_count.count)
|
|
{
|
|
//item_int(mission_item_int.seq+1);//发送航点
|
|
mission_status.transmit.isWaiteforRequest = 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) > 1000)
|
|
{
|
|
timeout_count ++;
|
|
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取
|
|
{
|
|
step = 0;
|
|
mission_status.m_Mode = Nop_Mode;
|
|
mission_status.transmit.isWaiteforACK = false;
|
|
timeout_count = 0;
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
//收到ACK
|
|
if(mission_status.transmit.isWaiteforACK == false)
|
|
{
|
|
step = 0;
|
|
mission_status.m_Mode = Nop_Mode;
|
|
mission_status.transmit.isWaiteforRequest = false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
/*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(2,MAV_COMP_ID_MISSIONPLANNER, &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(2,MAV_COMP_ID_MISSIONPLANNER, &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(2, MAV_COMP_ID_MISSIONPLANNER, &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(2, MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_request);
|
|
SendMessage(msg);
|
|
}
|
|
|
|
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 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(2,MAV_COMP_ID_MISSIONPLANNER, &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(2,MAV_COMP_ID_MISSIONPLANNER, &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(2,MAV_COMP_ID_MISSIONPLANNER, &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(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_current);
|
|
SendMessage(msg);
|
|
}
|
|
|
|
void MissionProcess::setcurrent(void)
|
|
{
|
|
static mavlink_message_t msg;
|
|
static mavlink_mission_set_current_t mission_set_current;
|
|
|
|
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 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(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_item_reached_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_request_partial_list_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_write_partial_list_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_write_partial_list);
|
|
SendMessage(msg);
|
|
}
|