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

使用定时器的方式做超时，，但是失效，不起作用

Browse Source
This commit is contained in:
hm
2020-03-12 20:38:52 +08:00
parent 4839cd046e
commit f6d83b53f4
5 changed files with 460 additions and 218 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MavLinkNode/missionprocess.cpp
+244 -138
View File
@@ -1,23 +1,44 @@
#include "missionprocess.h"
void SendMessageTo(uint8_t ch, uint8_t *data,uint16_t len)
{
}
MissionProcess::MissionProcess(QObject *parent) : QObject(parent)
{
mTimer = new QTimer;
if(!mTimer)
{
mTimer = new QTimer;
connect(mTimer,&QTimer::timeout,this,&MissionProcess::mTimerOut);
}
}
void MissionProcess::mTimerOut()
{
ismTimerTimeOut = true;
qDebug() << ismTimerTimeOut;
}
void MissionProcess::mTimerReset(uint32_t time)
void MissionProcess::mTimerReset(int time)
{
mTimer->start(time);
if(mTimer)
mTimer->start(time);
ismTimerTimeOut = false;
}
void MissionProcess::mTimerStop()
{
if(mTimer)
mTimer->stop();
}
@@ -67,22 +88,24 @@ void MissionProcess::stop()
void MissionProcess::process()//线程函数
{
uint8_t count = 0;
while (running_flag)
{
count ++;
QThread::msleep(1000/running_frq);
//状态机
//读取航线
if(isReadMission == true) //如果收到读取航线的指令
{
//qDebug() << "ReadStateMachine";
ReadStateMachine();
}
else if(isWriteMission == true)
{
WriteStateMachine();
}
//如果要读取航线，线发送读取指令，然后等待返回，如果返回，那么启动线程进入循环接收
//如果要写入，那么先发，然后等待，如果反馈，那么继续发送，如果不反馈，发送10次，无响应超时
}
//退出线程
Missionthread->quit();
@@ -106,55 +129,202 @@ void MissionProcess::ReadCmd(uint8_t m_sysid, uint8_t m_compid)
sysid = m_sysid;
compid = m_compid;
isReadMission = true;
ismTimerTimeOut = false;
start();//开启线程
}
//这个函数类似中断，专门处理接收到的状态
void MissionProcess::MissionParse(mavlink_message_t msg)
{
switch (msg.msgid) {
//航线部分
case MAVLINK_MSG_ID_MISSION_COUNT: {//飞控发来计数值，然后开始读取
case MAVLINK_MSG_ID_MISSION_COUNT: {
mavlink_msg_mission_count_decode(&msg,&mission_count);
isRecieveCount = true;//收到了计数，那可以开始读取第一个
isRecieveCount = true;
}break;
case MAVLINK_MSG_ID_MISSION_ITEM: {
mavlink_msg_mission_item_decode(&msg,&mission_item);
isWaitingforItem = true;
}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();
}
isWaitingforItem = true;
}break;
case MAVLINK_MSG_ID_MISSION_REQUEST: {
mavlink_msg_mission_request_decode(&msg,&mission_request);
//emit mission_item_request(vehicle.mission_request.seq);
isRecieveRequest = true;
}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);
isRecieveRequest = true;
}break;
}
}
//读航线状态机
void MissionProcess::ReadStateMachine(void)
{
static uint8_t step = 0;
static uint8_t timeout_count = 0;
if(step == 0)
{
request_list();//请求读取某一个设备的航线
mTimerReset(1000);//1秒，如果超时，那么重新发送一次指令
step++;//下一个阶段
}
else if(step == 1)//等待收到Count
{
//如果超时，那么重新发一次指令
if(ismTimerTimeOut)
{
step = 1;
timeout_count ++;
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
{
mTimerStop();
step = 0;
isReadMission = false;
timeout_count = 0;
qDebug() << "mission read time out";
}
}
else//如果没有超时，那么就进入下一个阶段
{
if(isRecieveCount == true)//收到count
{
mTimerStop();//停止定时器
timeout_count = 0;
isWaitingforItem = false;
step ++;
}
}
}
else if(step ==3)//请求航点
{
if(isWaitingforItem == false)
{
//分成两类，int和不带int
if(mission_item_int.seq < mission_count.count)
{
request_int(mission_item_int.seq+1);
//开启定时器
mTimerReset(1000);
isWaitingforItem = true;
}
else
{
step++;
}
}
else
{
if(ismTimerTimeOut)
{
timeout_count ++;
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
{
isWaitingforItem = false;
timeout_count = 0;
}
mTimerStop();
isWaitingforItem = false;
}
else
{
}
}
}
else if(step == 4)//航线传输结束，发送ack
{
ack();
step = 0;
isReadMission = false;
isRecieveCount = false;
mTimerStop();//停止定时器
}
}
//读航线状态机
void MissionProcess::WriteStateMachine(void)
{
static uint8_t step = 0;
static uint8_t timeout_count = 0;
if(step == 0)
{
count(20);//发送count
mTimerReset(1000);//1秒，如果超时，那么重新发送一次指令
step++;//下一个阶段
}
else if(step == 1)//等待收到Request
{
//如果超时，那么重新发一次指令
if(ismTimerTimeOut)
{
step = 1;
timeout_count ++;
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
{
mTimerStop();
step = 0;
isReadMission = false;
timeout_count = 0;
}
}
else//如果没有超时，那么就进入下一个阶段
{
if(isRecieveCount == true)//收到Request
{
mTimerStop();//停止定时器
timeout_count = 0;
isWaitingforItem = false;
step ++;
}
}
}
else if(step ==3)//发送航点
{
if(isWaitingforItem == false)
{
//分成两类，int和不带int
if(mission_item_int.seq < mission_count.count)
{
request_int(mission_item_int.seq+1);//发送航点
//开启定时器
mTimerReset(1000);
isWaitingforItem = true;
}
else
{
step++;
}
}
else
{
//check issended and timeout
}
}
else if(step == 4)//航线传输结束，发送ack
{
ack();
step = 0;
isReadMission = false;
isRecieveCount = false;
mTimerStop();//停止定时器
}
}
/*All msg
*
* MISSION_REQUEST_LIST
@@ -168,74 +338,11 @@ void MissionProcess::MissionParse(mavlink_message_t msg)
* MISSION_SET_CURRENT
* STATUSTEXT
* MISSION_CLEAR_ALL
* MISSION_ITEM_REACHED
* MISSION_REQUEST_PARTIAL_LIST
* MISSION_WRITE_PARTIAL_LIST
*
*
* 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)
{
request_list(0,0);//请求读取某一个设备的航线
mTimerReset(1000);//1秒，如果超时，那么重新发送一次指令
step++;//下一个阶段
}
else if(step == 1)//等待收到Count
{
//如果超时，那么重新发一次指令
if(ismTimerTimeOut)
{
step = 1;
timeout_count ++;
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
{
mTimerStop();
isReadMission = false;
}
}
else//如果没有超时，那么就进入下一个阶段
{
if(isRecieveCount == true)//收到count
{
mTimerStop();//停止定时器
step ++;
}
}
}
else if(step ==3)//请求航点
{
//
//如果当前航点没有是第（n-1）航点，就正常传输MISSION_ITEM_INT，MISSION_ITEM
//如果当前已经是第（n-1）个，那么下一步step++,
//分两个部分，分别是发送和等待
}
else if(step == 4)//航线传输结束，发送ack
{
ack(0,0);
step = 0;
isReadMission = false;
isRecieveCount = false;
mTimerStop();//停止定时器
}
}
}
void MissionProcess::request_list(uint8_t sysid,uint8_t compid)//读取整列请求
void MissionProcess::request_list(void)//读取整列请求
{
static mavlink_message_t msg;
static mavlink_mission_request_list_t mission_request_list;
@@ -246,7 +353,6 @@ void MissionProcess::request_list(uint8_t sysid,uint8_t compid)//读取整列请
mavlink_msg_mission_request_list_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_request_list);
SendMessage(msg);
}
void MissionProcess::count(uint16_t count)//计数值
@@ -256,8 +362,8 @@ void MissionProcess::count(uint16_t count)//计数值
mission_count.count = count;
mission_count.mission_type = 0;
mission_count.target_system = vehicle.sysid;
mission_count.target_component = vehicle.compid;
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);
@@ -270,8 +376,8 @@ void MissionProcess::request_int(uint16_t seq)//读取请求
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;
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);
@@ -284,8 +390,8 @@ void MissionProcess::request(uint16_t seq)//读取请求
mission_request.mission_type = 0;
mission_request.seq = seq;
mission_request.target_system = vehicle.sysid;
mission_request.target_component = vehicle.compid;
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);
}
@@ -297,28 +403,28 @@ void MissionProcess::item_int(float param1,float param2,float param3,float param
uint8_t autocontinue,uint8_t mission_type)
{
static mavlink_message_t msg;
static mavlink_mission_item_t mission_item;
static mavlink_mission_item_int_t mission_item_int;
mission_item.target_system = vehicle.sysid;
mission_item.target_component = vehicle.compid;
mission_item_int.target_system = sysid;
mission_item_int.target_component = compid;
mission_item.param1 = param1;
mission_item.param2 = param2;
mission_item.param3 = param3;
mission_item.param4 = param4;
mission_item_int.param1 = param1;
mission_item_int.param2 = param2;
mission_item_int.param3 = param3;
mission_item_int.param4 = param4;
mission_item.x = x;
mission_item.y = y;
mission_item.z = z;
mission_item_int.x = x;
mission_item_int.y = y;
mission_item_int.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;
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_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_item);
mavlink_msg_mission_item_int_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_item_int);
SendMessage(msg);
}
@@ -331,8 +437,8 @@ void MissionProcess::item(float param1,float param2,float param3,float param4,
static mavlink_message_t msg;
static mavlink_mission_item_t mission_item;
mission_item.target_system = vehicle.sysid;
mission_item.target_component = vehicle.compid;
mission_item.target_system = sysid;
mission_item.target_component = compid;
mission_item.param1 = param1;
mission_item.param2 = param2;
@@ -354,7 +460,7 @@ void MissionProcess::item(float param1,float param2,float param3,float param4,
SendMessage(msg);
}
void MissionProcess::ack(uint8_t sysid,uint8_t compid)
void MissionProcess::ack(void)
{
static mavlink_message_t msg;
static mavlink_mission_ack_t mission_ack;
@@ -373,7 +479,7 @@ void MissionProcess::ack(uint8_t sysid,uint8_t compid)
void MissionProcess::current(void)
{
static mavlink_message_t msg;
static mavlink_mission_set_current_t mission_current;
static mavlink_mission_current_t mission_current;
mavlink_msg_mission_current_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_current);
SendMessage(msg);
@@ -393,8 +499,8 @@ void MissionProcess::clear_all(void)
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.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);
@@ -406,7 +512,7 @@ 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);
mavlink_msg_mission_item_reached_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_item_reached);
SendMessage(msg);
}
@@ -415,7 +521,7 @@ 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);
mavlink_msg_mission_request_partial_list_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_request_partial_list);
SendMessage(msg);
}
@@ -424,6 +530,6 @@ 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);
mavlink_msg_mission_write_partial_list_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_write_partial_list);
SendMessage(msg);
}