zhangzhuo/M300CO2
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Files
09a5436ad79856c74350b16f3037532a73f9e046
M300CO2/Source/MainAcqThread.cpp

281 lines
9.5 KiB
C++
Raw Normal View History

第一次提交
2023-03-22 09:47:10 +08:00
#include "MainAcqThread.h"
#include "unistd.h"
#include <cmath>
#include "hal_uart.h"
CMainAcqThread::CMainAcqThread(QObject* parent /*= nullptr*/)
{
m_iFlagCaptureStatus = 0;
iFlagIsPathGenerated = false;
m_clsCapTimer.setTimerType(Qt::PreciseTimer);
#ifdef ZZ_FLAG_TEST
//connect(&m_clsCapTimer, &QTimer::timeout, this, &CMainAcqThread::OnTestTimer);
#endif
//m_clsCapTimer.start()
}
CMainAcqThread::~CMainAcqThread()
{
}
int CMainAcqThread::SetupContext()
{
m_ctrlConfigParser.SetFilePath("/home/data/Settings/MainSettings.ini");
m_ctrlConfigParser.GetParams(m_struMiscCtrls,m_struM300RTKSs,m_struSensorPort);
#ifdef ZZ_FLAG_TEST
qDebug() << m_struSensorPort.qstrGasSensorPort;
qDebug() << m_struSensorPort.qstrWindSensorPort;
qDebug() << m_struMiscCtrls.iPumpGPIOPort;
qDebug() << m_struM300RTKSs.qstrM300RTKSettingsFilePath;
qDebug() << m_struM300RTKSs.qstrM300RTKWidgetFilePath;
#endif
m_ctrlVehicle.SetupEnvironment_M300RTK();
SetupMessagePipe();
return 0;
}
int CMainAcqThread::StartUp()
{
///pump control
system("sudo gpio mode 7 out");
m_ctrlVehicle.StartupPSDK_M300RTK();
QString qstrMessage = "Initializing sensors,Please wait...";
emit Signal_UpdateVehicleMessage(qstrMessage);
int iRes = m_ctrlGasSensor.Initialize(m_struSensorPort.qstrGasSensorPort.toStdString());
if (iRes!=0)
{
qstrMessage.clear();
qstrMessage.append("Fatal Error!!! Gas sensor not connected.Please check.");
//qstrMessage = "Fatal Error!!! Gas sensor not connected.Please check.";
emit Signal_UpdateVehicleMessage(qstrMessage);
qDebug() << qstrMessage;
//return 1;
}
iRes = m_ctrlWindSensor.Initialize(m_struSensorPort.qstrWindSensorPort.toStdString());
if (iRes != 0)
{
qstrMessage.append("Fatal Error!!! Wind sensor not connected.Please check.");
//qstrMessage = "Fatal Error!!! Wind sensor not connected.Please check.";
emit Signal_UpdateVehicleMessage(qstrMessage);
qDebug() << qstrMessage;
//return 2;
}
qstrMessage.append("System ready.Waiting for signals");
//qstrMessage = "System ready.Waiting for signals";
emit Signal_UpdateVehicleMessage(qstrMessage);
return 0;
}
int CMainAcqThread::SetupMessagePipe()
{
connect(&m_ctrlVehicle, &VehicleController::Signal_StartCapture, this, &CMainAcqThread::Slot_StartCapture);
connect(&m_ctrlVehicle, &VehicleController::Signal_StopCapture, this, &CMainAcqThread::Slot_StopCapture);
connect(this, &CMainAcqThread::Signal_UpdateVehicleMessage, &m_ctrlVehicle, &VehicleController::Signal_UpdateVehicleMessage);
connect(&m_clsCapTimer, &QTimer::timeout, this, &CMainAcqThread::OnTimerCapture);
return 0;
}
int CMainAcqThread::WindSensorCorrection()
{
QuaternionToRotationMatrix();
ConvertWindData();
RotateWindVec();
FormFixedWindData();
return 0;
}
int CMainAcqThread::QuaternionToRotationMatrix()
{
m_dRotationMatrix[0] = 1.0 - 2.0 * (m_struM300RTKDataFrame.stQuaternion.y_q2 * m_struM300RTKDataFrame.stQuaternion.y_q2 + m_struM300RTKDataFrame.stQuaternion.z_q3 * m_struM300RTKDataFrame.stQuaternion.z_q3);
m_dRotationMatrix[1] = 2.0 * (m_struM300RTKDataFrame.stQuaternion.x_q1 * m_struM300RTKDataFrame.stQuaternion.y_q2 - m_struM300RTKDataFrame.stQuaternion.z_q3 * m_struM300RTKDataFrame.stQuaternion.w_q0);
m_dRotationMatrix[2] = 2.0 * (m_struM300RTKDataFrame.stQuaternion.x_q1 * m_struM300RTKDataFrame.stQuaternion.z_q3 + m_struM300RTKDataFrame.stQuaternion.y_q2 * m_struM300RTKDataFrame.stQuaternion.w_q0);
m_dRotationMatrix[3] = 2.0 * (m_struM300RTKDataFrame.stQuaternion.x_q1 * m_struM300RTKDataFrame.stQuaternion.y_q2 + m_struM300RTKDataFrame.stQuaternion.z_q3 * m_struM300RTKDataFrame.stQuaternion.w_q0);
m_dRotationMatrix[4] = 1.0 - 2.0 * (m_struM300RTKDataFrame.stQuaternion.x_q1 * m_struM300RTKDataFrame.stQuaternion.x_q1 + m_struM300RTKDataFrame.stQuaternion.z_q3 * m_struM300RTKDataFrame.stQuaternion.z_q3);
m_dRotationMatrix[5] = 2.0 * (m_struM300RTKDataFrame.stQuaternion.y_q2 * m_struM300RTKDataFrame.stQuaternion.z_q3 - m_struM300RTKDataFrame.stQuaternion.x_q1 * m_struM300RTKDataFrame.stQuaternion.w_q0);
m_dRotationMatrix[6] = 2.0 * (m_struM300RTKDataFrame.stQuaternion.x_q1 * m_struM300RTKDataFrame.stQuaternion.z_q3 - m_struM300RTKDataFrame.stQuaternion.y_q2 * m_struM300RTKDataFrame.stQuaternion.w_q0);
m_dRotationMatrix[7] = 2.0 * (m_struM300RTKDataFrame.stQuaternion.y_q2 * m_struM300RTKDataFrame.stQuaternion.z_q3 + m_struM300RTKDataFrame.stQuaternion.x_q1 * m_struM300RTKDataFrame.stQuaternion.w_q0);
m_dRotationMatrix[8] = 1.0 - 2.0 * (m_struM300RTKDataFrame.stQuaternion.x_q1 * m_struM300RTKDataFrame.stQuaternion.x_q1 + m_struM300RTKDataFrame.stQuaternion.y_q2 * m_struM300RTKDataFrame.stQuaternion.y_q2);
// R[0] = r11; R[1] = r12; R[2] = r13;
// R[3] = r21; R[4] = r22; R[5] = r23;
// R[6] = r31; R[7] = r32; R[8] = r33;
return 0;
}
int CMainAcqThread::ConvertWindData()
{
m_fTempWindVecX = m_struUASDataFrame.fWindSpeed * cos(m_struUASDataFrame.fWindDirection / 180 * 3.14159);
m_fTempWindVecY = m_struUASDataFrame.fWindSpeed * sin(m_struUASDataFrame.fWindDirection / 180 * 3.14159);
m_fTempWindVecZ = 0;
return 0;
}
int CMainAcqThread::RotateWindVec()
{
m_fTempFixedWindVecX = m_dRotationMatrix[0] * m_fTempWindVecX + m_dRotationMatrix[1] * m_fTempWindVecY /*+ m_dRotationMatrix[2] * m_fTempWindVecZ*/;
m_fTempFixedWindVecY = m_dRotationMatrix[3] * m_fTempWindVecX + m_dRotationMatrix[4] * m_fTempWindVecY /*+ m_dRotationMatrix[5] * m_fTempWindVecZ*/;
m_fTempFixedWindVecZ = m_dRotationMatrix[6] * m_fTempWindVecX + m_dRotationMatrix[7] * m_fTempWindVecY /*+ m_dRotationMatrix[8] * m_fTempWindVecZ*/;
return 0;
}
int CMainAcqThread::FormFixedWindData()
{
m_fTempFixedWindVecX = m_fTempFixedWindVecX-m_struM300RTKDataFrame.stVelocity.x;
m_fTempFixedWindVecY = m_fTempFixedWindVecY - m_struM300RTKDataFrame.stVelocity.y;
m_fTempFixedWindVecZ = m_fTempFixedWindVecZ - m_struM300RTKDataFrame.stVelocity.z;
double dDotProduct = m_fTempFixedWindVecX; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double dVectorLength = sqrt(m_fTempFixedWindVecX * m_fTempFixedWindVecX + m_fTempFixedWindVecY * m_fTempFixedWindVecY + m_fTempFixedWindVecZ * m_fTempFixedWindVecZ); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3>
double dAngleInRadians = acos(dDotProduct / dVectorLength); // <20><><EFBFBD><EFBFBD><EFBFBD>нǵĻ<C7B5><C4BB><EFBFBD>ֵ
double angleInDegrees = dAngleInRadians * (180.0 / M_PI); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
m_struUASDataFrame.fFixedWindDirection = angleInDegrees;
m_struUASDataFrame.fFixedWindSpeed = dVectorLength;
return 0;
}
void CMainAcqThread::OnTestTimer()
{
qDebug() << "entering time:"<< QTime::currentTime();
QTime currentTime;
int elapsed = 0;
if (lastTime == QTime())
{
lastTime = QTime::currentTime();
}
else
{
currentTime = QTime::currentTime();
elapsed = lastTime.msecsTo(currentTime);
lastTime = QTime:: currentTime();
}
qDebug() << "Run.elapsed =" << elapsed << "ms";
for (int i=0;i< 99999999;i++)
{
int b = i + 1;
if (b%2==0)
{
b = b + 2;
}
}
qDebug() << "leave time:" << QTime::currentTime();
}
int CMainAcqThread::GetData()
{
unsigned long ulCO2=0, ulH2O=0;
float fTemp=-1,fPP=-1,fPB=-1;
m_ctrlGasSensor.GetMeasuredData(ulCO2, ulH2O, fTemp, fPP, fPB);
m_struGSDataFrame.ulCO2 = ulCO2;
m_struGSDataFrame.ulH2O = ulH2O;
m_struGSDataFrame.fTemp = fTemp;
m_struGSDataFrame.fPP = fPP;
m_struGSDataFrame.fPB = fPB;
float fSpeed=-1, fAngle=-1;
m_ctrlWindSensor.GetSA_NChk(fSpeed,fAngle);
m_struUASDataFrame.fWindDirection = fAngle;
m_struUASDataFrame.fWindSpeed = fSpeed;
1.添加了EGM96-5的数据集和算法支持，可以正确求解GEOID Offset高度。 2.在同步时间时现在会将起飞点的海拔高度写回到配置文件/home/data/Settings/MainSettings.ini键值为WBACK/HeightOfHomePoint 3.现在的高度回调函数以及获取函数调整为了DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_FUSED。
2023-04-07 15:46:09 +08:00
m_ctrlVehicle.GetOneDataFrame(m_struM300RTKDataFrame);//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
第一次提交
2023-03-22 09:47:10 +08:00
}
int CMainAcqThread::OnTimerCapture()
{
#ifdef ZZ_FLAG_TEST
qDebug() << "OnTimerCapture entering time:" << QTime::currentTime();
#endif
//m_ctrlGasSensor.GetMeasuredData();
//m_ctrlWindSensor.GetSA_NChk();
//m_ctrlVehicle.GetOneDataFrame(m_struM300RTKDataFrame);
GetData();
WindSensorCorrection();
//////////////////////////////////////////////////////////////////////////update message to controller
//m_struGSDataFrame.ulH2O = 223;
//m_struGSDataFrame.ulCO2 = 530;
QString qstrMessage;
qstrMessage = QString("CO2: %1ppm, H20: %2ppm\r\nFixed Wind Speed: %3m/s\r\nFixed Wind Direction: %4deg").arg(m_struGSDataFrame.ulCO2).arg(m_struGSDataFrame.ulH2O)
.arg(m_struUASDataFrame.fFixedWindSpeed).arg(m_struUASDataFrame.fFixedWindDirection);
emit Signal_UpdateVehicleMessage(qstrMessage);
//////////////////////////////////////////////////////////////////////////
#ifdef ZZ_FLAG_TEST
qDebug() << m_struM300RTKDataFrame.stGPSPosition.x << m_struM300RTKDataFrame.stGPSPosition.y << m_struM300RTKDataFrame.stGPSPosition.z
<< m_struM300RTKDataFrame.stVelocity.x << m_struM300RTKDataFrame.stVelocity.y << m_struM300RTKDataFrame.stVelocity.z;
#endif
m_ctrlData.WriteData(m_struM300RTKDataFrame, m_struGSDataFrame, m_struUASDataFrame);
#ifdef ZZ_FLAG_TEST
qDebug() << "OnTimerCapture leave time:" << QTime::currentTime();
#endif
return 0;
}
int CMainAcqThread::Slot_StartCapture()
{
// ground test
system("sudo gpio write 7 1");
iFlagIsPathGenerated = true;
m_ctrlVehicle.UpdateUIConfig();
m_ctrlData.GenerateFilePath();
m_ctrlData.GenerateFile();
m_clsCapTimer.start(1000);
#ifdef ZZ_FLAG_TEST
qDebug() << "CMainAcqThread Test Cap Started";
#endif // ZZ_FLAG_TEST
return 0;
}
int CMainAcqThread::Slot_StopCapture()
{
// ground test
system("sudo gpio write 7 0");
iFlagIsPathGenerated = false;
m_clsCapTimer.stop();
m_ctrlData.CloseData();
#ifdef ZZ_FLAG_TEST
qDebug() << "CMainAcqThread Test Cap Stopped";
#endif // ZZ_FLAG_TEST
QString qstrMessage = QString("Capture Stopped.");
emit Signal_UpdateVehicleMessage(qstrMessage);
return 0;
}
Reference in New Issue Copy Permalink