zhangzhuo/M300CO2
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

1.添加了EGM96-5的数据集和算法支持，可以正确求解GEOID Offset高度。

Browse Source 
2.在同步时间时现在会将起飞点的海拔高度写回到配置文件/home/data/Settings/MainSettings.ini键值为WBACK/HeightOfHomePoint
3.现在的高度回调函数以及获取函数调整为了DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_FUSED。
This commit is contained in:
zhangzhuo
2023-04-07 15:46:09 +08:00
parent a714945d2d
commit 09a5436ad7
9 changed files with 65752 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -31,6 +31,7 @@ VehicleController::VehicleController(QObject* parent /*= nullptr*/)
m_iFlagIsVehicleCapturing = 0;
m_iFlagIsCaptureModeInited = 0;
m_fHeightOfHomePoint = -1;
//m_cCaptureMode = 0;
}
@ -46,11 +47,11 @@ T_DjiReturnCode VehicleController::DjiTest_FcSubscriptionReceiveFlightStatusCall
T_DjiReturnCode VehicleController::DjiTest_FcSubscriptionReceiveHeightCallback(const uint8_t* data, uint16_t dataSize, const T_DjiDataTimestamp* timestamp)
{
float fHeight = *((T_DjiFcSubscriptionHeightFusion*)data);
float fHeight = *((T_DjiFcSubscriptionAltitudeFused*)data);
//qDebug() <<"[Fused Height]:"<< fHeight;
///Pump need to be added
if (m_siFlagIsPumpWorking==0&& fHeight>10)
if (m_siFlagIsPumpWorking==0&& fHeight > 10)
{
//system("sudo gpio mode 7 out");
m_siFlagIsPumpWorking = 1;
@ -319,7 +320,7 @@ int VehicleController::StartupPSDK_M300RTK()
//osalHandler->TaskSleepMs(5000);
SetupMessagePipe();
SetupWaypointStatusCallback();
InitSystemDateTime();
InitSystemParams();
SetupSubscriptions();
@ -398,11 +399,11 @@ int VehicleController::SetupSubscriptions()
// qDebug() << "init data subscription module finished.";
// }
tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_HEIGHT_FUSION, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, DjiTest_FcSubscriptionReceiveHeightCallback);
tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_FUSED, 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.";
qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_FUSED error.";
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}
@ -531,9 +532,11 @@ int VehicleController::SetupWidget()
return 0;
}
int VehicleController::InitSystemDateTime()
int VehicleController::InitSystemParams()
{
T_DjiReturnCode tDjiReturnCode;
QString qstrWBackPath("/home/data/Settings/MainSettings.ini");
///Init
tDjiReturnCode = DjiFcSubscription_Init();
@ -547,7 +550,7 @@ int VehicleController::InitSystemDateTime()
qDebug() << "InitSystemDateTime finished.";
}
///SubscribeTopic
///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)
{
@ -556,6 +559,7 @@ int VehicleController::InitSystemDateTime()
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}
///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)
{
@ -564,6 +568,7 @@ int VehicleController::InitSystemDateTime()
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}
///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)
{
@ -572,6 +577,15 @@ int VehicleController::InitSystemDateTime()
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}
///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 DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}
///Make sure Gps avalible
T_DjiOsalHandler* osalHandler = DjiPlatform_GetOsalHandler();
bool bStop = false;
@ -617,6 +631,8 @@ int VehicleController::InitSystemDateTime()
///Get GPS Datetime
T_DjiFcSubscriptionGpsDate tGpsDate = { 0 };
T_DjiFcSubscriptionGpsTime tGpsTime = { 0 };
T_DjiFcSubscriptionAltitudeOfHomePoint tAltOfHP = { 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)
{
@ -632,6 +648,18 @@ int VehicleController::InitSystemDateTime()
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;
}
m_fHeightOfHomePoint = tAltOfHP;
//////////////////////////////////////////////////////////////////////////write back AOHP
QSettings qsMainSettings(qstrWBackPath, QSettings::Format::IniFormat);
qsMainSettings.setValue(QString("WBACK/HeightOfHomePoint"), m_fHeightOfHomePoint);
//////////////////////////////////////////////////////////////////////////
//qDebug() << "Date" << tGpsDate << "Time" << tGpsTime;
QDateTime qdtDateTime;
@ -695,6 +723,13 @@ int VehicleController::InitSystemDateTime()
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;
}
return 0;
}

5
Source/M300/PSDK_Qt/Main/VehicleController.h
View File

@ -41,7 +41,8 @@ private:
int m_iFlagIsVehicleTakeoff;
int m_iFlagIsVehicleCapturing;
int m_iFlagIsCaptureModeInited;
float m_fHeightOfHomePoint;
public:
/// test func Do Not Call
static T_DjiReturnCode DjiTest_FcSubscriptionReceiveFlightStatusCallback(const uint8_t* ccData, uint16_t sDataSize,const T_DjiDataTimestamp* tDjiTimestamp);
@ -68,7 +69,7 @@ private:
int SetupWidget();
///
public:///for test
int InitSystemDateTime();
int InitSystemParams();
int SetupSubscriptions();
///
public:///for test