dji psdk开发（10）航线任务简介、KMZ文件的解析

本文主要是介绍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. 上传航线任务的整体信息
   一个航点任务包含航点任务的ID、航点任务的航点数、任务重复次数、航点任务结束后的动作、最大飞行速度和巡航速度。

2. 上传航点信息
   基础参数：航点坐标（设置航点的经度、纬度和相对于起飞点的高度）、航点类型、航向类型和飞行速度。

   可选参数：缓冲距离、航向角度、转向模式、兴趣点、单点最大飞行速度、单点巡航速度。

   说明： 仅开发者设置航点信息所有的基础参数后，才能设置航点信息的可选参数。

3. 设置动作信息（可选）
   设置动作的ID、触发器和执行器

4. 上传无人机的航点任务的信息

5. 控制无人机执行航点任务
   上传无人机航点和对应的动作信息后，开发者即可通过指定的接口控制航点任务，如开始、停止或暂停任务，设置或获取巡航速度等

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文件的解析的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---