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

merge

Browse Source
This commit is contained in:
hm
2020-09-23 17:09:44 +08:00
parent 0c7610a2a4 d4695556b3
commit 27408ef050
538 changed files with 68955 additions and 5342 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MavLinkNode/parameterprocess.cpp
+49 -57
View File
@@ -2,30 +2,7 @@
ParameterProcess::ParameterProcess(QObject *parent) : QObject(parent)
{
setRunFrq(1000);
paramInspect = new ParameterInspector();
qRegisterMetaType<uint8_t>("uint8_t");
qRegisterMetaType<uint16_t>("uint16_t");
qRegisterMetaType<uint32_t>("uint32_t");
//开启线程之后signal失效
connect(paramInspect,SIGNAL(ReadCmd(uint8_t,uint8_t,uint8_t)),
this,SLOT(ReadCmd(uint8_t,uint8_t,uint8_t)),Qt::DirectConnection);//多线程连接方式
connect(paramInspect,SIGNAL(WriteCmd(uint8_t,uint8_t,uint32_t)),
this,SLOT(WriteCmd(uint8_t,uint8_t,uint32_t)),Qt::DirectConnection);
//隐藏
paramInspect->hide();
setRunFrq(20);
}
void ParameterProcess::setRunFrq(uint32_t frq)
@@ -74,17 +51,10 @@ void ParameterProcess::stop()
void ParameterProcess::process()//线程函数
{
uint8_t count = 0;
connect(paramInspect,SIGNAL(ReadCmd(uint8_t,uint8_t,uint8_t)),
this,SLOT(ReadCmd(uint8_t,uint8_t,uint8_t)));
connect(paramInspect,SIGNAL(WriteCmd(uint8_t,uint8_t,uint32_t)),
this,SLOT(WriteCmd(uint8_t,uint8_t,uint32_t)));
while (running_flag)
{
count ++;
QThread::msleep(1000/running_frq);
QThread::msleep(1000.0/running_frq);
switch(status.m_Mode)
{
@@ -96,12 +66,19 @@ void ParameterProcess::process()//线程函数
}
//退出线程
disconnect(thread, nullptr, nullptr, nullptr);
thread->quit();
thread->wait(200);//等待结束
thread->wait();//等待结束
thread->deleteLater();
thread = nullptr;
}
void ParameterProcess::setID(int m_sysid,int m_compid)
{
//qDebug() << "Parameter set id";
sysid = (uint8_t)m_sysid;
compid = (uint8_t)m_compid;
}
void ParameterProcess::SendMessage(mavlink_message_t msg)
{
@@ -127,17 +104,35 @@ void ParameterProcess::ReadCmd(uint8_t m_sysid, uint8_t m_compid,uint8_t type)
start();//开启线程
}
void ParameterProcess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,uint32_t count )
void ParameterProcess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,const char *id,uint8_t type,float value)
{
Q_UNUSED(count);
//Q_UNUSED(count);
sysid = m_sysid;
compid = m_compid;
//mission_status.transmit.count = count;
status.m_Mode = TransmitMode;//发送模式
/*
if(sizeof(id) < 16)
{
memcpy(param_set.param_id,id,sizeof(id));
}
else
{
}
*/
memcpy(param_set.param_id,id,16);
param_set.param_type = type;
param_set.param_value = value;
qDebug() << "sysid" << sysid << endl
<< "compid" << compid << endl
<< "param_id" << param_set.param_id << endl
<< "param_type" << param_set.param_type << endl
<< "param_value" << param_set.param_value << endl
<< "Mode" << status.m_Mode << endl
<< "type" << status.recieve.type << endl
<< "start...";
@@ -149,18 +144,20 @@ void ParameterProcess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,uint32_t coun
//这个函数类似中断，专门处理接收到的状态
void ParameterProcess::Parse(mavlink_message_t msg)
{
qDebug() << "Parameter" << msg.msgid;
switch (msg.msgid) {
case MAVLINK_MSG_ID_PARAM_SET:{
mavlink_msg_param_set_decode(&msg,&param_set);
//mavlink_msg_param_set_decode(&msg,&param_set);
}break;
case MAVLINK_MSG_ID_PARAM_VALUE:{//
mavlink_msg_param_value_decode(&msg,&param_value);
paramInspect->appendParameter(msg);//发送的时候需要连同当前的sysid 和 compid也发送下去
emit RecieveValue(msg);
status.recieve.isWaitingforValue = false;
status.transmit.isWaitingforValue = false;
//qDebug() << "param_value.param_index" << param_value.param_index;
qDebug() << "param_value.param_index" << param_value.param_index;
}break;
case MAVLINK_MSG_ID_PARAM_EXT_ACK:{
mavlink_msg_param_ext_ack_decode(&msg,&param_ext_ack);
@@ -188,7 +185,7 @@ void ParameterProcess::Parse(mavlink_message_t msg)
}
//读航线状态机
//读状态机
void ParameterProcess::ReadStateMachine(void)//一整列
{
static uint8_t step = 0;
@@ -209,7 +206,7 @@ void ParameterProcess::ReadStateMachine(void)//一整列
else if(step == 1)//等待收到Item
{
//如果超时，那么结束
if((QTime::currentTime().msecsSinceStartOfDay() - time) > 1000)
if((QTime::currentTime().msecsSinceStartOfDay() - time) > readTimeout)
{
//做一个列表，记录谁丢了，后面单独请求
@@ -218,13 +215,10 @@ void ParameterProcess::ReadStateMachine(void)//一整列
qDebug() << "parameter item time out ";
timeout_count = 0;
if(status.recieve.type == _readtype::All)//如果是ALL的情况下，那么再用单独一个一个去请求一次
{
qDebug() << "try One mode";
status.recieve.type =_readtype::One;
status.recieve.isWaitingforValue = true;
time = QTime::currentTime().msecsSinceStartOfDay();
request_read("xxx",0);//读取0
@@ -241,9 +235,7 @@ void ParameterProcess::ReadStateMachine(void)//一整列
{
//qDebug() << "parameter item reccieved";
timeout_count = 0;
time = QTime::currentTime().msecsSinceStartOfDay();
//如果参数count 大于当前index，那么继续等待
if((param_value.param_index+1) < param_value.param_count)
{
@@ -261,7 +253,6 @@ void ParameterProcess::ReadStateMachine(void)//一整列
}
}
}
}
else if(step == 2)//传输结束
{
@@ -288,13 +279,13 @@ void ParameterProcess::WriteStateMachine(void)
//向其他线程或者自己读取航点的数量
status.transmit.isWaitingforValue = true;
set("id",0,1);//发送一个参数
set(param_set.param_id,param_set.param_type,param_set.param_value);//发送一个参数
time = QTime::currentTime().msecsSinceStartOfDay();
step++;//下一个阶段
}
else if(step == 1)//等待返回
{
if((QTime::currentTime().msecsSinceStartOfDay() - time) > 1000)
if((QTime::currentTime().msecsSinceStartOfDay() - time) > sendTimeout)
{
timeout_count ++;
step = 0;//返回上一个步骤
@@ -312,6 +303,7 @@ void ParameterProcess::WriteStateMachine(void)
//收到Value
if(status.transmit.isWaitingforValue == false)
{
qDebug() << "parameter set value ok";
step = 0;
status.m_Mode = Nop_Mode;
status.transmit.isWaitingforValue = false;
@@ -349,7 +341,7 @@ void ParameterProcess::set(const char *id,uint8_t type,float value)//
param_set.target_system = sysid;
param_set.target_component = compid;
mavlink_msg_param_set_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&param_set);
mavlink_msg_param_set_encode(Current_sysID,Current_CompID, &msg,&param_set);
SendMessage(msg);
}
@@ -367,7 +359,7 @@ void ParameterProcess::value(float value)//一般只接收不发送
//param_value. = sysid;
//param_value.target_component = compid;
mavlink_msg_param_value_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&param_value);
mavlink_msg_param_value_encode(Current_sysID,Current_CompID, &msg,&param_value);
SendMessage(msg);
}
@@ -386,7 +378,7 @@ void ParameterProcess::ext_ack(float value)
//param_ext_ack.target_system = sysid;
//param_ext_ack.target_component = compid;
mavlink_msg_param_ext_ack_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&param_ext_ack);
mavlink_msg_param_ext_ack_encode(Current_sysID,Current_CompID, &msg,&param_ext_ack);
SendMessage(msg);
}
@@ -403,7 +395,7 @@ void ParameterProcess::ext_set(const char *id,uint8_t type,float value)
param_ext_set.target_system = sysid;
param_ext_set.target_component = compid;
mavlink_msg_param_ext_set_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&param_ext_set);
mavlink_msg_param_ext_set_encode(Current_sysID,Current_CompID, &msg,&param_ext_set);
SendMessage(msg);
}
@@ -420,7 +412,7 @@ void ParameterProcess::ext_value(float value)
//param_ext_value.target_system = sysid;
//param_ext_value.target_component = compid;
mavlink_msg_param_ext_value_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&param_ext_value);
mavlink_msg_param_ext_value_encode(Current_sysID,Current_CompID, &msg,&param_ext_value);
SendMessage(msg);
}
@@ -432,7 +424,7 @@ void ParameterProcess::request_list(void)//
param_request_list.target_system = sysid;
param_request_list.target_component = compid;
mavlink_msg_param_request_list_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&param_request_list);
mavlink_msg_param_request_list_encode(Current_sysID,Current_CompID, &msg,&param_request_list);
SendMessage(msg);
}
@@ -446,7 +438,7 @@ void ParameterProcess::request_read(const char *id, int16_t index)//
param_request_read.target_system = sysid;
param_request_read.target_component = compid;
mavlink_msg_param_request_read_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&param_request_read);
mavlink_msg_param_request_read_encode(Current_sysID,Current_CompID, &msg,&param_request_read);
SendMessage(msg);
}