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
App/mainwindow.cpp
+1 -1
View File
@@ -128,7 +128,7 @@ void MainWindow::keyPressEvent(QKeyEvent *event) //键盘按下事件
if(event->modifiers() == Qt::AltModifier)
{//下载参数
qInfo() << "atl + P";
//dlink->Mavlink_param_request_list();
dlink->mavlinknode->Parameter->ReadCmd(1,1,0);
}
}break;
MavLinkNode/mavlinknode.cpp
+6 -8
View File
@@ -11,17 +11,15 @@ MavLinkNode::MavLinkNode(QObject *parent) : QObject(parent)
//初始化buff
initbuff();
Mission = new MissionProcess();
connect(Mission,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
Mission = new MissionProcess(this);
/*connect(Mission,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
this,SIGNAL(SendMessageTo(quint8,quint8*,quint16)));
*/
Parameter = new ParameterProcess();
connect(Parameter,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
Parameter = new ParameterProcess(this);
/*connect(Parameter,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
this,SIGNAL(SendMessageTo(quint8,quint8*,quint16)));
//Parameterthread = new QThread();
*/
}
MavLinkNode/missionprocess.cpp
+2 -2
View File
@@ -96,7 +96,7 @@ 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;//接收模式
mission_status.m_Mode = TransmitMode;//发送模式
start();//开启线程
}
@@ -352,7 +352,7 @@ void MissionProcess::WriteStateMachine(void)
else
{
step++;
mission_status.transmit.isWaiteforACK == true;
mission_status.transmit.isWaiteforACK = true;
time = QTime::currentTime().msecsSinceStartOfDay();
}
}
MavLinkNode/missionprocess.h
+1 -3
View File
@@ -80,10 +80,8 @@ private slots:
//状态机
void ReadStateMachine(void);
void WriteStateMachine(void);
//航线传输
//void ReadMissionRequest();
//所有的相关函数
void request_list(void);
void count(uint16_t count);
void request_int(uint16_t seq);
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;
MavLinkNode/parameterprocess.h
+52 -3
View File
@@ -16,9 +16,53 @@ class ParameterProcess : public QObject
{
#endif
Q_OBJECT
enum _modetype {
Nop_Mode = 0,
RecieveMode,
TransmitMode
};
enum _readtype {
All = 0,
One = 1
};
typedef struct {
//bool isWaitingforCount;
bool isWaitingforValue;
_readtype type;
}_recieve;
typedef struct {
uint32_t count;
//bool isWaiteforRequest;
bool isWaitingforValue;
}_transmit;
typedef struct
{
_recieve recieve;
_transmit transmit;
_modetype m_Mode;
}_parameter_;
public:
explicit ParameterProcess(QObject *parent = nullptr);
_parameter_ status;
//读取和写入指令
void ReadCmd(uint8_t m_sysid, uint8_t m_compid, uint8_t type);
void WriteCmd(uint8_t m_sysid, uint8_t m_compid ,uint32_t count );
public slots:
//线程对外接口
@@ -35,14 +79,19 @@ private slots:
void process();
//状态机
void ReadStateMachine(void);
void WriteStateMachine(void);
void set(char id[], uint8_t type, float value);
//所有的相关函数
void set(const char *id, uint8_t type, float value);
void value(float value);
void ext_ack(float value);
void ext_set(char id[], uint8_t type, float value);
void ext_set(const char *id, uint8_t type, float value);
void ext_value(float value);
void request_list(void);
void request_read(const char id[], int16_t index);
void request_read(const char *id, int16_t index);
signals:
void readError();
dlink/dlink.cpp
+10 -1
View File
@@ -10,7 +10,13 @@ DLink::DLink(QObject *parent) : QObject(parent)
qDebug() << "Dlink " << QThread::currentThreadId();
mavlinknode = new MavLinkNode();//不允许带参数,因为这是单独的线程
connect(mavlinknode,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
/*connect(mavlinknode,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
this,SLOT(SendMessageTo(quint8,quint8*,quint16)));*/
connect(mavlinknode->Mission,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
this,SLOT(SendMessageTo(quint8,quint8*,quint16)));
connect(mavlinknode->Parameter,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
this,SLOT(SendMessageTo(quint8,quint8*,quint16)));
}
DLink::~DLink()
@@ -22,12 +28,15 @@ DLink::~DLink()
int DLink::SendMessageTo(quint8 ch, quint8 *msg, quint16 len)
{
Q_UNUSED(ch);
if (DLink::Clientsock)
{
foreach(Node node,clientSockets)
{
DLink::Clientsock->writeDatagram((const char *)msg, len,
node.addr, node.port);
//qDebug() << "Client Send Msg";
}
}