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

相关文章

Python通过模块化开发优化代码的技巧分享

《Python通过模块化开发优化代码的技巧分享》模块化开发就是把代码拆成一个个“零件”,该封装封装,该拆分拆分,下面小编就来和大家简单聊聊python如何用模块化开发进行代码优化吧... 目录什么是模块化开发如何拆分代码改进版:拆分成模块让模块更强大:使用 __init__.py你一定会遇到的问题模www.

Spring Security基于数据库的ABAC属性权限模型实战开发教程

《SpringSecurity基于数据库的ABAC属性权限模型实战开发教程》:本文主要介绍SpringSecurity基于数据库的ABAC属性权限模型实战开发教程,本文给大家介绍的非常详细,对大... 目录1. 前言2. 权限决策依据RBACABAC综合对比3. 数据库表结构说明4. 实战开始5. MyBA

Java图片压缩三种高效压缩方案详细解析

《Java图片压缩三种高效压缩方案详细解析》图片压缩通常涉及减少图片的尺寸缩放、调整图片的质量(针对JPEG、PNG等)、使用特定的算法来减少图片的数据量等,:本文主要介绍Java图片压缩三种高效... 目录一、基于OpenCV的智能尺寸压缩技术亮点:适用场景:二、JPEG质量参数压缩关键技术:压缩效果对比

使用Python开发一个简单的本地图片服务器

《使用Python开发一个简单的本地图片服务器》本文介绍了如何结合wxPython构建的图形用户界面GUI和Python内建的Web服务器功能,在本地网络中搭建一个私人的,即开即用的网页相册,文中的示... 目录项目目标核心技术栈代码深度解析完整代码工作流程主要功能与优势潜在改进与思考运行结果总结你是否曾经

关于WebSocket协议状态码解析

《关于WebSocket协议状态码解析》:本文主要介绍关于WebSocket协议状态码的使用方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录WebSocket协议状态码解析1. 引言2. WebSocket协议状态码概述3. WebSocket协议状态码详解3

Linux中的计划任务(crontab)使用方式

《Linux中的计划任务(crontab)使用方式》:本文主要介绍Linux中的计划任务(crontab)使用方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录一、前言1、linux的起源与发展2、什么是计划任务(crontab)二、crontab基础1、cro

CSS Padding 和 Margin 区别全解析

《CSSPadding和Margin区别全解析》CSS中的padding和margin是两个非常基础且重要的属性,它们用于控制元素周围的空白区域,本文将详细介绍padding和... 目录css Padding 和 Margin 全解析1. Padding: 内边距2. Margin: 外边距3. Padd

Oracle数据库常见字段类型大全以及超详细解析

《Oracle数据库常见字段类型大全以及超详细解析》在Oracle数据库中查询特定表的字段个数通常需要使用SQL语句来完成,:本文主要介绍Oracle数据库常见字段类型大全以及超详细解析,文中通过... 目录前言一、字符类型(Character)1、CHAR:定长字符数据类型2、VARCHAR2:变长字符数

使用Jackson进行JSON生成与解析的新手指南

《使用Jackson进行JSON生成与解析的新手指南》这篇文章主要为大家详细介绍了如何使用Jackson进行JSON生成与解析处理,文中的示例代码讲解详细,感兴趣的小伙伴可以跟随小编一起学习一下... 目录1. 核心依赖2. 基础用法2.1 对象转 jsON(序列化)2.2 JSON 转对象(反序列化)3.

Springboot @Autowired和@Resource的区别解析

《Springboot@Autowired和@Resource的区别解析》@Resource是JDK提供的注解,只是Spring在实现上提供了这个注解的功能支持,本文给大家介绍Springboot@... 目录【一】定义【1】@Autowired【2】@Resource【二】区别【1】包含的属性不同【2】@