456 lines
12 KiB
C++
456 lines
12 KiB
C++
#include "parameterprocess.h"
|
|
|
|
ParameterProcess::ParameterProcess(QObject *parent) : QObject(parent)
|
|
{
|
|
setRunFrq(20);
|
|
}
|
|
|
|
void ParameterProcess::setRunFrq(uint32_t frq)
|
|
{
|
|
if((frq != 0)||(frq <= 1000))
|
|
{
|
|
running_frq = frq;
|
|
qDebug() << "set parameter thread running frquency:" <<frq <<"Hz";
|
|
}
|
|
}
|
|
|
|
void ParameterProcess::start()
|
|
{
|
|
if(thread == nullptr)
|
|
{
|
|
thread = new QThread();
|
|
this->moveToThread(thread);
|
|
connect(thread, &QThread::started, this, &ParameterProcess::process);
|
|
}
|
|
|
|
if(!thread->isRunning())
|
|
{
|
|
running_flag = true;
|
|
thread->start();
|
|
qDebug() << "thread start" << running_flag;
|
|
}
|
|
else
|
|
{
|
|
qDebug() << "thread has started";
|
|
}
|
|
}
|
|
|
|
void ParameterProcess::stop()
|
|
{
|
|
if(thread->isRunning())
|
|
{
|
|
running_flag = false;
|
|
qDebug() << "thread stop" << running_flag;
|
|
}
|
|
else
|
|
{
|
|
qDebug() << "thread is not running";
|
|
}
|
|
}
|
|
|
|
void ParameterProcess::process()//线程函数
|
|
{
|
|
uint8_t count = 0;
|
|
while (running_flag)
|
|
{
|
|
count ++;
|
|
QThread::msleep(1000.0/running_frq);
|
|
|
|
switch(status.m_Mode)
|
|
{
|
|
default:
|
|
case Nop_Mode :break;
|
|
case RecieveMode : ReadStateMachine();break;
|
|
case TransmitMode : WriteStateMachine();break;
|
|
}
|
|
|
|
}
|
|
//退出线程
|
|
disconnect(thread, nullptr, nullptr, nullptr);
|
|
thread->quit();
|
|
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)
|
|
{
|
|
uint8_t buff[256+20];
|
|
uint16_t len = mavlink_msg_to_send_buffer(buff, &msg);
|
|
emit SendMessageTo(0,buff, len);//使用信号和槽
|
|
}
|
|
|
|
|
|
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);
|
|
|
|
qDebug() << "sysid" << sysid << endl
|
|
<< "compid" << compid << endl
|
|
<< "Mode" << status.m_Mode << endl
|
|
<< "type" << status.recieve.type << endl
|
|
<< "start...";
|
|
|
|
start();//开启线程
|
|
}
|
|
|
|
void ParameterProcess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,const char *id,uint8_t type,float value)
|
|
{
|
|
//Q_UNUSED(count);
|
|
|
|
sysid = m_sysid;
|
|
compid = m_compid;
|
|
|
|
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...";
|
|
|
|
start();//开启线程
|
|
}
|
|
|
|
|
|
//这个函数类似中断，专门处理接收到的状态
|
|
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,¶m_set);
|
|
}break;
|
|
case MAVLINK_MSG_ID_PARAM_VALUE:{//
|
|
mavlink_msg_param_value_decode(&msg,¶m_value);
|
|
|
|
emit RecieveValue(msg);
|
|
|
|
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,¶m_ext_ack);
|
|
}break;
|
|
case MAVLINK_MSG_ID_PARAM_EXT_SET:{
|
|
mavlink_msg_param_ext_set_decode(&msg,¶m_ext_set);
|
|
}break;
|
|
case MAVLINK_MSG_ID_PARAM_EXT_VALUE:{
|
|
mavlink_msg_param_ext_value_decode(&msg,¶m_ext_value);
|
|
}break;
|
|
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:{
|
|
mavlink_msg_param_request_list_decode(&msg,¶m_request_list);
|
|
}break;
|
|
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:{
|
|
mavlink_msg_param_request_read_decode(&msg,¶m_request_read);
|
|
}break;
|
|
case MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST:{
|
|
mavlink_msg_param_ext_request_list_decode(&msg,¶m_ext_request_list);
|
|
}break;
|
|
case MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ:{
|
|
mavlink_msg_param_ext_request_read_decode(&msg,¶m_ext_request_read);
|
|
}break;
|
|
|
|
}
|
|
}
|
|
|
|
|
|
//读状态机
|
|
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;
|
|
|
|
request_list();//一整列,也可以一个,龚老师飞控并不支持这个
|
|
|
|
step++;
|
|
time = QTime::currentTime().msecsSinceStartOfDay();
|
|
}
|
|
else if(step == 1)//等待收到Item
|
|
{
|
|
//如果超时，那么结束
|
|
if((QTime::currentTime().msecsSinceStartOfDay() - time) > readTimeout)
|
|
{
|
|
|
|
//做一个列表，记录谁丢了，后面单独请求
|
|
//qmap list
|
|
|
|
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
|
|
}
|
|
else
|
|
{
|
|
step = 2;
|
|
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 complete";
|
|
step = 0;
|
|
status.m_Mode = Nop_Mode;//切换到什么都不做
|
|
timeout_count = 0;
|
|
status.recieve.isWaitingforValue = false;
|
|
//停止线程,不能停止
|
|
//stop();
|
|
|
|
}
|
|
}
|
|
|
|
//写状态机
|
|
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(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) > sendTimeout)
|
|
{
|
|
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)
|
|
{
|
|
qDebug() << "parameter set value ok";
|
|
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_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_EXT_REQUEST_LIST
|
|
MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ
|
|
*/
|
|
|
|
void ParameterProcess::set(const char *id,uint8_t type,float value)//
|
|
{
|
|
static mavlink_message_t msg;
|
|
static mavlink_param_set_t param_set;
|
|
|
|
memcpy(param_set.param_id,id,16);
|
|
param_set.param_type = type;
|
|
param_set.param_value = value;
|
|
param_set.target_system = sysid;
|
|
param_set.target_component = compid;
|
|
|
|
mavlink_msg_param_set_encode(Current_sysID,Current_CompID, &msg,¶m_set);
|
|
SendMessage(msg);
|
|
}
|
|
|
|
void ParameterProcess::value(float value)//一般只接收不发送
|
|
{
|
|
static mavlink_message_t msg;
|
|
static mavlink_param_value_t param_value;
|
|
|
|
|
|
//param_value.param_count
|
|
//param_value.param_id
|
|
//param_value.param_index
|
|
//param_value.param_type
|
|
param_value.param_value = value;
|
|
//param_value. = sysid;
|
|
//param_value.target_component = compid;
|
|
|
|
mavlink_msg_param_value_encode(Current_sysID,Current_CompID, &msg,¶m_value);
|
|
SendMessage(msg);
|
|
}
|
|
|
|
void ParameterProcess::ext_ack(float value)
|
|
{
|
|
static mavlink_message_t msg;
|
|
static mavlink_param_ext_ack_t param_ext_ack;
|
|
|
|
Q_UNUSED(value);
|
|
|
|
//param_ext_ack.param_id
|
|
//param_ext_ack.param_result
|
|
//param_ext_ack.param_type
|
|
|
|
//param_ext_ack.param_value = value;
|
|
//param_ext_ack.target_system = sysid;
|
|
//param_ext_ack.target_component = compid;
|
|
|
|
mavlink_msg_param_ext_ack_encode(Current_sysID,Current_CompID, &msg,¶m_ext_ack);
|
|
SendMessage(msg);
|
|
}
|
|
|
|
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;
|
|
|
|
Q_UNUSED(value);
|
|
|
|
memcpy(param_ext_set.param_id,id,16);
|
|
param_ext_set.param_type = type;
|
|
//param_ext_set.param_value = value;
|
|
param_ext_set.target_system = sysid;
|
|
param_ext_set.target_component = compid;
|
|
|
|
mavlink_msg_param_ext_set_encode(Current_sysID,Current_CompID, &msg,¶m_ext_set);
|
|
SendMessage(msg);
|
|
}
|
|
|
|
void ParameterProcess::ext_value(float value)
|
|
{
|
|
static mavlink_message_t msg;
|
|
static mavlink_param_ext_value_t param_ext_value;
|
|
|
|
Q_UNUSED(value);
|
|
|
|
//param_ext_value.param_value = value;
|
|
|
|
|
|
//param_ext_value.target_system = sysid;
|
|
//param_ext_value.target_component = compid;
|
|
|
|
mavlink_msg_param_ext_value_encode(Current_sysID,Current_CompID, &msg,¶m_ext_value);
|
|
SendMessage(msg);
|
|
}
|
|
|
|
void ParameterProcess::request_list(void)//
|
|
{
|
|
static mavlink_message_t msg;
|
|
static mavlink_param_request_list_t param_request_list;
|
|
|
|
param_request_list.target_system = sysid;
|
|
param_request_list.target_component = compid;
|
|
|
|
mavlink_msg_param_request_list_encode(Current_sysID,Current_CompID, &msg,¶m_request_list);
|
|
SendMessage(msg);
|
|
}
|
|
|
|
void ParameterProcess::request_read(const char *id, int16_t index)//
|
|
{
|
|
static mavlink_message_t msg;
|
|
static mavlink_param_request_read_t param_request_read;
|
|
|
|
memcpy(param_request_read.param_id,id,16);
|
|
param_request_read.param_index = index;
|
|
param_request_read.target_system = sysid;
|
|
param_request_read.target_component = compid;
|
|
|
|
mavlink_msg_param_request_read_encode(Current_sysID,Current_CompID, &msg,¶m_request_read);
|
|
SendMessage(msg);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|