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-20 18:03:25 +08:00
parent dc87a664fe
commit cd4f252c6f
7 changed files with 228 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MavLinkNode/parameterprocess.cpp
+156 -9
View File
@@ -2,7 +2,7 @@
ParameterProcess::ParameterProcess(QObject *parent) : QObject(parent)
{
setRunFrq(200);
setRunFrq(1000);
}
void ParameterProcess::setRunFrq(uint32_t frq)
@@ -56,7 +56,13 @@ void ParameterProcess::process()//线程函数
count ++;
QThread::msleep(1000/running_frq);
switch(status.m_Mode)
{
default:
case Nop_Mode :break;
case RecieveMode : ReadStateMachine();break;
case TransmitMode : WriteStateMachine();break;
}
}
//退出线程
@@ -75,6 +81,25 @@ void ParameterProcess::SendMessage(mavlink_message_t msg)
}
void ParameterProcess::ReadCmd(uint8_t m_sysid, uint8_t m_compid,uint8_t type)
{
sysid = m_sysid;
compid = m_compid;
status.m_Mode = RecieveMode;//接收模式
status.recieve.type = (type)?(_readtype::One):(_readtype::All);
start();//开启线程
}
void ParameterProcess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,uint32_t count )
{
sysid = m_sysid;
compid = m_compid;
//mission_status.transmit.count = count;
status.m_Mode = TransmitMode;//发送模式
start();//开启线程
}
//这个函数类似中断，专门处理接收到的状态
void ParameterProcess::ParameterParse(mavlink_message_t msg)
@@ -85,6 +110,9 @@ void ParameterProcess::ParameterParse(mavlink_message_t msg)
}break;
case MAVLINK_MSG_ID_PARAM_VALUE:{//
mavlink_msg_param_value_decode(&msg,&param_value);
status.recieve.isWaitingforValue = false;
status.transmit.isWaitingforValue = false;
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);
@@ -112,23 +140,142 @@ void ParameterProcess::ParameterParse(mavlink_message_t msg)
}
//读航线状态机
void ParameterProcess::ReadStateMachine(void)//一整列
{
static uint8_t step = 0;
static uint8_t timeout_count = 0;
static uint32_t time = 0;
if(step == 0)
{
qDebug() << "start reading parameter";
status.recieve.isWaitingforValue = true;
if(status.recieve.type == _readtype::All)
{
request_list();//一整列,也可以一个,龚老师飞控并不支持这个
}
else if(status.recieve.type == _readtype::One)
{
request_read("xxx",0);//读取0
}
step++;
time = QTime::currentTime().msecsSinceStartOfDay();
}
else if(step == 1)//等待收到Item
{
//如果超时，那么结束
if((QTime::currentTime().msecsSinceStartOfDay() - time) > 1000)
{
qDebug() << "parameter item time out ";
step = 2;
timeout_count = 0;
qDebug() << "parameter read fail";
}
else//如果没有超时
{
if(status.recieve.isWaitingforValue == false)//收到item
{
qDebug() << "parameter item reccieved";
timeout_count = 0;
time = QTime::currentTime().msecsSinceStartOfDay();
//如果参数count 大于当前index，那么继续等待
if((param_value.param_index+1) < param_value.param_count)
{
status.recieve.isWaitingforValue = true;
if(status.recieve.type == _readtype::One)
{
request_read("xxx",param_value.param_index+1);//读取
}
}
else
{
step ++;
}
}
}
}
else if(step == 2)//传输结束
{
qDebug() << "step" << step << "recieve ok";
step = 0;
status.m_Mode = Nop_Mode;//切换到什么都不做
timeout_count = 0;
status.recieve.isWaitingforValue = false;
}
}
//写航线状态机
void ParameterProcess::WriteStateMachine(void)
{
static uint8_t step = 0;
static uint8_t timeout_count = 0;
static uint32_t time = 0;
if(step == 0)
{
//向其他线程或者自己读取航点的数量
status.transmit.isWaitingforValue = true;
set("id",0,1);//发送一个参数
time = QTime::currentTime().msecsSinceStartOfDay();
step++;//下一个阶段
}
else if(step == 1)//等待返回
{
if((QTime::currentTime().msecsSinceStartOfDay() - time) > 1000)
{
timeout_count ++;
step = 0;//返回上一个步骤
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束发送
{
qDebug() << "parameter set value fail";
step = 0;
status.m_Mode = Nop_Mode;
status.transmit.isWaitingforValue = false;
timeout_count = 0;
}
}
else
{
//收到Value
if(status.transmit.isWaitingforValue == false)
{
step = 0;
status.m_Mode = Nop_Mode;
status.transmit.isWaitingforValue = false;
timeout_count = 0;
}
}
}
}
/*all msg
MAVLINK_MSG_ID_PARAM_SET
MAVLINK_MSG_ID_PARAM_VALUE
MAVLINK_MSG_ID_PARAM_SET//
MAVLINK_MSG_ID_PARAM_VALUE//
MAVLINK_MSG_ID_PARAM_EXT_ACK
MAVLINK_MSG_ID_PARAM_EXT_SET
MAVLINK_MSG_ID_PARAM_EXT_VALUE
MAVLINK_MSG_ID_PARAM_REQUEST_LIST
MAVLINK_MSG_ID_PARAM_REQUEST_READ
MAVLINK_MSG_ID_PARAM_REQUEST_LIST//
MAVLINK_MSG_ID_PARAM_REQUEST_READ//
MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST
MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ
*/
void ParameterProcess::set(char id[16],uint8_t type,float value)//
void ParameterProcess::set(const char *id,uint8_t type,float value)//
{
static mavlink_message_t msg;
static mavlink_param_set_t param_set;
@@ -180,7 +327,7 @@ void ParameterProcess::ext_ack(float value)
SendMessage(msg);
}
void ParameterProcess::ext_set(char id[16],uint8_t type,float value)
void ParameterProcess::ext_set(const char *id,uint8_t type,float value)
{
static mavlink_message_t msg;
static mavlink_param_ext_set_t param_ext_set;
@@ -226,7 +373,7 @@ void ParameterProcess::request_list(void)//
SendMessage(msg);
}
void ParameterProcess::request_read(const char id[16],int16_t index)//
void ParameterProcess::request_read(const char *id, int16_t index)//
{
static mavlink_message_t msg;
static mavlink_param_request_read_t param_request_read;