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

2024-03-19 22:28

本文主要是介绍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、工作流程

航点任务功能按如下流程,控制无人机执行航点任务:

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



http://www.chinasem.cn/article/827421

相关文章

Java中Redisson 的原理深度解析

《Java中Redisson的原理深度解析》Redisson是一个高性能的Redis客户端,它通过将Redis数据结构映射为Java对象和分布式对象,实现了在Java应用中方便地使用Redis,本文... 目录前言一、核心设计理念二、核心架构与通信层1. 基于 Netty 的异步非阻塞通信2. 编解码器三、

Java HashMap的底层实现原理深度解析

《JavaHashMap的底层实现原理深度解析》HashMap基于数组+链表+红黑树结构,通过哈希算法和扩容机制优化性能,负载因子与树化阈值平衡效率,是Java开发必备的高效数据结构,本文给大家介绍... 目录一、概述:HashMap的宏观结构二、核心数据结构解析1. 数组(桶数组)2. 链表节点(Node

Java 虚拟线程的创建与使用深度解析

《Java虚拟线程的创建与使用深度解析》虚拟线程是Java19中以预览特性形式引入,Java21起正式发布的轻量级线程,本文给大家介绍Java虚拟线程的创建与使用,感兴趣的朋友一起看看吧... 目录一、虚拟线程简介1.1 什么是虚拟线程?1.2 为什么需要虚拟线程?二、虚拟线程与平台线程对比代码对比示例:三

一文解析C#中的StringSplitOptions枚举

《一文解析C#中的StringSplitOptions枚举》StringSplitOptions是C#中的一个枚举类型,用于控制string.Split()方法分割字符串时的行为,核心作用是处理分割后... 目录C#的StringSplitOptions枚举1.StringSplitOptions枚举的常用

一文详解Python如何开发游戏

《一文详解Python如何开发游戏》Python是一种非常流行的编程语言,也可以用来开发游戏模组,:本文主要介绍Python如何开发游戏的相关资料,文中通过代码介绍的非常详细,需要的朋友可以参考下... 目录一、python简介二、Python 开发 2D 游戏的优劣势优势缺点三、Python 开发 3D

Python函数作用域与闭包举例深度解析

《Python函数作用域与闭包举例深度解析》Python函数的作用域规则和闭包是编程中的关键概念,它们决定了变量的访问和生命周期,:本文主要介绍Python函数作用域与闭包的相关资料,文中通过代码... 目录1. 基础作用域访问示例1:访问全局变量示例2:访问外层函数变量2. 闭包基础示例3:简单闭包示例4

MyBatis延迟加载与多级缓存全解析

《MyBatis延迟加载与多级缓存全解析》文章介绍MyBatis的延迟加载与多级缓存机制,延迟加载按需加载关联数据提升性能,一级缓存会话级默认开启,二级缓存工厂级支持跨会话共享,增删改操作会清空对应缓... 目录MyBATis延迟加载策略一对多示例一对多示例MyBatis框架的缓存一级缓存二级缓存MyBat

基于Python开发Windows自动更新控制工具

《基于Python开发Windows自动更新控制工具》在当今数字化时代,操作系统更新已成为计算机维护的重要组成部分,本文介绍一款基于Python和PyQt5的Windows自动更新控制工具,有需要的可... 目录设计原理与技术实现系统架构概述数学建模工具界面完整代码实现技术深度分析多层级控制理论服务层控制注

前端缓存策略的自解方案全解析

《前端缓存策略的自解方案全解析》缓存从来都是前端的一个痛点,很多前端搞不清楚缓存到底是何物,:本文主要介绍前端缓存的自解方案,文中通过代码介绍的非常详细,需要的朋友可以参考下... 目录一、为什么“清缓存”成了技术圈的梗二、先给缓存“把个脉”:浏览器到底缓存了谁?三、设计思路:把“发版”做成“自愈”四、代码

Java集合之Iterator迭代器实现代码解析

《Java集合之Iterator迭代器实现代码解析》迭代器Iterator是Java集合框架中的一个核心接口,位于java.util包下,它定义了一种标准的元素访问机制,为各种集合类型提供了一种统一的... 目录一、什么是Iterator二、Iterator的核心方法三、基本使用示例四、Iterator的工