#include "VehicleController.h" #include #include #include #include #include #include "osal.h" #include "osal_fs.h" #include "osal_socket.h" #include "hal_usb_bulk.h" #include "hal_uart.h" #include "hal_network.h" #include "time.h" #include "ZZ_Types_M300.h" #define ZZ_UNUSED(x) ((x) = (x)) #define ZZ_MIN(a, b) (((a) < (b)) ? (a) : (b)) #define ZZ_MAX(a, b) (((a) > (b)) ? (a) : (b)) int VehicleController::m_siFlagIsPumpWorking = 0; int VehicleController::m_siFlagIsStartCaptureSignalEmitted = 0; VehicleController* VehicleController::spCaller = NULL; char VehicleController::m_scCaptureMode = 0; VehicleController::VehicleController(QObject* parent /*= nullptr*/) { m_iFlagIsVehicleTakeoff = 0; m_iFlagIsVehicleCapturing = 0; m_iFlagIsCaptureModeInited = 0; //m_cCaptureMode = 0; } VehicleController::~VehicleController() { } T_DjiReturnCode VehicleController::DjiTest_FcSubscriptionReceiveFlightStatusCallback(const uint8_t* ccData, uint16_t sDataSize, const T_DjiDataTimestamp* tDjiTimestamp) { return 0; } T_DjiReturnCode VehicleController::DjiTest_FcSubscriptionReceiveHeightCallback(const uint8_t* data, uint16_t dataSize, const T_DjiDataTimestamp* timestamp) { float fHeight = *((T_DjiFcSubscriptionHeightFusion*)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); return 0; } T_DjiReturnCode VehicleController::ZZ_DjiWaypointV2EventCallback(T_DjiWaypointV2MissionEventPush eventData) { if (spCaller==NULL) { qDebug() << "ZZ_DjiWaypointV2EventCallback Fatal Error.spCaller Undefined"; return -1; } 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.event == 0x11) { qDebug() << "eventData.event" << m_siFlagIsStartCaptureSignalEmitted; if (m_siFlagIsStartCaptureSignalEmitted > 0) { m_siFlagIsStartCaptureSignalEmitted = 0; spCaller->emit Signal_StopCapture(); } } return 0; } int VehicleController::Initialize() { return 0; } int VehicleController::SetupEnvironment_M300RTK() { using namespace ZZ::Device::DJI::M300RTK; 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); T_DjiReturnCode returnCode; T_DjiOsalHandler osalHandler; T_DjiHalUartHandler uartHandler; T_DjiHalUsbBulkHandler usbBulkHandler; T_DjiLoggerConsole printConsole; T_DjiLoggerConsole localRecordConsole; T_DjiFileSystemHandler fileSystemHandler; T_DjiSocketHandler socketHandler; T_DjiHalNetworkHandler networkHandler; 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; 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; uartHandler.UartInit = HalUart_Init; uartHandler.UartDeInit = HalUart_DeInit; uartHandler.UartWriteData = HalUart_WriteData; uartHandler.UartReadData = HalUart_ReadData; uartHandler.UartGetStatus = HalUart_GetStatus; 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; returnCode = DjiPlatform_RegOsalHandler(&osalHandler); if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "VehicleController: Func DjiPlatform_RegOsalHandler. Register osal handler error"; throw std::runtime_error("Register osal handler error."); } returnCode = DjiPlatform_RegHalUartHandler(&uartHandler); if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "VehicleController: Func DjiPlatform_RegHalUartHandler. Register hal uart handler error"; throw std::runtime_error("Register hal uart handler error."); } if (m_struHardwareInfo.enumHCM== HardwareConnectionMode::UartAndNetwork) { 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"); } returnCode = DjiPlatform_RegFileSystemHandler(&fileSystemHandler); if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "VehicleController: Func DjiPlatform_RegFileSystemHandler. Register osal filesystem handler error"; throw std::runtime_error("Register osal filesystem handler error."); } returnCode = DjiLogger_AddConsole(&printConsole); if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "VehicleController: Func DjiPlatform_RegFileSystemHandler. Register osal filesystem handler error"; throw std::runtime_error("Add printf console error."); } // if (DjiUser_LocalWriteFsInit(DJI_LOG_PATH) != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { // throw std::runtime_error("File system init error."); // } // // returnCode = DjiLogger_AddConsole(&printConsole); // if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { // throw std::runtime_error("Add printf console error."); // } // // returnCode = DjiLogger_AddConsole(&localRecordConsole); // if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { // throw std::runtime_error("Add printf console error."); // } return 0; } int VehicleController::StartupPSDK_M300RTK() { T_DjiUserInfo userInfo; T_DjiReturnCode returnCode; T_DjiAircraftInfoBaseInfo aircraftInfoBaseInfo; T_DjiOsalHandler* osalHandler = DjiPlatform_GetOsalHandler(); returnCode = LoadUserAppInfo(&userInfo); if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { throw std::runtime_error("Fill user info error, please check user info config."); } returnCode = DjiCore_Init(&userInfo); 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) { throw std::runtime_error("Get aircraft base info error."); } // if (aircraftInfoBaseInfo.mountPosition != DJI_MOUNT_POSITION_EXTENSION_PORT) // { // throw std::runtime_error("Please run this sample on extension port."); // } returnCode = DjiCore_SetAlias("PSDK_APPALIAS"); 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) { throw std::runtime_error("Start sdk application error."); } //osalHandler->TaskSleepMs(5000); SetupWidget(); //osalHandler->TaskSleepMs(5000); SetupMessagePipe(); SetupWaypointStatusCallback(); InitSystemDateTime(); SetupSubscriptions(); qDebug()<<"M300RTK PSDK Channel Started."; return 0; } int VehicleController::UpdateUIConfig() { m_clsWidget.GetSettings(m_struUIConfig); m_clsConfigParser.UpdateUIConfig(m_struUIConfig); return 0; } int VehicleController::GetOneDataFrame(M300RTKDataFrame& M300RTKDataFrame) { T_DjiReturnCode tDjiReturnCode; T_DjiFcSubscriptionVelocity tDjiVelocity = { 0 }; T_DjiFcSubscriptionQuaternion tDjiQuaternion = { 0 }; T_DjiFcSubscriptionGpsPosition tDjiGpsPosition = { 0 }; T_DjiDataTimestamp tDjiTimestamp = { 0 }; tDjiReturnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY,(uint8_t*)&tDjiVelocity,sizeof(T_DjiFcSubscriptionVelocity),&tDjiTimestamp); if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "DjiFcSubscription_GetLatestValueOfTopic error.DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY"; //return 1; } tDjiReturnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION, (uint8_t*)&tDjiGpsPosition, sizeof(T_DjiFcSubscriptionGpsPosition), &tDjiTimestamp); if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "DjiFcSubscription_GetLatestValueOfTopic error.DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY"; //return 2; } tDjiReturnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_QUATERNION, (uint8_t*)&tDjiQuaternion, sizeof(T_DjiFcSubscriptionQuaternion), &tDjiTimestamp); if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "DjiFcSubscription_GetLatestValueOfTopic error.DJI_FC_SUBSCRIPTION_TOPIC_QUATERNION"; //return 3; } M300RTKDataFrame.stVelocity.x = tDjiVelocity.data.x; M300RTKDataFrame.stVelocity.y = tDjiVelocity.data.y; M300RTKDataFrame.stVelocity.z = tDjiVelocity.data.z; M300RTKDataFrame.stQuaternion.w_q0 = tDjiQuaternion.q0; M300RTKDataFrame.stQuaternion.x_q1 = tDjiQuaternion.q1; M300RTKDataFrame.stQuaternion.y_q2 = tDjiQuaternion.q2; M300RTKDataFrame.stQuaternion.z_q3 = tDjiQuaternion.q3; M300RTKDataFrame.stGPSPosition.x = tDjiGpsPosition.x; M300RTKDataFrame.stGPSPosition.y = tDjiGpsPosition.y; M300RTKDataFrame.stGPSPosition.z = tDjiGpsPosition.z; #ifdef ZZ_FLAG_TEST qDebug() << tDjiVelocity.data.x << tDjiVelocity.data.y << tDjiVelocity.data.z << tDjiGpsPosition.x << tDjiGpsPosition.y << tDjiGpsPosition.z; #endif return 0; } 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_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; } tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY, DJI_DATA_SUBSCRIPTION_TOPIC_5_HZ, NULL); if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY error."; return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN; } tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION, DJI_DATA_SUBSCRIPTION_TOPIC_5_HZ, NULL); if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION error."; return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN; } tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_QUATERNION, DJI_DATA_SUBSCRIPTION_TOPIC_5_HZ, NULL); if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_QUATERNION error."; return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN; } // tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_BAROMETER, DJI_DATA_SUBSCRIPTION_TOPIC_5_HZ, NULL); // if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) // { // // qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_BAROMETER error."; // return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN; // } return 0; } int VehicleController::SetupWaypointStatusCallback() { T_DjiReturnCode returnCode; returnCode = DjiWaypointV2_Init(); if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug()<<"Init waypoint V2 module error, error code:"<< returnCode; return returnCode; } returnCode = DjiWaypointV2_RegisterMissionEventCallback(ZZ_DjiWaypointV2EventCallback); if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() <<"Register waypoint V2 event failed, error code:" << returnCode; return returnCode; } return 0; } // int VehicleController::TransSignal_StartCapture(VehicleController* pCaller) // { // pCaller->emit Signal_StartCapture(); // return 0; // } int VehicleController::LoadUserAppInfo(T_DjiUserInfo* struDjiUserInfo) { using namespace ZZ::Device::DJI::M300RTK; memset(struDjiUserInfo->appName, 0, sizeof(struDjiUserInfo->appName)); memset(struDjiUserInfo->appId, 0, sizeof(struDjiUserInfo->appId)); memset(struDjiUserInfo->appKey, 0, sizeof(struDjiUserInfo->appKey)); memset(struDjiUserInfo->appLicense, 0, sizeof(struDjiUserInfo->appLicense)); memset(struDjiUserInfo->developerAccount, 0, sizeof(struDjiUserInfo->developerAccount)); memset(struDjiUserInfo->baudRate, 0, 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; } 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->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); return 0; } T_DjiReturnCode VehicleController::DjiUser_PrintMessage(const uint8_t* data, uint16_t dataLen) { QString qstrMessage((char*)data); qDebug() << qstrMessage; return 0; } int VehicleController::SetupMessagePipe() { 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(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 return 0; } int VehicleController::SetupWidget() { m_clsWidget.SetUIFilePath("/home/DJI/Widget",100); m_clsWidget.SetSettings(m_struUIConfig); m_clsWidget.PreparteEnvironment(); return 0; } int VehicleController::InitSystemDateTime() { T_DjiReturnCode tDjiReturnCode; ///Init tDjiReturnCode = DjiFcSubscription_Init(); if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "init data subscription module error."; return 1; } else { qDebug() << "InitSystemDateTime finished."; } ///SubscribeTopic tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_SIGNAL_LEVEL, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, NULL); if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_GPS_SIGNAL_LEVEL error."; return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN; } tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, NULL); if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE error."; return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN; } tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_TIME, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, NULL); if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_GPS_TIME error."; return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN; } ///Make sure Gps avalible T_DjiOsalHandler* osalHandler = DjiPlatform_GetOsalHandler(); bool bStop = false; /////for test #ifdef ZZ_FLAG_TEST bStop = 1; #endif T_DjiFcSubscriptionGpsSignalLevel tDjiGpsSignalLevel = { 0 }; T_DjiDataTimestamp tTimestamp = { 0 }; osalHandler->TaskSleepMs(5000); QTime qtStart = QTime::currentTime(); int iRetryTime = 0; while (!bStop) { if (iRetryTime>10) { bStop = 1; } osalHandler->TaskSleepMs(2000); DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_SIGNAL_LEVEL, (uint8_t*)&tDjiGpsSignalLevel, sizeof(T_DjiFcSubscriptionGpsSignalLevel), &tTimestamp); qDebug() << "DJI_FC_SUBSCRIPTION_TOPIC_GPS_SIGNAL_LEVEL" << tDjiGpsSignalLevel; QTime qtElapsed = QTime::currentTime(); if (tDjiGpsSignalLevel>1) { bStop = 1; //emit Signal_UpdatePSDKFloatMessage(""); } else { QString qstrInfo = QString("Warning:Weak GPS signal, Waiting....GPS_SIGNAL_LEVEL:%1, Elapsed time:%2").arg(tDjiGpsSignalLevel).arg(qtStart.msecsTo(qtElapsed)); emit Signal_UpdateVehicleMessage(qstrInfo); } iRetryTime++; } ///Get GPS Datetime T_DjiFcSubscriptionGpsDate tGpsDate = { 0 }; T_DjiFcSubscriptionGpsTime tGpsTime = { 0 }; tDjiReturnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE, (uint8_t*)&tGpsDate, sizeof(T_DjiFcSubscriptionGpsDate), &tTimestamp); if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "DjiFcSubscription_GetLatestValueOfTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE."; return -1; } tDjiReturnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_TIME, (uint8_t*)&tGpsTime, sizeof(T_DjiFcSubscriptionGpsTime), &tTimestamp); if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "DjiFcSubscription_GetLatestValueOfTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE."; return -2; } //qDebug() << "Date" << tGpsDate << "Time" << tGpsTime; QDateTime qdtDateTime; QString qstrDate, qstrTime,qstrDateTime; qstrDate = QString::number(tGpsDate); qstrTime = QString::number(tGpsTime); if (qstrTime.length()==5) { qstrTime = "0" + qstrTime/*+"000"*/; } // else // { // qstrTime= qstrTime + "000"; // } // QDate qd; // qd = QDate::fromString("20121022", "yyyyMMdd"); // QTime qt; // qt.fromString(qstrTime,"hhmmsszzz"); qstrDateTime = qstrDate + qstrTime; qdtDateTime = QDateTime::fromString(qstrDateTime, "yyyyMMddhhmmss"); time_t stdTime = qdtDateTime.toTime_t(); if (qdtDateTime.isValid()) { stime(&stdTime); } #ifndef ZZ_FLAG_TEST //stime(&stdTime); #else #endif // qDebug() << qstrDate; // qDebug() << qstrTime; // qDebug() << qd.isValid(); // qDebug() << qd.toString("yyyyMMdd"); // qDebug() << qt.isValid(); // qDebug() << qt; qDebug() <<"[GPS Time Check]" << qdtDateTime.isValid(); qDebug() <<"[GPS Time:]" << qstrDateTime << "####" << qdtDateTime.toString("yyyyMMddhhmmsszzz"); ///UnSubscribeTopic tDjiReturnCode = DjiFcSubscription_UnSubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_SIGNAL_LEVEL); if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "DjiFcSubscription_UnSubscribeTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_GPS_SIGNAL_LEVEL."; return -11; } tDjiReturnCode = DjiFcSubscription_UnSubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE); if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "DjiFcSubscription_UnSubscribeTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE."; return -12; } tDjiReturnCode = DjiFcSubscription_UnSubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_TIME); if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { qDebug() << "DjiFcSubscription_UnSubscribeTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_GPS_TIME."; return -13; } return 0; } int VehicleController::Slot_OnChangeCaptureMode(char cMode) { //ZZ_Widget_M300RTK* test = qobject_cast(sender()); //if (test== &m_clsWidget) //{ // qDebug() << "from ZZ_Widget_M300RTK"; //} ///0:Auto 1:Manual switch (cMode) { case 0: //m_clsWidget.SetUIFilePath("/home/DJI/Widget_Auto/", 100); //m_clsWidget.PreparteEnvironment(); //m_clsWidget.UpdateCaptureStatus(0); m_scCaptureMode = 0; break; case 1: //m_clsWidget.SetUIFilePath("/home/DJI/Widget_Manual/", 100); //m_clsWidget.PreparteEnvironment(); //m_clsWidget.UpdateCaptureStatus(1); m_scCaptureMode = 1; break; default: break; } return 0; } int VehicleController::Slot_TestStartCapture() { qDebug() << "VehicleController Test Cap Started"; return 0; } int VehicleController::Slot_TestStopCapture() { qDebug() << "VehicleController Test Cap Stopped"; return 0; }