本文主要是介绍dji psdk开发(10)航线任务简介、KMZ文件的解析,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!
航线任务到目前主要使用waypoint v2 和 waypoint v3三个版本。
- Waypoint v2.0:只支持 Matrice 300 RTK 和 Matrice 350 RTK。
- waypoint v3.0:支持 Matrice 30 Series、Mavic 3 Enterprise Series 和 Matrice 3D/3TD 机型,不支持 Matrice 300 RTK 和 Matrice 350 RTK。
这里先简要介绍 Waypoint 3.0,其只需要导入一个KMZ文件即可实现和Pilot2相同的航线规划功能。之后再介绍相对复杂的waypoint v2版本 api,手动解析kml/kml文件。
文章目录
- 0、工作流程
- 1、Waypoint 3.0
- 1.1、航点任务初始化
- 1.2、上传航点任务
- 1.3、控制无人机执行航点任务
- 1.4、监测航线状态
- 2、Waypoint v2.0
- 2.1、航点任务初始化
- 2.2、上传航点任务
- 2.3、控制无人机执行航点任务
- 2.4、无人机的巡航速度
- 3、KMZ/KML文件解析
- 3.1、KMZ文件解压
- 3.2、KML文件解析
- 3.3、测试
0、工作流程
航点任务功能按如下流程,控制无人机执行航点任务:
-
上传航线任务的整体信息
一个航点任务包含航点任务的ID、航点任务的航点数、任务重复次数、航点任务结束后的动作、最大飞行速度和巡航速度。 -
上传航点信息
基础参数:航点坐标(设置航点的经度、纬度和相对于起飞点的高度)、航点类型、航向类型和飞行速度。可选参数:缓冲距离、航向角度、转向模式、兴趣点、单点最大飞行速度、单点巡航速度。
说明: 仅开发者设置航点信息所有的基础参数后,才能设置航点信息的可选参数。
-
设置动作信息(可选)
设置动作的ID、触发器和执行器 -
上传无人机的航点任务的信息
-
控制无人机执行航点任务
上传无人机航点和对应的动作信息后,开发者即可通过指定的接口控制航点任务,如开始、停止或暂停任务,设置或获取巡航速度等
1、Waypoint 3.0
简要介绍,使用固件、app匹配的 Pilot2 在航线规划中导出一个kmz文件,之后在PSDK程序中加载。
Waypoint 3.0功能提供了上传航线任务、监控航线状态和控制航线执行动作等功能。相比Waypoint 2.0,可以直接导入标准格式的KMZ文件,通用性和兼容性更好。KMZ格式请参考:航线文件格式
这里仅介绍几个api接口。实际模拟或测试代码不给出,参看demo即可。
1.1、航点任务初始化
在使用航线任务-Waypoint 3.0功能之前,需要先调用接口 DjiWaypointV3_Init
接口进行初始化操作。
returnCode = DjiWaypointV3_Init();
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("Waypoint v3 init failed.");return returnCode;
}
结束时调用反初始化函数 T_DjiReturnCode DjiWaypointV3_DeInit(void);
。
1.2、上传航点任务
相比Waypoint 2.0复杂的航线配置,Waypoint 3.0的KMZ文件更加简单易用。用户可以通过调用 DjiWaypointV3_UploadKmzFile
直接传入KMZ文件数据,执行航线任务。KMZ文件可以在DJI Pilot提前规划好航线导出。
T_DjiReturnCode DjiWaypointV3_UploadKmzFile(const uint8_t *data, uint32_t dataLen);
1.3、控制无人机执行航点任务
当航线上传完成之后,即可调用DjiWaypointV3_Action
执行航线任务,包括开始、停止、暂停和继续执行航线任务。
typedef enum {DJI_WAYPOINT_V3_ACTION_START = 0, /*!< Waypoint v3 mission start action. */DJI_WAYPOINT_V3_ACTION_STOP = 1, /*!< Waypoint v3 mission stop action. */DJI_WAYPOINT_V3_ACTION_PAUSE = 2, /*!< Waypoint v3 mission pause action. */DJI_WAYPOINT_V3_ACTION_RESUME = 3, /*!< Waypoint v3 mission resume action. */
} E_DjiWaypointV3Action;T_DjiReturnCode DjiWaypointV3_Action(E_DjiWaypointV3Action action);
1.4、监测航线状态
在用户开始执行航线任务之前,建议注册航线状态回调接口,在执行航线过程中,可以通过注册的回调函数实时接收航线状态信息,保证飞行安全。
/**
* Waypoint v3 current mission state.
*/
typedef enum {DJI_WAYPOINT_V3_MISSION_STATE_IDLE = 0, /*!< Waypoint v3 mission in idle state. */DJI_WAYPOINT_V3_MISSION_STATE_PREPARE = 16, /*!< Waypoint v3 mission in prepare state. */DJI_WAYPOINT_V3_MISSION_STATE_TRANS_MISSION = 32, /*!< Waypoint v3 mission in trans mission state. */DJI_WAYPOINT_V3_MISSION_STATE_MISSION = 48, /*!< Waypoint v3 mission in mission state. */DJI_WAYPOINT_V3_MISSION_STATE_BREAK = 64, /*!< Waypoint v3 mission in break state. */DJI_WAYPOINT_V3_MISSION_STATE_RESUME = 80, /*!< Waypoint v3 mission in resume state. */DJI_WAYPOINT_V3_MISSION_STATE_RETURN_FIRSTPOINT = 98, /*!< Waypoint v3 mission in return first point state. */
} E_DjiWaypointV3MissionState;/**
* Waypoint v3 current action state.
*/
typedef enum {DJI_WAYPOINT_V3_ACTION_STATE_IDLE = 0, /*!< Waypoint v3 action in idle state. */DJI_WAYPOINT_V3_ACTION_STATE_RUNNING = 1, /*!< Waypoint v3 action in idle state. */DJI_WAYPOINT_V3_ACTION_STATE_FINISHED = 5, /*!< Waypoint v3 action in idle state. */
} E_DjiWaypointV3ActionState;/**
* Waypoint v3 mission state.
*/
typedef struct {E_DjiWaypointV3MissionState state; /*!< Waypoint v3 current mission state, #E_DjiWaypointV3MissionState. */uint32_t wayLineId; /*!< Waypoint v3 current way line id. */uint16_t currentWaypointIndex; /*!< Waypoint v3 current waypoint index. */
} T_DjiWaypointV3MissionState;/**
* Waypoint v3 action state.
*/
typedef struct {E_DjiWaypointV3ActionState state; /*!< Waypoint v3 current action state, #E_DjiWaypointV3ActionState. */uint32_t wayLineId; /*!< Waypoint v3 current way line id. */uint16_t currentWaypointIndex; /*!< Waypoint v3 current waypoint index. */uint16_t actionGroupId; /*!< Waypoint v3 current action group index. */uint16_t actionId; /*!< Waypoint v3 current action index. */
} T_DjiWaypointV3ActionState;returnCode = DjiWaypointV3_RegMissionStateCallback(DjiTest_WaypointV3StateCallback);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("Register waypoint v3 state callback failed.");goto out;
}returnCode = DjiWaypointV3_RegActionStateCallback(DjiTest_WaypointV3ActionStateCallback);
if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("Register waypoint v3 state callback failed.");goto out;
}
2、Waypoint v2.0
当我们了解kml航线文件中各种字段数据含义之后,使用Waypoint v2就需要使用大量的接口定义函数来创建一个航线任务。
Waypoint 2.0功能提供了上传航线任务、获取和设置巡航速度、监控航线状态和控制航线执行动作等功能。
2.1、航点任务初始化
初始化航点任务的信息,向无人机飞行控制器发送航点任务的整体信息和航点信息,其中航点任务的整体信息包括无人机的巡航速度、断连控制和航点信息等;航点信息包含航点高度,航点经纬度等。
returnCode = DjiTest_WaypointV2Init();if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("Init waypoint V2 sample failed, error code: 0x%08X", returnCode);USER_LOG_INFO("Waypoint V2 sample end");return returnCode;}USER_LOG_INFO("--> Step 2: Subscribe gps fused data");DjiTest_WidgetLogAppend("--> Step 2: Subscribe gps fused data");returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_POSITION_FUSED,DJI_DATA_SUBSCRIPTION_TOPIC_50_HZ, NULL);if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("Subscribe gps fused data failed, error code: 0x%08X", returnCode);goto out;}USER_LOG_INFO("--> Step 3: Register waypoint V2 event and state callback\r\n");DjiTest_WidgetLogAppend("--> Step 3: Register waypoint V2 event and state callback\r\n");returnCode = DjiWaypointV2_RegisterMissionEventCallback(DjiTest_WaypointV2EventCallback);if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("Register waypoint V2 event failed, error code: 0x%08X", returnCode);goto out;}returnCode = DjiWaypointV2_RegisterMissionStateCallback(DjiTest_WaypointV2StateCallback);if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("Register waypoint V2 state failed, error code: 0x%08X", returnCode);goto out;}osalHandler->TaskSleepMs(timeOutMs);
2.2、上传航点任务
T_DjiReturnCode returnCode;uint16_t polygonNum = missionNum - 2;dji_f32_t radius = 6;uint16_t actionNum = 5;T_DjiWayPointV2MissionSettings missionInitSettings = {0};T_DJIWaypointV2ActionList actionList = {NULL, 0};/*! Generate actions*/actionList.actions = DjiTest_WaypointV2GenerateWaypointV2Actions(actionNum);actionList.actionNum = actionNum;/*! Init waypoint settings*/missionInitSettings.missionID = s_missionID + 10;USER_LOG_DEBUG("Generate mission id:%d", missionInitSettings.missionID);missionInitSettings.repeatTimes = 1;missionInitSettings.finishedAction = DJI_WAYPOINT_V2_FINISHED_GO_HOME;missionInitSettings.maxFlightSpeed = 10;missionInitSettings.autoFlightSpeed = 2;missionInitSettings.actionWhenRcLost = DJI_WAYPOINT_V2_MISSION_KEEP_EXECUTE_WAYPOINT_V2;missionInitSettings.gotoFirstWaypointMode = DJI_WAYPOINT_V2_MISSION_GO_TO_FIRST_WAYPOINT_MODE_POINT_TO_POINT;missionInitSettings.mission = DjiTest_WaypointV2GeneratePolygonWaypointV2(radius, polygonNum);missionInitSettings.missTotalLen = missionNum;missionInitSettings.actionList = actionList;returnCode = DjiWaypointV2_UploadMission(&missionInitSettings);if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {USER_LOG_ERROR("Init waypoint V2 mission setting failed, ErrorCode:0x%lX", returnCode);return returnCode;}
2.3、控制无人机执行航点任务
控制无人机在执行航点任务的几个动作:
-
开始执行航点任务 无人机开始执行航点任务前,将先检查用户上传的任务信息,若检查失败,无人机将无法执行航点任务,详情请查看错误码信息。
T_DjiReturnCode DjiWaypointV2_Start(void);
-
暂停执行航点任务 控制无人机暂停执行航点任务。
T_DjiReturnCode DjiWaypointV2_Pause(void);
-
恢复执行航点任务 控制无人机从暂停的位置,继续执行为完成的航点任务。
T_DjiReturnCode DjiWaypointV2_Resume(void);
-
停止执行航点任务 控制无人机停止执行航点任务。
T_DjiReturnCode DjiWaypointV2_Stop(void);
2.4、无人机的巡航速度
设置或获取无人机在执行航点任务时的巡航速度。
- 设置巡航速度
T_DjiReturnCode DjiWaypointV2_SetGlobalCruiseSpeed(T_DjiWaypointV2GlobalCruiseSpeed cruiseSpeed);
- 获取巡航速度
T_DjiReturnCode DjiWaypointV2_GetGlobalCruiseSpeed(T_DjiWaypointV2GlobalCruiseSpeed
3、KMZ/KML文件解析
KMZ文件其实是kml文件的zip压缩包。大疆航线任务自定义使用的航线文件格式标准(DJI WPML)格式。这里介绍2个部分代码:kmz文件解压,kml文件解析。
大疆kmz文件修改后缀为zip之后,包含一个wpmz文件夹,里面包含2个kml格式文件 template.kml 和 waylines.wpml。从内容上,emplate.kml 定义业务属性,可以理解为全局的一些配置,方便用户进行快速调整编辑;waylines.wpml 定义每个航点执行细节。
我们设计一个简单的类来实现以上功能,其中依赖c++17是为了使用filesystem(gcc8低版本需要显示链接stdc++fs),另外两个依赖包为 zip 和 tinyxml2 。
#pragma once
#include <string>
/*Requied:libraries: zip tinyxml2c++17 filesystem: stdc++fs
*/
class KmlParser{
public:KmlParser() = default;bool ReadKMZ(const std::string& filename);bool ReadKML(const std::string& filename);private:bool parseXML(const std::string& filename);
};
3.1、KMZ文件解压
这里以 waylines.wpml 为目标解压保存为临时文件。
bool KmlParser::ReadKMZ(const std::string& filename)
{int error = 0;zip* archive = zip_open(filename.c_str(), 0, &error);if (error) {printf("Could not open KMZ file: %s\n", filename.c_str());return false;}const long nFiles = (long)zip_get_num_entries(archive, 0);if(nFiles < 1) {printf("KMZ file seems empty or not valid: %s\n", filename.c_str());zip_close(archive);return false;}std::string extractedKmlFile;struct zip_stat sb;for (long i=0; i<nFiles; i++) {if (zip_stat_index(archive, i, 0, &sb) != 0) {printf("while reading KMZ, unable to get details of a file in the ZIP.\n");continue;}int len = (int)strlen(sb.name);// Skip dirsif (sb.name[len - 1] == '/') continue;// Skip empty filesif(sb.size == 0) continue;fs::path zippedFilePath(sb.name);if( strcmp(".wpml", zippedFilePath.extension().c_str()) ) {continue;}struct zip_file* zf = zip_fopen_index(archive, i, 0);if (zf == nullptr) {printf("while extracting, unable to open KML file from KMZ: %s\n", filename.c_str());continue;}extractedKmlFile = fs::path(fs::temp_directory_path() / zippedFilePath.filename()).string();// printf("extracting KML filepwz: %s\n", extractedKmlFile.c_str());// To avoid problems it is better to delete the KML file if already existing, user has already been warnedif (fs::exists(extractedKmlFile)) std::remove(extractedKmlFile.c_str()); // Delete KML filestd::ofstream kmlFile;kmlFile.open(extractedKmlFile, std::ios::out | std::ios::trunc | std::ios::binary);if (!kmlFile.is_open() || kmlFile.bad()) {printf("While extracting KML file, unable to write: %s\n", extractedKmlFile.c_str());zip_fclose(zf);zip_close(archive);return false;}// Read from the ZIP and write to the extracted filechar buf[1024]; // 8000, the size of this read buffer, is just quite big number which I likeunsigned long sum = 0;while (sum < sb.size) {len = (int)zip_fread(zf, buf, 1024);if (len < 0) {printf("While extracting KML file, unable read compressed data from: %s\n", filename.c_str());kmlFile.close();zip_fclose(zf);zip_close(archive);return false;}kmlFile.write(buf, len);sum += len;}// Close allkmlFile.close();zip_fclose(zf);printf("Extracted KML file: %s\n", sb.name);// If we arrived at this point we assume that we just found and correctly extracted the KML file and so we don't need to go further in the KMZbreak;}// Close the ZIP archivezip_close(archive);// No KML... no party...if(extractedKmlFile.empty()) return false;bool retValue = ReadKML(extractedKmlFile);// std::remove(extractedKmlFile.c_str());return retValue;
}
3.2、KML文件解析
解析 waylines.wpml 文件中的内容,这里不构建一个大的结构体对象来存储,仅打印所有的航点位置信息。
bool KmlParser::ReadKML(const std::string& filename)
{std::ifstream input(filename);if (!input.is_open() || input.bad()) {printf("Unable to open KML file: %s\n", filename.c_str());return false;}// parse xml with Tinyxml2 librarybool retValue = parseXML(filename);return retValue;
}bool KmlParser::parseXML(const std::string& filename)
{using namespace tinyxml2;XMLDocument document;int res = document.LoadFile(filename.c_str());if (res != 0) {printf("load xml file failed");return res;}XMLElement* root = document.FirstChildElement( "kml" );if (!root) {printf("xml has no element `Document`\n");return false;}if(!root->Attribute("xmlns","http://www.opengis.net/kml/2.2") || !root->Attribute("xmlns:wpml","http://www.dji.com/wpmz/1.0.4") ) {printf("wrong attribute\n");return false;}// printf("%s\n",root->Attribute("xmlns"));// printf("%s\n",root->Attribute("xmlns:wpml"));XMLElement* missonCfgElement = root->FirstChildElement("Document")->FirstChildElement("wpml:missionConfig");XMLElement* WaypointsElement = root->FirstChildElement("Document")->FirstChildElement("Folder");if(!missonCfgElement || !WaypointsElement){printf("wrong mission or waypoint elements\n");return false; }// mission elementprintf("finishAction: %s\n", missonCfgElement->FirstChildElement("wpml:finishAction")->GetText());printf("takeOffSecurityHeight: %d\n", missonCfgElement->FirstChildElement("wpml:takeOffSecurityHeight")->IntText());printf("globalTransitionalSpeed: %d\n", missonCfgElement->FirstChildElement("wpml:globalTransitionalSpeed")->IntText());// waypoint elementprintf("executeHeightMode: %s\n", WaypointsElement->FirstChildElement("wpml:executeHeightMode")->GetText());printf("autoFlightSpeed: %d\n", WaypointsElement->FirstChildElement("wpml:autoFlightSpeed")->IntText());XMLElement* placemark = WaypointsElement->FirstChildElement("Placemark");while(placemark){int idx = placemark->FirstChildElement("wpml:index")->IntText();int executeHeight = placemark->FirstChildElement("wpml:executeHeight")->IntText();int waypointSpeed = placemark->FirstChildElement("wpml:waypointSpeed")->IntText();auto coord = placemark->FirstChildElement("Point")->FirstChildElement("coordinates")->GetText();double lng = 0, lat=0;if( 2 == sscanf(coord, "%Lf,%Lf", &lng, &lat)) { // 注意数据格式printf(" index: %d, h=%d, v=%d, gps=(%.12f,%.13f)\n", idx, executeHeight, waypointSpeed, lng, lat);}placemark = placemark->NextSiblingElement("Placemark");}return true;
}
3.3、测试
提供的6.kmz文件,部分内容如下:
测试代码
KmlParser parser;
if(!parser.ReadKMZ("6.kmz")){printf("parse KMZ failed.");
}
运行结果如下:
Extracted KML file: wpmz/waylines.wpml
finishAction: goHome
takeOffSecurityHeight: 20
globalTransitionalSpeed: 15
executeHeightMode: relativeToStartPoint
autoFlightSpeed: 5index: 0, h=100, v=5, gps=(118.860121291480,32.1576696738785)index: 1, h=100, v=5, gps=(118.849509272455,32.1516826299922)index: 2, h=100, v=5, gps=(118.881124323972,32.1531827432831)
这篇关于dji psdk开发(10)航线任务简介、KMZ文件的解析的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!