674 lines
18 KiB
C++
674 lines
18 KiB
C++
#include "parameterprocess.h"
|
|
|
|
ParameterProcess::ParameterProcess(QObject *parent) : ThreadTemplet(parent)
|
|
{
|
|
setRunFrq(10);
|
|
|
|
status.m_Mode = Nop_Mode;
|
|
status.recieve.type = _readtype::All;
|
|
|
|
}
|
|
ParameterProcess::~ParameterProcess()
|
|
{
|
|
qDebug() << "stop parameter" << QThread::currentThreadId();
|
|
}
|
|
|
|
void ParameterProcess::process()//线程函数
|
|
{
|
|
QThread::msleep(5000);//5s后再发送
|
|
uint8_t count = 0;
|
|
while (true)
|
|
{
|
|
count ++;
|
|
switch(status.m_Mode)
|
|
{
|
|
default:
|
|
case Nop_Mode : QThread::msleep(1000/frq());break;
|
|
case RecieveMode : ReadStateMachine();QApplication::processEvents();break;
|
|
case TransmitMode : WriteStateMachine();QApplication::processEvents();break;
|
|
}
|
|
|
|
if(isInterruptionRequested())//退出
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void ParameterProcess::ReadCmd(uint8_t m_sysid, uint8_t m_compid,uint8_t type)
|
|
{
|
|
if(status.m_Mode == Nop_Mode)
|
|
{
|
|
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...";
|
|
}
|
|
}
|
|
|
|
void ParameterProcess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,const char *id,uint8_t type,float value)
|
|
{
|
|
if(status.m_Mode == Nop_Mode)
|
|
{
|
|
sysid = m_sysid;
|
|
compid = m_compid;
|
|
|
|
status.m_Mode = TransmitMode;//发送模式
|
|
|
|
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...";
|
|
|
|
|
|
|
|
|
|
}
|
|
}
|
|
|
|
|
|
void ParameterProcess::WriteCmd2(uint8_t m_sysid, uint8_t m_compid ,QVariant id,QVariant type,QVariant value)
|
|
{
|
|
if(status.m_Mode == Nop_Mode)
|
|
{
|
|
|
|
sysid = m_sysid;
|
|
compid = m_compid;
|
|
|
|
float par = getvalue(value,gettype(type));
|
|
|
|
WriteCmd(sysid,compid,getid(id.toString()),gettype(type),par);
|
|
}
|
|
}
|
|
|
|
//这个函数类似中断，专门处理接收到的状态
|
|
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;
|
|
|
|
emit showMessage(tr("收到参数 %1 %2 %3").arg(param_value.param_index).arg(setid(param_value.param_id)).arg(setvalue(param_value).toDouble()));
|
|
qDebug() << "param_value" << param_value.param_index << param_value.param_value;
|
|
|
|
}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;
|
|
}
|
|
}
|
|
|
|
//写状态机
|
|
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(GCS_SysID,GCS_CompID, &msg,¶m_set);
|
|
Send(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(GCS_SysID,GCS_CompID, &msg,¶m_value);
|
|
Send(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(GCS_SysID,GCS_CompID, &msg,¶m_ext_ack);
|
|
Send(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(GCS_SysID,GCS_CompID, &msg,¶m_ext_set);
|
|
Send(msg);
|
|
}
|
|
|
|
void ParameterProcess::ext_value(float value)
|
|
{
|
|
static mavlink_message_t msg;
|
|
static mavlink_param_ext_value_t param_ext_value;
|
|
|
|
Q_UNUSED(value);
|
|
|
|
|
|
mavlink_msg_param_ext_value_encode(GCS_SysID,GCS_CompID, &msg,¶m_ext_value);
|
|
Send(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(GCS_SysID,GCS_CompID, &msg,¶m_request_list);
|
|
Send(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(GCS_SysID,GCS_CompID, &msg,¶m_request_read);
|
|
Send(msg);
|
|
}
|
|
|
|
|
|
|
|
|
|
//================================
|
|
|
|
QVariant ParameterProcess::setvalue(mavlink_param_value_t param)
|
|
{
|
|
QVariant value;
|
|
|
|
switch (param.param_type)
|
|
{
|
|
case MAV_PARAM_TYPE_UINT8:
|
|
{
|
|
value = ((uint8_t *)¶m.param_value)[0];
|
|
}break;
|
|
case MAV_PARAM_TYPE_INT8:
|
|
{
|
|
value = ((int8_t *)¶m.param_value)[0];
|
|
}break;
|
|
case MAV_PARAM_TYPE_UINT16:
|
|
{
|
|
value = ((uint16_t *)¶m.param_value)[0];
|
|
}break;
|
|
case MAV_PARAM_TYPE_INT16:
|
|
{
|
|
value = ((int16_t *)¶m.param_value)[0];
|
|
}break;
|
|
case MAV_PARAM_TYPE_UINT32:
|
|
{
|
|
value = ((uint32_t *)¶m.param_value)[0];
|
|
}break;
|
|
case MAV_PARAM_TYPE_INT32:
|
|
{
|
|
value = ((int32_t *)¶m.param_value)[0];
|
|
}break;
|
|
case MAV_PARAM_TYPE_UINT64:
|
|
{
|
|
#ifdef unix
|
|
//value = ((ulong *)¶m.param_value)[0];
|
|
#else
|
|
value = ((uint64_t *)¶m.param_value)[0];
|
|
#endif
|
|
}break;
|
|
case MAV_PARAM_TYPE_INT64:
|
|
{
|
|
#ifdef unix
|
|
//value = ((long *)¶m.param_value)[0];
|
|
#else
|
|
value = ((int64_t *)¶m.param_value)[0];
|
|
#endif
|
|
}break;
|
|
case MAV_PARAM_TYPE_REAL32:
|
|
{
|
|
value = ((float *)¶m.param_value)[0];
|
|
}break;
|
|
case MAV_PARAM_TYPE_REAL64:
|
|
{
|
|
value = ((double *)¶m.param_value)[0];
|
|
}break;
|
|
default:
|
|
{
|
|
value = ((float *)¶m.param_value)[0];
|
|
//qDebug() << "Unsupported type " << param.param_type;
|
|
}break;
|
|
|
|
}
|
|
|
|
return value;
|
|
}
|
|
|
|
float ParameterProcess::getvalue(QVariant param,uint8_t type)
|
|
{
|
|
float value;
|
|
union {
|
|
uint8_t u8[8];
|
|
int8_t i8[8];
|
|
uint16_t u16[4];
|
|
int16_t i16[4];
|
|
uint32_t u32[2];
|
|
int32_t i32[2];
|
|
uint64_t u64;
|
|
int64_t i64;
|
|
float f[2];
|
|
double d;
|
|
}src;
|
|
|
|
switch (type)
|
|
{
|
|
case MAV_PARAM_TYPE_UINT8:
|
|
{
|
|
src.u8[0] = param.toUInt();
|
|
((uint8_t *)&value)[0] = src.u8[0];
|
|
|
|
}break;
|
|
case MAV_PARAM_TYPE_INT8:
|
|
{
|
|
src.i8[0] = param.toUInt();
|
|
((int8_t *)&value)[0] = src.i8[0];
|
|
}break;
|
|
case MAV_PARAM_TYPE_UINT16:
|
|
{
|
|
src.u16[0] = param.toUInt();
|
|
((uint16_t *)&value)[0] = src.u16[0];
|
|
}break;
|
|
case MAV_PARAM_TYPE_INT16:
|
|
{
|
|
src.i16[0] = param.toUInt();
|
|
((int16_t *)&value)[0] = src.i16[0];
|
|
}break;
|
|
case MAV_PARAM_TYPE_UINT32:
|
|
{
|
|
src.u32[0] = param.toUInt();
|
|
((uint32_t *)&value)[0] = (uint32_t)src.u32[0];
|
|
}break;
|
|
case MAV_PARAM_TYPE_INT32:
|
|
{
|
|
src.i32[0] = param.toUInt();
|
|
((int32_t *)&value)[0] = (int32_t)src.i32[0];
|
|
}break;
|
|
case MAV_PARAM_TYPE_UINT64:
|
|
{
|
|
src.u64 = param.toUInt();
|
|
((uint64_t *)&value)[0] = (uint64_t )src.u64;
|
|
}break;
|
|
case MAV_PARAM_TYPE_INT64:
|
|
{
|
|
src.i64 = param.toUInt();
|
|
((int64_t *)&value)[0] = (int64_t)src.i64;
|
|
}break;
|
|
case MAV_PARAM_TYPE_REAL32:
|
|
{
|
|
src.f[0] = param.toFloat();
|
|
((float *)&value)[0] = (float)src.f[0];
|
|
}break;
|
|
case MAV_PARAM_TYPE_REAL64:
|
|
{
|
|
src.d = param.toDouble();
|
|
((double *)&value)[0]= (double)src.d;
|
|
}break;
|
|
default:
|
|
{
|
|
src.f[0] = param.toFloat();
|
|
((float *)&value)[0] = (float)src.i32[0];
|
|
}break;
|
|
|
|
}
|
|
|
|
return value;
|
|
}
|
|
|
|
|
|
QVariant ParameterProcess::settype(mavlink_param_value_t param)
|
|
{
|
|
QVariant value = 0;
|
|
|
|
switch (param.param_type)
|
|
{
|
|
case MAV_PARAM_TYPE_UINT8:
|
|
{
|
|
value = "uint8";
|
|
}break;
|
|
case MAV_PARAM_TYPE_INT8:
|
|
{
|
|
value = "int8";
|
|
}break;
|
|
case MAV_PARAM_TYPE_UINT16:
|
|
{
|
|
value = "uint16";
|
|
}break;
|
|
case MAV_PARAM_TYPE_INT16:
|
|
{
|
|
value = "int16";
|
|
}break;
|
|
case MAV_PARAM_TYPE_UINT32:
|
|
{
|
|
value = "uint32";
|
|
}break;
|
|
case MAV_PARAM_TYPE_INT32:
|
|
{
|
|
value = "int32";
|
|
}break;
|
|
case MAV_PARAM_TYPE_UINT64:
|
|
{
|
|
value = "uint64";
|
|
}break;
|
|
case MAV_PARAM_TYPE_INT64:
|
|
{
|
|
value = "int64";
|
|
}break;
|
|
case MAV_PARAM_TYPE_REAL32:
|
|
{
|
|
value = "float";
|
|
}break;
|
|
case MAV_PARAM_TYPE_REAL64:
|
|
{
|
|
value = "double";
|
|
}break;
|
|
default:
|
|
{
|
|
value = "unknow";
|
|
}break;
|
|
|
|
}
|
|
|
|
return value;
|
|
}
|
|
|
|
uint8_t ParameterProcess::gettype(QVariant type)
|
|
{
|
|
uint8_t value = 0;
|
|
|
|
if(type.toString() == "uint8")
|
|
{
|
|
value = MAV_PARAM_TYPE_UINT8;
|
|
}
|
|
else if(type.toString() == "int8")
|
|
{
|
|
value = MAV_PARAM_TYPE_INT8;
|
|
}
|
|
else if(type.toString() == "uint16")
|
|
{
|
|
value = MAV_PARAM_TYPE_UINT16;
|
|
}
|
|
else if(type.toString() == "int16")
|
|
{
|
|
value = MAV_PARAM_TYPE_INT16;
|
|
}
|
|
else if(type.toString() == "uint32")
|
|
{
|
|
value = MAV_PARAM_TYPE_UINT32;
|
|
}
|
|
else if(type.toString() == "int32")
|
|
{
|
|
value = MAV_PARAM_TYPE_INT32;
|
|
}
|
|
else if(type.toString() == "uint64")
|
|
{
|
|
value = MAV_PARAM_TYPE_UINT64;
|
|
}
|
|
else if(type.toString() == "int64")
|
|
{
|
|
value = MAV_PARAM_TYPE_INT64;
|
|
}
|
|
else if(type.toString() == "float")
|
|
{
|
|
value = MAV_PARAM_TYPE_REAL32;
|
|
}
|
|
else if(type.toString() == "double")
|
|
{
|
|
value = MAV_PARAM_TYPE_REAL64;
|
|
}
|
|
else if(type.toString() == "unknow")
|
|
{
|
|
value = 0;
|
|
}
|
|
|
|
return value;
|
|
}
|
|
|
|
|
|
//浣跨敤QString
|
|
QString ParameterProcess::setid(char *id)//璁剧疆id,濡傛灉id灏忎簬16锛岄偅涔堣兘澶熸壘鍒°/0锛屽¦傛灉绛変簬鎴栬€呭ぇ浜庯紝閭ｄ箞浼氭壘涓嶅埌锛屽洜姝や贡鐮
|
|
{
|
|
QString Name;
|
|
|
|
Name.append(id);
|
|
|
|
if(Name.size() >= 16)
|
|
{
|
|
Name = Name.left(16);
|
|
}
|
|
|
|
return Name;
|
|
}
|
|
|
|
char * ParameterProcess::getid(QString id)//寰楀埌id
|
|
{
|
|
return id.toLatin1().data();
|
|
}
|