tangchao0503/airborne_CO2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

M350b版本

Browse Source
This commit is contained in:
xin
2026-01-08 16:00:08 +08:00
parent 7396728ea7
commit a76d4b77e9
213 changed files with 8883 additions and 7196579 deletions
Download Patch File Download Diff File Expand all files Collapse all files

500
Source/M300/PSDK_Qt/Main/VehicleController.cpp
View File

@ -14,13 +14,13 @@
#include "hal_network.h"
#include "time.h"
//#include "ZZ_Types_M300.h"
#include "ZZ_Types_DJI.h"
using namespace ZZ_DATA_DEF::DJI;
#define ZZ_UNUSED(x) ((x) = (x))
#define ZZ_MIN(a, b) (((a) < (b)) ? (a) : (b))
#define ZZ_MAX(a, b) (((a) > (b)) ? (a) : (b))
#include "dji_low_speed_data_channel.h"
#include "dji_high_speed_data_channel.h"
int VehicleController::m_siFlagIsPumpWorking = 0;
int VehicleController::m_siFlagIsStartCaptureSignalEmitted = 0;
VehicleController* VehicleController::spCaller = NULL;
@ -34,7 +34,7 @@ VehicleController::VehicleController(QObject* parent /*= nullptr*/)
m_fHeightOfHomePoint_BM = -1;
//m_cCaptureMode = 0;
}
VehicleController::~VehicleController()
{
@ -50,27 +50,27 @@ T_DjiReturnCode VehicleController::DjiTest_FcSubscriptionReceiveFlightStatusCall
T_DjiReturnCode VehicleController::DjiTest_FcSubscriptionReceiveHeightCallback(const uint8_t* data, uint16_t dataSize, const T_DjiDataTimestamp* timestamp)
{
// float fHeight = *((T_DjiFcSubscriptionAltitudeFused*)data);
// //qDebug() <<"[Fused Height]:"<< fHeight;
float fHeight = *((T_DjiFcSubscriptionAltitudeFused*)data);
//qDebug() <<"[Fused Height]:"<< fHeight;
// ///Pump need to be added
// if (m_siFlagIsPumpWorking==0&& fHeight > 10)
//{
// //system("sudo gpio mode 7 out");
// m_siFlagIsPumpWorking = 1;
// system("sudo gpio write 7 1");
// qDebug() << "[Fused Height]:" << fHeight;
// }
// if (m_siFlagIsPumpWorking == 1 && fHeight < 10)
// {
// //system("sudo gpio mode 7 out");
// m_siFlagIsPumpWorking = 0;
// system("sudo gpio write 7 0");
// qDebug() << "[Fused Height]:" << fHeight;
// }
//
// //T_DjiOsalHandler* osalHandler = DjiPlatform_GetOsalHandler();
// //osalHandler->TaskSleepMs(2000);
///Pump need to be added
if (m_siFlagIsPumpWorking==0&& fHeight > 10)
{
//system("sudo gpio mode 7 out");
m_siFlagIsPumpWorking = 1;
system("sudo gpio write 7 1");
qDebug() << "[Fused Height]:" << fHeight;
}
if (m_siFlagIsPumpWorking == 1 && fHeight < 10)
{
//system("sudo gpio mode 7 out");
m_siFlagIsPumpWorking = 0;
system("sudo gpio write 7 0");
qDebug() << "[Fused Height]:" << fHeight;
}
//T_DjiOsalHandler* osalHandler = DjiPlatform_GetOsalHandler();
//osalHandler->TaskSleepMs(2000);
return 0;
}
@ -83,126 +83,152 @@ T_DjiReturnCode VehicleController::ZZ_DjiWaypointV2EventCallback(T_DjiWaypointV2
}
if (eventData.event == 0x10)
{
// if (eventData.data.waypointIndex==0 && m_siFlagIsStartCaptureSignalEmitted==0)
// {
// if (m_scCaptureMode==0)
// {
// m_siFlagIsStartCaptureSignalEmitted++;
//spCaller->emit Signal_StartCapture();
// }
// // emit Signal_StartCapture();
//
// }
if (eventData.data.waypointIndex==0 && m_siFlagIsStartCaptureSignalEmitted==0)
{
if (m_scCaptureMode==0)
{
m_siFlagIsStartCaptureSignalEmitted++;
spCaller->emit Signal_StartCapture();
}
// emit Signal_StartCapture();
}
}
if (eventData.event == 0x11)
{
qDebug() << "eventData.event" << m_siFlagIsStartCaptureSignalEmitted;
/*if (m_siFlagIsStartCaptureSignalEmitted > 0)
if (m_siFlagIsStartCaptureSignalEmitted > 0)
{
m_siFlagIsStartCaptureSignalEmitted = 0;
spCaller->emit Signal_StopCapture();
}*/
}
}
return 0;
}
int VehicleController::Initialize()
{
//m_clsWidget.GetSettings(m_struUIConfig);
m_clsConfigParser.GetParams(m_struAppRegInfo, m_struHardwareInfo, m_struSensorPort);
//m_clsConfigParser.UpdateUIConfig(m_struUIConfig);
return 0;
}
int VehicleController::SetupEnvironment()
static T_DjiReturnCode DjiUser_LocalWrite(const uint8_t *data, uint16_t dataLen)
{
// using namespace ZZ::Device::DJI::M300RTK;
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
#include<dji_power_management.h>
#include<test_power_management.h>
static T_DjiReturnCode DjiTest_HighPowerApplyPinInit()
{
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
static T_DjiReturnCode DjiTest_WriteHighPowerApplyPin(E_DjiPowerManagementPinState pinState)
{
//attention: please pull up the HWPR pin state by hardware.
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
int VehicleController::SetupEnvironment_M300RTK()
{
using namespace ZZ::Device::DJI;
spCaller = this;
//for test
//ZZ_ConfigParser_M300RTK clsConfigParser;
//clsConfigParser.GetParams(m_struAppRegInfo, m_struHardwareInfo, m_struUIConfig);
//
//
//for use
//m_clsConfigParser.GetParams(m_struAppRegInfo, m_struHardwareInfo, m_struUIConfig);
m_clsConfigParser.GetParams(m_struAppRegInfo, m_struHardwareInfo, m_struUIConfig);
T_DjiReturnCode returnCode;
/*********************** OSAL HANDLER *************************/
T_DjiOsalHandler osalHandler;
memset(&osalHandler, 0, sizeof(T_DjiOsalHandler));
T_DjiHalUsbBulkHandler usbBulkHandler = { 0 };
osalHandler.TaskCreate = Osal_TaskCreate;
osalHandler.TaskDestroy = Osal_TaskDestroy;
osalHandler.TaskSleepMs = Osal_TaskSleepMs;
osalHandler.MutexCreate = Osal_MutexCreate;
osalHandler.MutexDestroy = Osal_MutexDestroy;
osalHandler.MutexLock = Osal_MutexLock;
osalHandler.MutexUnlock = Osal_MutexUnlock;
osalHandler.SemaphoreCreate = Osal_SemaphoreCreate;
osalHandler.SemaphoreDestroy = Osal_SemaphoreDestroy;
osalHandler.SemaphoreWait = Osal_SemaphoreWait;
osalHandler.SemaphoreTimedWait = Osal_SemaphoreTimedWait;
osalHandler.SemaphorePost = Osal_SemaphorePost;
osalHandler.Malloc = Osal_Malloc;
osalHandler.Free = Osal_Free;
osalHandler.GetRandomNum = Osal_GetRandomNum;
osalHandler.GetTimeMs = Osal_GetTimeMs;
osalHandler.GetTimeUs = Osal_GetTimeUs;
///////////
T_DjiReturnCode returnCode;
T_DjiOsalHandler osalHandler = { 0 };
T_DjiHalUartHandler uartHandler = { 0 };
T_DjiHalUsbBulkHandler usbBulkHandler = { 0 };
T_DjiLoggerConsole printConsole;
T_DjiLoggerConsole localRecordConsole;
T_DjiFileSystemHandler fileSystemHandler = { 0 };
T_DjiSocketHandler socketHandler{ 0 };
T_DjiHalNetworkHandler networkHandler = { 0 };
/*********************** LOGGER *************************/
T_DjiLoggerConsole loggerConsole;
memset(&loggerConsole, 0, sizeof(T_DjiLoggerConsole));
networkHandler.NetworkInit = HalNetWork_Init;
networkHandler.NetworkDeInit = HalNetWork_DeInit;
loggerConsole.func = DjiUser_PrintMessage;
loggerConsole.consoleLevel = DJI_LOGGER_CONSOLE_LOG_LEVEL_INFO;
loggerConsole.isSupportColor = true;
/*********************** UART *************************/
T_DjiHalUartHandler uartHandler;
memset(&uartHandler, 0, sizeof(T_DjiHalUartHandler));
uartHandler.UartInit = HalUart_Init;
uartHandler.UartDeInit = HalUart_DeInit;
uartHandler.UartWriteData = HalUart_WriteData;
uartHandler.UartReadData = HalUart_ReadData;
uartHandler.UartGetStatus = HalUart_GetStatus;
uartHandler.UartGetDeviceInfo = HalUart_GetDeviceInfo;
/*********************** NETWORK *************************/
T_DjiHalNetworkHandler networkHandler;
memset(&networkHandler, 0, sizeof(T_DjiHalNetworkHandler));
networkHandler.NetworkInit = HalNetWork_Init;
networkHandler.NetworkDeInit = HalNetWork_DeInit;
networkHandler.NetworkGetDeviceInfo = HalNetWork_GetDeviceInfo;
socketHandler.Socket = Osal_Socket;
socketHandler.Bind = Osal_Bind;
socketHandler.Close = Osal_Close;
socketHandler.UdpSendData = Osal_UdpSendData;
socketHandler.UdpRecvData = Osal_UdpRecvData;
socketHandler.TcpListen = Osal_TcpListen;
socketHandler.TcpAccept = Osal_TcpAccept;
socketHandler.TcpConnect = Osal_TcpConnect;
socketHandler.TcpSendData = Osal_TcpSendData;
socketHandler.TcpRecvData = Osal_TcpRecvData;
/*********************** FILE SYSTEM *************************/
T_DjiFileSystemHandler fileSystemHandler;
memset(&fileSystemHandler, 0, sizeof(T_DjiFileSystemHandler));
osalHandler.TaskCreate = Osal_TaskCreate;
osalHandler.TaskDestroy = Osal_TaskDestroy;
osalHandler.TaskSleepMs = Osal_TaskSleepMs;
osalHandler.MutexCreate = Osal_MutexCreate;
osalHandler.MutexDestroy = Osal_MutexDestroy;
osalHandler.MutexLock = Osal_MutexLock;
osalHandler.MutexUnlock = Osal_MutexUnlock;
osalHandler.SemaphoreCreate = Osal_SemaphoreCreate;
osalHandler.SemaphoreDestroy = Osal_SemaphoreDestroy;
osalHandler.SemaphoreWait = Osal_SemaphoreWait;
osalHandler.SemaphoreTimedWait = Osal_SemaphoreTimedWait;
osalHandler.SemaphorePost = Osal_SemaphorePost;
osalHandler.Malloc = Osal_Malloc;
osalHandler.Free = Osal_Free;
osalHandler.GetTimeMs = Osal_GetTimeMs;
osalHandler.GetTimeUs = Osal_GetTimeUs;
osalHandler.GetRandomNum = Osal_GetRandomNum;
fileSystemHandler.FileOpen = Osal_FileOpen;
fileSystemHandler.FileClose = Osal_FileClose;
fileSystemHandler.FileWrite = Osal_FileWrite;
fileSystemHandler.FileRead = Osal_FileRead;
fileSystemHandler.FileSync = Osal_FileSync;
fileSystemHandler.FileSeek = Osal_FileSeek;
fileSystemHandler.DirOpen = Osal_DirOpen;
fileSystemHandler.DirClose = Osal_DirClose;
fileSystemHandler.DirRead = Osal_DirRead;
fileSystemHandler.Mkdir = Osal_Mkdir;
fileSystemHandler.Unlink = Osal_Unlink;
fileSystemHandler.Rename = Osal_Rename;
fileSystemHandler.Stat = Osal_Stat;
uartHandler.UartInit = HalUart_Init;
uartHandler.UartDeInit = HalUart_DeInit;
uartHandler.UartWriteData = HalUart_WriteData;
uartHandler.UartReadData = HalUart_ReadData;
uartHandler.UartGetStatus = HalUart_GetStatus;
/*********************** SOCKET *************************/
T_DjiSocketHandler socketHandler;
memset(&socketHandler, 0, sizeof(T_DjiSocketHandler));
usbBulkHandler.UsbBulkInit = HalUsbBulk_Init;
usbBulkHandler.UsbBulkDeInit = HalUsbBulk_DeInit;
usbBulkHandler.UsbBulkWriteData = HalUsbBulk_WriteData;
usbBulkHandler.UsbBulkReadData = HalUsbBulk_ReadData;
usbBulkHandler.UsbBulkGetDeviceInfo = HalUsbBulk_GetDeviceInfo;
fileSystemHandler.FileOpen = Osal_FileOpen,
fileSystemHandler.FileClose = Osal_FileClose,
fileSystemHandler.FileWrite = Osal_FileWrite,
fileSystemHandler.FileRead = Osal_FileRead,
fileSystemHandler.FileSync = Osal_FileSync,
fileSystemHandler.FileSeek = Osal_FileSeek,
fileSystemHandler.DirOpen = Osal_DirOpen,
fileSystemHandler.DirClose = Osal_DirClose,
fileSystemHandler.DirRead = Osal_DirRead,
fileSystemHandler.Mkdir = Osal_Mkdir,
fileSystemHandler.Unlink = Osal_Unlink,
fileSystemHandler.Rename = Osal_Rename,
fileSystemHandler.Stat = Osal_Stat,
printConsole.func = DjiUser_PrintMessage;
printConsole.consoleLevel = DJI_LOGGER_CONSOLE_LOG_LEVEL_INFO;
printConsole.isSupportColor = false;
socketHandler.Socket = Osal_Socket;
socketHandler.Bind = Osal_Bind;
socketHandler.Close = Osal_Close;
socketHandler.UdpSendData = Osal_UdpSendData;
socketHandler.UdpRecvData = Osal_UdpRecvData;
socketHandler.TcpListen = Osal_TcpListen;
socketHandler.TcpAccept = Osal_TcpAccept;
socketHandler.TcpConnect = Osal_TcpConnect;
socketHandler.TcpSendData = Osal_TcpSendData;
socketHandler.TcpRecvData = Osal_TcpRecvData;
usbBulkHandler.UsbBulkInit = HalUsbBulk_Init;
usbBulkHandler.UsbBulkDeInit = HalUsbBulk_DeInit;
usbBulkHandler.UsbBulkWriteData = HalUsbBulk_WriteData;
usbBulkHandler.UsbBulkReadData = HalUsbBulk_ReadData;
usbBulkHandler.UsbBulkGetDeviceInfo = HalUsbBulk_GetDeviceInfo;
returnCode = DjiPlatform_RegOsalHandler(&osalHandler);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
@ -217,50 +243,41 @@ int VehicleController::SetupEnvironment()
qDebug() << "VehicleController: Func DjiPlatform_RegHalUartHandler. Register hal uart handler error";
throw std::runtime_error("Register hal uart handler error.");
}
//
// if (/*m_struHardwareInfo.enumHCM == HardwareConnectionMode::UartAndUSBBulk*/1)
// if (m_struHardwareInfo.enumHCM == HardwareConnectionMode::UartAndNetwork)
// {
// returnCode = DjiPlatform_RegHalUsbBulkHandler(&usbBulkHandler);
// returnCode = DjiPlatform_RegHalNetworkHandler(&networkHandler);
// if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
// {
// qDebug() << "VehicleController: Func DjiPlatform_RegHalUsbBulkHandler. Register hal usb bulk handler error";
// throw std::runtime_error("Register hal usb bulk handler error.");
// qDebug() << "VehicleController: Func DjiPlatform_RegHalNetworkHandler. Register hal network handler error";
// throw std::runtime_error("Register hal network handler error");
// }
// }
if (/*m_struHardwareInfo.enumHCM == HardwareConnectionMode::UartAndNetwork*/ 1)
{
returnCode = DjiPlatform_RegHalNetworkHandler(&networkHandler);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
qDebug() << "VehicleController: Func DjiPlatform_RegHalNetworkHandler. Register hal network handler error";
throw std::runtime_error("Register hal network handler error");
}
}
//else if (m_struHardwareInfo.enumHCM == HardwareConnectionMode::UartAndUSBBulk)
//{
// returnCode = DjiPlatform_RegHalUsbBulkHandler(&usbBulkHandler);
// if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
// {
// qDebug() << "VehicleController: Func DjiPlatform_RegHalUsbBulkHandler. Register hal usb bulk handler error";
// throw std::runtime_error("Register hal usb bulk handler error.");
// }
// }
//else if (m_struHardwareInfo.enumHCM == HardwareConnectionMode::UartOnly)
//{
// returnCode = DjiPlatform_RegSocketHandler(&socketHandler);
// if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
// {
// qDebug() << "VehicleController: Func DjiPlatform_RegSocketHandler. Register osal socket handler error";
// throw std::runtime_error("register osal socket handler error");
// }
//}
//else
//{
// qDebug() << "VehicleController: Func DjiPlatform_RegSocketHandler. Register osal socket handler error";
// throw std::runtime_error("hardware connection configuration error");
//}
{
//<2F><><EFBFBD><EFBFBD>
returnCode = DjiPlatform_RegHalNetworkHandler(&networkHandler);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
printf("register hal network handler error");
return DJI_ERROR_SYSTEM_MODULE_CODE_SYSTEM_ERROR;
}
//
// //Attention: if you want to use camera stream view function, please uncomment it.
returnCode = DjiPlatform_RegSocketHandler(&socketHandler);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
printf("register osal socket handler error");
return DJI_ERROR_SYSTEM_MODULE_CODE_SYSTEM_ERROR;
}
}
// {
// returnCode = DjiPlatform_RegHalUsbBulkHandler(&usbBulkHandler);
// if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
// printf("register hal usb bulk handler error");
// return DJI_ERROR_SYSTEM_MODULE_CODE_SYSTEM_ERROR;
// }
//
// }
returnCode = DjiPlatform_RegFileSystemHandler(&fileSystemHandler);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
@ -268,7 +285,7 @@ int VehicleController::SetupEnvironment()
throw std::runtime_error("Register osal filesystem handler error.");
}
returnCode = DjiLogger_AddConsole(&printConsole);
returnCode = DjiLogger_AddConsole(&loggerConsole);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
qDebug() << "VehicleController: Func DjiPlatform_RegFileSystemHandler. Register osal filesystem handler error";
@ -276,12 +293,19 @@ int VehicleController::SetupEnvironment()
}
//
// returnCode = DjiLogger_AddConsole(&localRecordConsole);
// if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
// printf("add printf console error");
// return DJI_ERROR_SYSTEM_MODULE_CODE_SYSTEM_ERROR;
// }
return 0;
}
int VehicleController::StartupPSDK_M350RTK()
{
int VehicleController::StartupPSDK_M300RTK()
{
T_DjiUserInfo userInfo;
T_DjiReturnCode returnCode;
T_DjiAircraftInfoBaseInfo aircraftInfoBaseInfo;
@ -290,19 +314,19 @@ int VehicleController::StartupPSDK_M350RTK()
//signal(SIGTERM, DjiUser_NormalExitHandler);
returnCode = LoadUserAppInfo(&userInfo);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
throw std::runtime_error("Fill user info error, please check user info config.");
}
returnCode = DjiCore_SetSerialNumber("IRIS00001");
returnCode = DjiCore_Init(&userInfo);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
throw std::runtime_error("Core init error.");
}
returnCode = DjiAircraftInfo_GetBaseInfo(&aircraftInfoBaseInfo);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
throw std::runtime_error("Get aircraft base info error.");
}
@ -313,39 +337,54 @@ int VehicleController::StartupPSDK_M350RTK()
// }
returnCode = DjiCore_SetAlias("PSDK_APPALIAS");
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
throw std::runtime_error("Set alias error.");
}
returnCode = DjiCore_ApplicationStart();
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
throw std::runtime_error("Start sdk application error.");
}
// T_DjiTestApplyHighPowerHandler applyHighPowerHandler = {
// .pinInit = DjiTest_HighPowerApplyPinInit,
// .pinWrite = DjiTest_WriteHighPowerApplyPin,
// };
//
// returnCode = DjiTest_RegApplyHighPowerHandler(&applyHighPowerHandler);
// if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
// USER_LOG_ERROR("regsiter apply high power handler error");
// }
//
// returnCode = DjiTest_PowerManagementStartService();
// if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
// USER_LOG_ERROR("power management init error");
// }
osalHandler->TaskSleepMs(5000);
SetupWidget();
SetupWidget();//<2F>?<EFBFBD><C9BC>?<EFBFBD><DFBC><3E><><EFBFBD>
osalHandler->TaskSleepMs(5000);
SetupMessagePipe();
//SetupWaypointStatusCallback();
//InitSystemParams();
SetupSubscriptions();
SetupMessagePipe();//widget<65><74>vehicle
// SetupWaypointStatusCallback();
// InitSystemParams();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?<EFBFBD>?<EFBFBD><E4A3AC><EFBFBD><EFBFBD>???<CDB3>??<E4A3AC><C8BB>?<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD>?<EFBFBD><C4A3><EFBFBD>?<EFBFBD>?<EFBFBD>??<CDAC><CAB1><EFBFBD><EFBFBD>5<EFBFBD><35><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SetupSubscriptions();//?<><CAB5><EFBFBD><EFBFBD>?<EFBFBD>?<EFBFBD><C4B6>?<EFBFBD><C4A3><EFBFBD><EFBFBD><EFBFBD>?<EFBFBD><CCAB><EFBFBD><EFBFBD>
qDebug()<<"M350RTK PSDK Channel Started.";
qDebug()<<"M300RTK PSDK Channel Started.";
return 0;
}
int VehicleController::UpdateUIConfig()
{
//m_clsWidget.GetSettings(m_struUIConfig);
//m_clsConfigParser.UpdateUIConfig(m_struUIConfig);
m_clsWidget.GetSettings(m_struUIConfig);
m_clsConfigParser.UpdateUIConfig(m_struUIConfig);
return 0;
}
int VehicleController::GetOneDataFrame(M350RTKDataFrame& M350RTKDataFrame)
int VehicleController::GetOneDataFrame(M300RTKDataFrame& M300RTKDataFrame)
{
T_DjiReturnCode tDjiReturnCode;
T_DjiFcSubscriptionVelocity tDjiVelocity = { 0 };
T_DjiFcSubscriptionQuaternion tDjiQuaternion = { 0 };
@ -388,29 +427,29 @@ int VehicleController::GetOneDataFrame(M350RTKDataFrame& M350RTKDataFrame)
//return 4;
}
M350RTKDataFrame.stVelocity.x = tDjiVelocity.data.x;
M350RTKDataFrame.stVelocity.y = tDjiVelocity.data.y;
M350RTKDataFrame.stVelocity.z = tDjiVelocity.data.z;
M300RTKDataFrame.stVelocity.x = tDjiVelocity.data.x;
M300RTKDataFrame.stVelocity.y = tDjiVelocity.data.y;
M300RTKDataFrame.stVelocity.z = tDjiVelocity.data.z;
M350RTKDataFrame.stQuaternion.w_q0 = tDjiQuaternion.q0;
M350RTKDataFrame.stQuaternion.x_q1 = tDjiQuaternion.q1;
M350RTKDataFrame.stQuaternion.y_q2 = tDjiQuaternion.q2;
M350RTKDataFrame.stQuaternion.z_q3 = tDjiQuaternion.q3;
M300RTKDataFrame.stQuaternion.w_q0 = tDjiQuaternion.q0;
M300RTKDataFrame.stQuaternion.x_q1 = tDjiQuaternion.q1;
M300RTKDataFrame.stQuaternion.y_q2 = tDjiQuaternion.q2;
M300RTKDataFrame.stQuaternion.z_q3 = tDjiQuaternion.q3;
M350RTKDataFrame.stGPSPosition.x = tDjiGpsPosition.x;
M350RTKDataFrame.stGPSPosition.y = tDjiGpsPosition.y;
M350RTKDataFrame.stGPSPosition.z = tDjiGpsPosition.z;
M300RTKDataFrame.stGPSPosition.x = tDjiGpsPosition.x;
M300RTKDataFrame.stGPSPosition.y = tDjiGpsPosition.y;
M300RTKDataFrame.stGPSPosition.z = tDjiGpsPosition.z;
M350RTKDataFrame.fAltitudeFused = tDjiAltFused;
M300RTKDataFrame.fAltitudeFused = tDjiAltFused;
//////////////////////////////////////////////////////////////////////////Calc Euler From Quaternion 20230428
M350RTKDataFrame.stEulerAngle.pitch = (dji_f64_t)asinf(-2 * tDjiQuaternion.q1 * tDjiQuaternion.q3 + 2 * tDjiQuaternion.q0 * tDjiQuaternion.q2) * 57.3;
M300RTKDataFrame.stEulerAngle.pitch = (dji_f64_t)asinf(-2 * tDjiQuaternion.q1 * tDjiQuaternion.q3 + 2 * tDjiQuaternion.q0 * tDjiQuaternion.q2) * 57.3;
M350RTKDataFrame.stEulerAngle.roll = (dji_f64_t)atan2f(2 * tDjiQuaternion.q2 * tDjiQuaternion.q3 + 2 * tDjiQuaternion.q0 * tDjiQuaternion.q1,
M300RTKDataFrame.stEulerAngle.roll = (dji_f64_t)atan2f(2 * tDjiQuaternion.q2 * tDjiQuaternion.q3 + 2 * tDjiQuaternion.q0 * tDjiQuaternion.q1,
-2 * tDjiQuaternion.q1 * tDjiQuaternion.q1 - 2 * tDjiQuaternion.q2 * tDjiQuaternion.q2 + 1) * 57.3;
M350RTKDataFrame.stEulerAngle.yaw = (dji_f64_t)atan2f(2 * tDjiQuaternion.q1 * tDjiQuaternion.q2 + 2 * tDjiQuaternion.q0 * tDjiQuaternion.q3,
M300RTKDataFrame.stEulerAngle.yaw = (dji_f64_t)atan2f(2 * tDjiQuaternion.q1 * tDjiQuaternion.q2 + 2 * tDjiQuaternion.q0 * tDjiQuaternion.q3,
-2 * tDjiQuaternion.q2 * tDjiQuaternion.q2 - 2 * tDjiQuaternion.q3 * tDjiQuaternion.q3 + 1) * 57.3;
//////////////////////////////////////////////////////////////////////////
@ -426,24 +465,24 @@ int VehicleController::SetupSubscriptions()
{
T_DjiReturnCode tDjiReturnCode;
tDjiReturnCode = DjiFcSubscription_Init();
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
qDebug() << "init data subscription module error.";
return 0;
}
else
{
qDebug() << "init data subscription module finished.";
}
tDjiReturnCode = DjiFcSubscription_Init();
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
qDebug() << "init data subscription module error.";
return 0;
}
else
{
qDebug() << "init data subscription module finished.";
}
// tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_HEIGHT_FUSION, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, DjiTest_FcSubscriptionReceiveHeightCallback);
//if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
//{
tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_HEIGHT_FUSION, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, DjiTest_FcSubscriptionReceiveHeightCallback);
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
// qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_HEIGHT_FUSION error.";
// return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
//}
qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_HEIGHT_FUSION error.";
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}
tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY, DJI_DATA_SUBSCRIPTION_TOPIC_5_HZ, NULL);
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
@ -492,7 +531,7 @@ int VehicleController::SetupWaypointStatusCallback()
}
returnCode = DjiWaypointV2_RegisterMissionEventCallback(ZZ_DjiWaypointV2EventCallback);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
qDebug() <<"Register waypoint V2 event failed, error code:" << returnCode;
return returnCode;
@ -508,7 +547,7 @@ int VehicleController::SetupWaypointStatusCallback()
int VehicleController::LoadUserAppInfo(T_DjiUserInfo* struDjiUserInfo)
{
//using namespace ZZ::Device::DJI::M300RTK;
using namespace ZZ::Device::DJI::M300RTK;
memset(struDjiUserInfo->appName, 0, sizeof(struDjiUserInfo->appName));
memset(struDjiUserInfo->appId, 0, sizeof(struDjiUserInfo->appId));
@ -517,38 +556,22 @@ int VehicleController::LoadUserAppInfo(T_DjiUserInfo* struDjiUserInfo)
memset(struDjiUserInfo->developerAccount, 0, sizeof(struDjiUserInfo->developerAccount));
memset(struDjiUserInfo->baudRate, 0, sizeof(struDjiUserInfo->baudRate));
//struDjiUserInfo->appName = "Project_Fiora";
//struDjiUserInfo->appId = "151266";
//struDjiUserInfo->appKey = "d8bc685458ca5dca9280abcdc992d93";
//struDjiUserInfo->appLicense = "lXcHSZDwjws7SQnm3TjUINbKIyCC7438vJqiRBBhGe/ckOg3WmhhbYJC5aS/uKSRRMnB2UrqmiE6z+Ta+fBbUOmQUObSQJFrn/VHAsgGKR9nOMdrRNDDSwBbfLcheDIbJwjVmBAgk8RPcTZPH3WNtCohlRcpxuLIrG7oUWaawbLDwwwJ11hx5RqKKHn31I/CXTrbYQbJOm2Da32l/U7/BPF22eBUIJKts7aNfBBbFESNJI3p/xosmxfALkuXYKXMGBiIHk9bu+u0dT53Ddhz++tHh/oFyNQuyhCH4EtecvZu0PR3aMsmxyXbvXhAIocB5+AKZN+kykxhF8ToQS61bQ==";
//struDjiUserInfo->developerAccount = "1033584732@qq.com";
//struDjiUserInfo->baudRate = "460800";
//strncpy(struDjiUserInfo->appName, "Project_Fiora", sizeof(struDjiUserInfo->appName));
//strncpy(struDjiUserInfo->appId, "151266", sizeof(struDjiUserInfo->appId));
//strncpy(struDjiUserInfo->appKey, "d8bc685458ca5dca9280abcdc992d93", sizeof(struDjiUserInfo->appKey));
//strncpy(struDjiUserInfo->appLicense, "lXcHSZDwjws7SQnm3TjUINbKIyCC7438vJqiRBBhGe/ckOg3WmhhbYJC5aS/uKSRRMnB2UrqmiE6z+Ta+fBbUOmQUObSQJFrn/VHAsgGKR9nOMdrRNDDSwBbfLcheDIbJwjVmBAgk8RPcTZPH3WNtCohlRcpxuLIrG7oUWaawbLDwwwJ11hx5RqKKHn31I/CXTrbYQbJOm2Da32l/U7/BPF22eBUIJKts7aNfBBbFESNJI3p/xosmxfALkuXYKXMGBiIHk9bu+u0dT53Ddhz++tHh/oFyNQuyhCH4EtecvZu0PR3aMsmxyXbvXhAIocB5+AKZN+kykxhF8ToQS61bQ==", sizeof(struDjiUserInfo->appLicense));
//strncpy(struDjiUserInfo->developerAccount, "1033584732@qq.com", sizeof(struDjiUserInfo->developerAccount));
//strncpy(struDjiUserInfo->baudRate, "921600", sizeof(struDjiUserInfo->baudRate));
//if (m_struAppRegInfo.qstrUserAppName.length() >= sizeof(struDjiUserInfo->appName) ||
// m_struAppRegInfo.qstrUserAppID.length() > sizeof(struDjiUserInfo->appId) ||
// m_struAppRegInfo.qstrUserAppKey.length() > sizeof(struDjiUserInfo->appKey) ||
// m_struAppRegInfo.qstrUserAppLic.length() > sizeof(struDjiUserInfo->appLicense) ||
// m_struAppRegInfo.qstrUserAppAcc.length() >= sizeof(struDjiUserInfo->developerAccount) ||
// m_struHardwareInfo.qstrBaudRate.length() > sizeof(struDjiUserInfo->baudRate))
// {
// qDebug()<<"VehicleController:LoadUserAppInfo.Length of user information string is beyond limit,Please check";
// return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER;
//}
if (m_struAppRegInfo.qstrUserAppName.length() >= sizeof(struDjiUserInfo->appName) ||
m_struAppRegInfo.qstrUserAppID.length() > sizeof(struDjiUserInfo->appId) ||
m_struAppRegInfo.qstrUserAppKey.length() > sizeof(struDjiUserInfo->appKey) ||
m_struAppRegInfo.qstrUserAppLic.length() > sizeof(struDjiUserInfo->appLicense) ||
m_struAppRegInfo.qstrUserAppAcc.length() >= sizeof(struDjiUserInfo->developerAccount) ||
m_struHardwareInfo.qstrBaudRate.length() > sizeof(struDjiUserInfo->baudRate))
{
qDebug()<<"VehicleController:LoadUserAppInfo.Length of user information string is beyond limit,Please check";
return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER;
}
strncpy(struDjiUserInfo->appName, m_struAppRegInfo.qstrUserAppName.toLatin1().data(), m_struAppRegInfo.qstrUserAppName.length());
memcpy(struDjiUserInfo->appId, m_struAppRegInfo.qstrUserAppID.toLatin1().data(), ZZ_MIN(sizeof(struDjiUserInfo->appId), m_struAppRegInfo.qstrUserAppID.length()));
memcpy(struDjiUserInfo->appKey, m_struAppRegInfo.qstrUserAppKey.toLatin1().data(), ZZ_MIN(sizeof(struDjiUserInfo->appKey), m_struAppRegInfo.qstrUserAppKey.length()));
memcpy(struDjiUserInfo->appLicense, m_struAppRegInfo.qstrUserAppLic.toLatin1().data(),ZZ_MIN(sizeof(struDjiUserInfo->appLicense), m_struAppRegInfo.qstrUserAppLic.length()));
memcpy(struDjiUserInfo->appLicense, m_struAppRegInfo.qstrUserAppLic.toLatin1().data(),
ZZ_MIN(sizeof(struDjiUserInfo->appLicense), m_struAppRegInfo.qstrUserAppLic.length()));
memcpy(struDjiUserInfo->baudRate, m_struHardwareInfo.qstrBaudRate.toLatin1().data(), ZZ_MIN(sizeof(struDjiUserInfo->baudRate), m_struHardwareInfo.qstrBaudRate.length()));
strncpy(struDjiUserInfo->developerAccount, m_struAppRegInfo.qstrUserAppAcc.toLatin1().data(), sizeof(struDjiUserInfo->developerAccount) - 1);
@ -564,22 +587,25 @@ T_DjiReturnCode VehicleController::DjiUser_PrintMessage(const uint8_t* data, uin
int VehicleController::SetupMessagePipe()
{
// connect(&m_clsWidget, &ZZ_Widget_M300RTK::Signal_UpdateCaptureMode, this, &VehicleController::Slot_OnChangeCaptureMode);
connect(&m_clsWidget, &ZZ_Widget_M300RTK::Signal_UpdateCaptureMode, this, &VehicleController::Slot_OnChangeCaptureMode);
connect(&m_clsWidget, &ZZ_Widget_M300RTK::Signal_StartCapture, this, &VehicleController::Signal_StartCapture);
connect(&m_clsWidget, &ZZ_Widget_M300RTK::Signal_StopCapture, this, &VehicleController::Signal_StopCapture);
connect(&m_clsWidget, &ZZ_Widget_M300RTK::Signal_StopCapture, this, &VehicleController::Signal_StopCapture);
connect(this, &VehicleController::Signal_UpdateVehicleMessage, &m_clsWidget, &ZZ_Widget_M300RTK::Slot_UpdatePSDKFloatMessage);
// /// for test
//#ifdef ZZ_FLAG_TEST
// connect(this, &VehicleController::Signal_StartCapture, this, &VehicleController::Slot_TestStartCapture);
// connect(this, &VehicleController::Signal_StopCapture, this, &VehicleController::Slot_TestStopCapture);
//#endif
/// for test
#ifdef ZZ_FLAG_TEST
connect(this, &VehicleController::Signal_StartCapture, this, &VehicleController::Slot_TestStartCapture);
connect(this, &VehicleController::Signal_StopCapture, this, &VehicleController::Slot_TestStopCapture);
#endif
return 0;
}
int VehicleController::SetupWidget()
{
m_clsWidget.SetUIFilePath(const_cast<char*>("/home/DJI/Widget"),100);
// m_clsWidget.SetSettings(m_struUIConfig);
m_clsWidget.SetUIFilePath("/home/DJI/Widget",100);
// m_clsWidget.SetUIFilePath("/home/pi/airborn/configfile/DJI/Widget",100);
m_clsWidget.SetSettings(m_struUIConfig);
m_clsWidget.PreparteEnvironment();
return 0;