870 lines
32 KiB
C++
870 lines
32 KiB
C++
#include "VehicleController.h"
|
|
|
|
#include <dji_platform.h>
|
|
#include <dji_logger.h>
|
|
#include <dji_core.h>
|
|
#include <dji_aircraft_info.h>
|
|
#include <dji_fc_subscription.h>
|
|
#include <csignal>
|
|
#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"
|
|
|
|
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))
|
|
|
|
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_fHeightOfHomePoint_BM = -1;
|
|
//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_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);
|
|
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()
|
|
{
|
|
//m_clsWidget.GetSettings(m_struUIConfig);
|
|
m_clsConfigParser.GetParams(m_struAppRegInfo, m_struHardwareInfo, m_struSensorPort);
|
|
|
|
//m_clsConfigParser.UpdateUIConfig(m_struUIConfig);
|
|
return 0;
|
|
}
|
|
|
|
int VehicleController::SetupEnvironment()
|
|
{
|
|
// 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 = { 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 };
|
|
|
|
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;
|
|
osalHandler.GetRandomNum = Osal_GetRandomNum;
|
|
|
|
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::UartAndUSBBulk*/1)
|
|
{
|
|
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.");
|
|
}
|
|
}
|
|
|
|
//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.");
|
|
}
|
|
|
|
|
|
return 0;
|
|
}
|
|
|
|
int VehicleController::StartupPSDK_M350RTK()
|
|
{
|
|
T_DjiUserInfo userInfo;
|
|
T_DjiReturnCode returnCode;
|
|
T_DjiAircraftInfoBaseInfo aircraftInfoBaseInfo;
|
|
T_DjiOsalHandler* osalHandler = DjiPlatform_GetOsalHandler();
|
|
|
|
//signal(SIGTERM, DjiUser_NormalExitHandler);
|
|
|
|
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();
|
|
//InitSystemParams();
|
|
SetupSubscriptions();
|
|
|
|
|
|
qDebug()<<"M350RTK PSDK Channel Started.";
|
|
return 0;
|
|
}
|
|
|
|
int VehicleController::UpdateUIConfig()
|
|
{
|
|
//m_clsWidget.GetSettings(m_struUIConfig);
|
|
//m_clsConfigParser.UpdateUIConfig(m_struUIConfig);
|
|
return 0;
|
|
}
|
|
|
|
int VehicleController::GetOneDataFrame(M350RTKDataFrame& M350RTKDataFrame)
|
|
{
|
|
T_DjiReturnCode tDjiReturnCode;
|
|
T_DjiFcSubscriptionVelocity tDjiVelocity = { 0 };
|
|
T_DjiFcSubscriptionQuaternion tDjiQuaternion = { 0 };
|
|
T_DjiFcSubscriptionGpsPosition tDjiGpsPosition = { 0 };
|
|
T_DjiDataTimestamp tDjiTimestamp = { 0 };
|
|
T_DjiFcSubscriptionAltitudeFused tDjiAltFused = { 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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
tDjiReturnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_BAROMETER, (uint8_t*)&tDjiAltFused, sizeof(T_DjiFcSubscriptionAltitudeFused), &tDjiTimestamp);
|
|
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
|
|
{
|
|
qDebug() << "DjiFcSubscription_GetLatestValueOfTopic error.DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_BAROMETER";
|
|
//return 4;
|
|
}
|
|
|
|
M350RTKDataFrame.stVelocity.x = tDjiVelocity.data.x;
|
|
M350RTKDataFrame.stVelocity.y = tDjiVelocity.data.y;
|
|
M350RTKDataFrame.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;
|
|
|
|
M350RTKDataFrame.stGPSPosition.x = tDjiGpsPosition.x;
|
|
M350RTKDataFrame.stGPSPosition.y = tDjiGpsPosition.y;
|
|
M350RTKDataFrame.stGPSPosition.z = tDjiGpsPosition.z;
|
|
|
|
M350RTKDataFrame.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;
|
|
|
|
M350RTKDataFrame.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,
|
|
-2 * tDjiQuaternion.q2 * tDjiQuaternion.q2 - 2 * tDjiQuaternion.q3 * tDjiQuaternion.q3 + 1) * 57.3;
|
|
//////////////////////////////////////////////////////////////////////////
|
|
|
|
|
|
#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_1_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));
|
|
|
|
//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;
|
|
//}
|
|
|
|
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_Nikira::Signal_StartCapture, this, &VehicleController::Signal_StartCapture);
|
|
connect(&m_clsWidget, &ZZ_Widget_Nikira::Signal_StopCapture, this, &VehicleController::Signal_StopCapture);
|
|
connect(this, &VehicleController::Signal_UpdateVehicleMessage, &m_clsWidget, &ZZ_Widget_Nikira::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(const_cast<char*>("/home/DJI/Widget"),100);
|
|
// m_clsWidget.SetSettings(m_struUIConfig);
|
|
m_clsWidget.PreparteEnvironment();
|
|
|
|
return 0;
|
|
}
|
|
|
|
int VehicleController::InitSystemParams()
|
|
{
|
|
T_DjiReturnCode tDjiReturnCode;
|
|
QString qstrWBackPath("/home/data/Settings/MainSettings.ini");
|
|
|
|
|
|
///Init
|
|
tDjiReturnCode = DjiFcSubscription_Init();
|
|
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
|
|
{
|
|
qDebug() << "init data subscription module error.";
|
|
return 1;
|
|
}
|
|
else
|
|
{
|
|
qDebug() << "InitSystemDateTime started.";
|
|
}
|
|
|
|
///SubscribeTopic GPS_SIGNAL_LEVEL
|
|
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 -1;
|
|
}
|
|
|
|
///SubscribeTopic GPS_DATE
|
|
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 -2;
|
|
}
|
|
|
|
///SubscribeTopic GPS_TIME
|
|
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 -3;
|
|
}
|
|
|
|
///SubscribeTopic ALTITUDE_OF_HOMEPOINT
|
|
tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, NULL);
|
|
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
|
|
{
|
|
|
|
qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT error.";
|
|
return -4;
|
|
}
|
|
|
|
///SubscribeTopic DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION
|
|
tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, NULL);
|
|
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
|
|
{
|
|
qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION error.";
|
|
return -5;
|
|
}
|
|
|
|
///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>60)
|
|
{
|
|
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++;
|
|
}
|
|
//osalHandler->TaskSleepMs(2000);
|
|
|
|
///Get GPS Datetime
|
|
T_DjiFcSubscriptionGpsDate tGpsDate = { 0 };
|
|
T_DjiFcSubscriptionGpsTime tGpsTime = { 0 };
|
|
///Get Other
|
|
T_DjiFcSubscriptionAltitudeOfHomePoint tAltOfHP = { 0 };
|
|
T_DjiFcSubscriptionGpsPosition tDjiGpsPosition = { 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;
|
|
}
|
|
|
|
tDjiReturnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT, (uint8_t*)&tAltOfHP, sizeof(T_DjiFcSubscriptionAltitudeOfHomePoint), &tTimestamp);
|
|
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
|
|
{
|
|
qDebug() << "DjiFcSubscription_GetLatestValueOfTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT.";
|
|
return -3;
|
|
}
|
|
|
|
tDjiReturnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION, (uint8_t*)&tDjiGpsPosition, sizeof(T_DjiFcSubscriptionGpsPosition), &tTimestamp);
|
|
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
|
|
{
|
|
qDebug() << "DjiFcSubscription_GetLatestValueOfTopic error.DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY";
|
|
return -4;
|
|
}
|
|
|
|
m_fHeightOfHomePoint_BM = tAltOfHP;
|
|
float fHeightOfHomePoint_GPS= tDjiGpsPosition.z;
|
|
QString qstrHeightOfHomePoint_BM = QString("%1").arg(m_fHeightOfHomePoint_BM);
|
|
QString qstrHeightOfHomePoint_GPS = QString("%1").arg(tDjiGpsPosition.z);
|
|
//////////////////////////////////////////////////////////////////////////write back AOHP
|
|
QSettings qsMainSettings(qstrWBackPath, QSettings::Format::IniFormat);
|
|
qsMainSettings.setValue(QString("WBACK/HeightOfHomePoint_BM"), qstrHeightOfHomePoint_BM);
|
|
qsMainSettings.setValue(QString("WBACK/HeightOfHomePoint_GPS"), qstrHeightOfHomePoint_GPS);
|
|
qDebug() << qstrHeightOfHomePoint_BM << m_fHeightOfHomePoint_BM;
|
|
qDebug() << qstrHeightOfHomePoint_GPS << tDjiGpsPosition.z;
|
|
//////////////////////////////////////////////////////////////////////////
|
|
|
|
//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_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;
|
|
}
|
|
|
|
tDjiReturnCode = DjiFcSubscription_UnSubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT);
|
|
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
|
|
{
|
|
qDebug() << "DjiFcSubscription_UnSubscribeTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT.";
|
|
return -14;
|
|
}
|
|
|
|
tDjiReturnCode = DjiFcSubscription_UnSubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION);
|
|
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
|
|
{
|
|
qDebug() << "DjiFcSubscription_UnSubscribeTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT.";
|
|
return -15;
|
|
}
|
|
|
|
|
|
|
|
return 0;
|
|
}
|
|
|
|
int VehicleController::Slot_OnChangeCaptureMode(char cMode)
|
|
{
|
|
//ZZ_Widget_M300RTK* test = qobject_cast<ZZ_Widget_M300RTK*>(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;
|
|
}
|
|
|