From bbb9bcace4782fe292d819c3ebd4d0119fc984c2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=BC=A0=E5=A3=B9?= Date: Fri, 11 Jul 2025 13:08:40 +0800 Subject: [PATCH] =?UTF-8?q?=E4=B8=8A=E4=BC=A0=E6=96=87=E4=BB=B6=E8=87=B3?= =?UTF-8?q?=20/?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- config.toml | 39 +++++ dji_kmz.cpp | 477 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 516 insertions(+) create mode 100644 config.toml create mode 100644 dji_kmz.cpp diff --git a/config.toml b/config.toml new file mode 100644 index 0000000..de1b40f --- /dev/null +++ b/config.toml @@ -0,0 +1,39 @@ +author = "zhangyi" +kmzFile = "Test.kmz" + +[placemark] +PointFile = "../data/ZJGFP01.kml" +useGlobalHeight = "yes" +useGlobalSpeed = "yes" +useGlobalHeadingParam = "yes" +useGlobalTurnParam = "yes" + +[missionConfig] +droneType = "Matrice 350 RTK" +flyToWaylineMode = "safely" +finishAction = "goHome" +exitOnRCLost = "goContinue" +takeOffSecurityHeight = 50.0 +globalTransitionalSpeed = 10.0 +globalRTHHeight = 50.0 + +[wayline] +templateType = "waypoint" +templateId = 0 +autoFlightSpeed = 8.0 +globalHeight = 30.0 +caliFlightEnable = "no" +gimbalPitchMode = "manual" +globalWaypointTurnMode = "coordinateTurn" +globalUseStraightLine = "yes" + +[wayline.waylineCoordinateSysParam] +coordinateMode = "WGS84" +heightMode = "relativeToStartPoint" +positioningType = "GPS" + +[wayline.globalWaypointHeadingParam] +waypointHeadingMode = "followWayline" +waypointHeadingAngle = 0.0 +waypointPoiPoint = [0.0, 0.0, 0.0] +waypointHeadingPoiIndex = 0 \ No newline at end of file diff --git a/dji_kmz.cpp b/dji_kmz.cpp new file mode 100644 index 0000000..5c977c6 --- /dev/null +++ b/dji_kmz.cpp @@ -0,0 +1,477 @@ +#include "iostream" +#include "fstream" +#include "sstream" +#include "iomanip" +#include "vector" +#include "filesystem" +#include "toml.hpp" + +using namespace std; + +namespace dji_kmz +{ + int get_droneEnumValue(std::string name) + { + if (name == "Matrice 350 RTK") return 89; + if (name == "Matrice 300 RTK") return 60; + if (name == "Matrice 30") return 67; + if (name == "Matrice 30T") return 67; + if (name == "Mavic 3E") return 77; + if (name == "Mavic 3T") return 77; + if (name == "Matrice 3D") return 91; + if (name == "Matrice 3TD") return 91; + if (name == "Matrice 4D") return 100; + if (name == "Matrice 4TD") return 100; + if (name == "Matrice 4E") return 99; + if (name == "Matrice 4T") return 99; + throw std::runtime_error("dji_kmz: Invalid drone type."); + } + + std::string get_flyToWaylineMode(std::string mode) + { + if (mode != "safely" && mode != "pointToPoint") + throw std::runtime_error("dji_kmz: Invalid fly to wayline mode."); + + return mode; + } + + std::string get_finishAction(std::string action) + { + if (action != "goHome" && action != "noAction" && + action != "autoLand" && action != "gotoFirstWaypoint") + throw std::runtime_error("dji_kmz: Invalid finish action."); + + return action; + } + + std::string get_exitOnRCLost(std::string action) + { + if (action != "goContinue" && action != "executeLostAction") + throw std::runtime_error("dji_kmz: Invalid exit mode on RC lost."); + + return action; + } + + std::string get_executeRCLostAction(std::string action) + { + if (action != "goBack" && action != "landing" && action != "hover") + throw std::runtime_error("dji_kmz: Invalid action on RC lost."); + + return action; + } + + std::string get_templateType(std::string type) + { + if (type != "waypoint" && type != "mapping2d" && + type != "mapping3d" && type != "mappingStrip") + throw std::runtime_error("dji_kmz: Invalid template type."); + + return type; + } + + int is_caliFlightEnable(std::string type) + { + if (type == "no") return 0; + if (type == "yes") return 1; + throw std::runtime_error("dji_kmz: Invalid caliberate flight mode."); + } + + int is_globalUseStraightLine(std::string type) + { + if (type == "no") return 0; + if (type == "yes") return 1; + throw std::runtime_error("dji_kmz: Invalid global use straight line mode."); + } + + std::string get_gimbalPitchMode(std::string type) + { + if (type != "manual" && type != "usePointSetting") + throw std::runtime_error("dji_kmz: Invalid gimbal pitch mode."); + + return type; + } + + std::string get_globalWaypointTurnMode(std::string mode) + { + if (mode != "coordinateTurn" && + mode != "toPointAndStopWithDiscontinuityCurvature" && + mode != "toPointAndStopWithContinuityCurvature" && + mode != "toPointAndPassWithContinuityCurvature") + throw std::runtime_error("dji_kmz: Invalid global waypoint turn mode."); + + return mode; + } + + std::string get_wayline_coordinateMode(std::string mode) + { + if (mode != "WGS84") + throw std::runtime_error("dji_kmz: Invalid wayline coordinate mode."); + + return mode; + } + + std::string get_wayline_heightMode(std::string mode) + { + if (mode != "EGM96" && mode != "relativeToStartPoint" && + mode != "aboveGroundLevel" && mode != "realTimeFollowSurface") + throw std::runtime_error("dji_kmz: Invalid wayline height mode."); + + return mode; + } + + std::string get_wayline_positioningType(std::string type) + { + if (type != "GPS" && type != "RTKBaseStation" && + type != "QianXun" && type != "Custom") + throw std::runtime_error("dji_kmz: Invalid wayline positioning type."); + + return type; + } + + std::string get_waypoint_HeadingMode(std::string mode) + { + if (mode != "followWayline" && mode != "manually" && + mode != "fixed" && mode != "smoothTransition" && + mode != "towardPOI") + throw std::runtime_error("dji_kmz: Invalid waypoint heading mode."); + + return mode; + } + + int is_placemark_useGlobalHeight(std::string type) + { + if (type == "no") return 0; + if (type == "yes") return 1; + throw std::runtime_error("dji_kmz: Invalid placemark use global height mode."); + } + + int is_placemark_useGlobalSpeed(std::string type) + { + if (type == "no") return 0; + if (type == "yes") return 1; + throw std::runtime_error("dji_kmz: Invalid placemark use global speed mode."); + } + + int is_placemark_useGlobalHeadingParam(std::string type) + { + if (type == "no") return 0; + if (type == "yes") return 1; + throw std::runtime_error("dji_kmz: Invalid placemark use global heading-param mode."); + } + + int is_placemark_useGlobalTurnParam(std::string type) + { + if (type == "no") return 0; + if (type == "yes") return 1; + throw std::runtime_error("dji_kmz: Invalid placemark use global turn-param mode."); + } + + void get_googleEarth_coorpoints(string google_kml, vector &plon, vector &plat) + { + ifstream ifile(google_kml); + if (!ifile) throw std::runtime_error("dji_kmz: Fail to open the google earth's .kml file."); + + plon.clear(); + plat.clear(); + + double lon, lat; + string line, cor_str; + stringstream line_ss, cor_ss; + while (getline(ifile, line)) + { + line.erase(0, line.find_first_not_of(" \t")); + line.erase(line.find_last_not_of(" \t") + 1); + + if (line == "") + { + getline(ifile, line); + line.erase(0, line.find_first_not_of(" \t")); + line.erase(line.find_last_not_of(" \t") + 1); + + line_ss.clear(); + line_ss.str(line); + while (line_ss >> cor_str) + { + replace(cor_str.begin(), cor_str.end(), ',', ' '); + + cor_ss.clear(); + cor_ss.str(cor_str); + cor_ss >> lon >> lat; + plon.push_back(lon); + plat.push_back(lat); + } + break; + } + } + ifile.close(); + } + + void read_PointFile(string filename, vector &lons, vector &lats, vector &hgts) + { + std::ifstream infile(filename); + if (!infile.is_open()) throw std::runtime_error("dji_kmz: Fail to open point file."); + + lons.clear(); + lats.clear(); + hgts.clear(); + + string line; + stringstream ss_line; + double lon, lat, hgt; + while (getline(infile, line)) + { + replace(line.begin(), line.end(), ',', ' '); + ss_line.clear(); + ss_line.str(line); + ss_line >> lon >> lat >> hgt; + + lons.push_back(lon); + lats.push_back(lat); + hgts.push_back(hgt); + } + infile.close(); + return; + } + + /** + * @brief 在多边形区域内按一定间隔生成航线点 + * + * @param plon 多边形顶点经度数组 + * @param plat 多边形顶点纬度数组 + * @param lon1 航线参考方向起点经度 + * @param lat1 航线参考方向起点纬度 + * @param lon2 航线参考方向终点经度 + * @param lat2 航线参考方向终点纬度 + * @param mini_len 最短航线长度 以公里表示 + * @param line_space 航线之间的间距 以公里表示 + * @param wlon 返回的航线经度数组 + * @param wlat 返回的航线纬度数组 + */ + void polygon2waypoints(const vector &plon, const vector &plat, double lon1, double lat1, + double lon2, double lat2, double mini_len, double line_space, vector &wlon, vector &wlat) + { + + } + + void template_placemark(ofstream &ofile, double lon, double lat, int hgt, int idx, + int global_height, int global_speed, int global_heading, int global_turn) + { + ofile << " \n"; + ofile << " \n"; + ofile << " \n"; + ofile << " " << setprecision(16) << lon << "," << lat << "\n"; + ofile << " \n"; + ofile << " \n"; + ofile << " " << idx << "\n"; + ofile << " " << hgt << "\n"; + ofile << " " << hgt << "\n"; + ofile << " " << global_height << "\n"; + ofile << " " << global_speed << "\n"; + ofile << " " << global_heading << "\n"; + ofile << " " << global_turn << "\n"; + ofile << " 0\n"; + ofile << " \n"; + return; + } +}; + + +int main(int argc, char const *argv[]) +{ + if (argc < 2) + { + cout << "Usage: dji_kmz [-h] \n"; + return -1; + } + else if (argc >= 2 && !strcmp(argv[1], "-h")) + { + cout << "Convert Google Earth's kml file or plain locations to DJI's wayline file. \ +This open-source software is provided “as is” without warranty of any kind, and the \ +author/contributors shall not be liable for any damages arising from its use.\n\n"; + cout << "Author: Dr. Yi Zhang\n"; + cout << "Contact: School of Earth Science, Zhejiang University\n"; + cout << "E-mail: yizhang-geo@zju.edu.cn\n\n"; + cout << "Usage: dji_kmz [-h] \n\n"; + cout << "Toml's Options:\n"; + cout << "----------------------------------\n"; + cout << "author = \"name\" # Author of output .kmz file\n"; + cout << "kmzFile = \"out.kmz\" # Output DJI's .kmz file\n"; + cout << "\n"; + cout << "[missionConfig]\n"; + cout << "droneType = \"Matrice 350 RTK\" # Type of drone used for the mission\n"; + cout << " # Possible values:\n"; + cout << " # - Matrice 350 RTK\n"; + cout << " # - Matrice 300 RTK\n"; + cout << " # - Matrice 30\n"; + cout << " # - Matrice 30T\n"; + cout << " # - Mavic 3E\n"; + cout << " # - Mavic 3T\n"; + cout << " # - Matrice 3D\n"; + cout << " # - Matrice 3TD\n"; + cout << " # - Matrice 4D\n"; + cout << " # - Matrice 4TD\n"; + cout << " # - Matrice 4E\n"; + cout << " # - Matrice 4T\n"; + cout << "flyToWaylineMode = \"safely\" # Mode to fly to wayline (safely or pointToPoint)\n"; + cout << "finishAction = \"goHome\" # Action to take on mission finish (goHome, noAction, autoLand, gotoFirstWaypoint)\n"; + cout << "exitOnRCLost = \"goContinue\" # Action on RC signal lost (goContinue, executeLostAction)\n"; + cout << "takeOffSecurityHeight = 50.0 # Height to ascend to before starting mission\n"; + cout << "globalTransitionalSpeed = 10.0 # Speed during transitional phases\n"; + cout << "globalRTHHeight = 50.0 # Height for Return to Home (RTH)\n"; + cout << "\n"; + cout << "[wayline]\n"; + cout << "templateType = \"waypoint\" # Type of wayline template (waypoint, mapping2d, mapping3d, mappingStrip)\n"; + cout << "templateId = 0 # Unique identifier for the wayline template\n"; + cout << "autoFlightSpeed = 5.0 # Speed during automatic flight\n"; + cout << "globalHeight = 30.0 # Default height for waypoints\n"; + cout << "caliFlightEnable = \"no\" # Enable calibration flight (yes/no)\n"; + cout << "gimbalPitchMode = \"manual\" # Gimbal pitch control mode (manual, usePointSetting)\n"; + cout << "globalWaypointTurnMode = \"coordinateTurn\" # Turn mode at waypoints (coordinateTurn, toPointAndStopWithDiscontinuityCurvature, toPointAndStopWithContinuityCurvature, toPointAndPassWithContinuityCurvature)\n"; + cout << "globalUseStraightLine = \"yes\" # Use straight line for waypoints (yes/no)\n"; + cout << "\n"; + cout << "[wayline.waylineCoordinateSysParam]\n"; + cout << "coordinateMode = \"WGS84\" # Coordinate system mode (WGS84)\n"; + cout << "heightMode = \"relativeToStartPoint\" # Height reference mode (EGM96, relativeToStartPoint, aboveGroundLevel, realTimeFollowSurface)\n"; + cout << "positioningType = \"GPS\" # Positioning type (GPS, RTKBaseStation, QianXun, Custom)\n"; + cout << "\n"; + cout << "[wayline.globalWaypointHeadingParam]\n"; + cout << "waypointHeadingMode = \"followWayline\" # Heading mode at waypoints (followWayline, manually, fixed, smoothTransition, towardPOI)\n"; + cout << "waypointHeadingAngle = 0.0 # Heading angle for waypoints\n"; + cout << "waypointPoiPoint = [0.0, 0.0, 0.0] # Point of interest coordinates for heading (x, y, z)\n"; + cout << "waypointHeadingPoiIndex = 0 # Index of the point of interest for heading\n"; + cout << "\n"; + cout << "[placemark]\n"; + cout << "PointFile = \"example.csv\" # File containing waypoint coordinates\n"; + cout << "useGlobalHeight = \"yes\" # Use global height settings for waypoints (yes/no)\n"; + cout << "useGlobalSpeed = \"yes\" # Use global speed settings for waypoints (yes/no)\n"; + cout << "useGlobalHeadingParam = \"yes\" # Use global heading parameters for waypoints (yes/no)\n"; + cout << "useGlobalTurnParam = \"yes\" # Use global turn parameters for waypoints (yes/no)\n\n"; + cout << R"(Point File Format: +---------------------------------- +This program can read coordinate marker files saved by Google Earth (in KML format) or ordinary text data files, where each line contains the longitude, latitude, and flight altitude of a waypoint (a negative value indicates the use of the global flight altitude defined by the globalHeight parameter). +)"; + return -1; + } + + // Read configuration + toml::value toml_data = toml::parse(argv[1]); + + // Basic options + std::string author = toml::find(toml_data, "author"); + std::string kmz_file = toml::find(toml_data, "kmzFile"); + std::string exten_name = kmz_file.substr(kmz_file.find_last_of('.')); + if (exten_name != ".kmz") kmz_file += ".kmz"; + + // mission options + int drone_enum = dji_kmz::get_droneEnumValue(toml::find(toml_data, "missionConfig", "droneType")); + string fly_to_wayline = dji_kmz::get_flyToWaylineMode(toml::find(toml_data, "missionConfig", "flyToWaylineMode")); + string finish_action = dji_kmz::get_finishAction(toml::find(toml_data, "missionConfig", "finishAction")); + string on_rc_lost = dji_kmz::get_exitOnRCLost(toml::find(toml_data, "missionConfig", "exitOnRCLost")); + float security_height = toml::find(toml_data, "missionConfig", "takeOffSecurityHeight"); + float trans_speed = toml::find(toml_data, "missionConfig", "globalTransitionalSpeed"); + float rth_height = toml::find(toml_data, "missionConfig", "globalRTHHeight"); + + // wayline options + int use_straight_line = dji_kmz::is_globalUseStraightLine(toml::find(toml_data, "wayline", "globalUseStraightLine")); + int template_id = toml::find(toml_data, "wayline", "templateId"); + int cali_flight = dji_kmz::is_caliFlightEnable(toml::find(toml_data, "wayline", "caliFlightEnable")); + float auto_speed = toml::find(toml_data, "wayline", "autoFlightSpeed"); + float global_hgt = toml::find(toml_data, "wayline", "globalHeight"); + string template_type = dji_kmz::get_templateType(toml::find(toml_data, "wayline", "templateType")); + string pitch_mode = dji_kmz::get_gimbalPitchMode(toml::find(toml_data, "wayline", "gimbalPitchMode")); + string global_trun_mode = dji_kmz::get_globalWaypointTurnMode(toml::find(toml_data, "wayline", "globalWaypointTurnMode")); + // wayline waylineCoordinateSysParam + string coor_mode = dji_kmz::get_wayline_coordinateMode(toml::find(toml_data, "wayline", "waylineCoordinateSysParam", "coordinateMode")); + string hgt_mode = dji_kmz::get_wayline_heightMode(toml::find(toml_data, "wayline", "waylineCoordinateSysParam", "heightMode")); + string posi_type = dji_kmz::get_wayline_positioningType(toml::find(toml_data, "wayline", "waylineCoordinateSysParam", "positioningType")); + // wayline globalWaypointHeadingParam + int poi_indx = toml::find(toml_data, "wayline", "globalWaypointHeadingParam", "waypointHeadingPoiIndex"); + float heading_angle = toml::find(toml_data, "wayline", "globalWaypointHeadingParam", "waypointHeadingAngle"); + string heading_mode = dji_kmz::get_waypoint_HeadingMode(toml::find(toml_data, "wayline", "globalWaypointHeadingParam", "waypointHeadingMode")); + std::vector poi_point = toml::find >(toml_data, "wayline", "globalWaypointHeadingParam", "waypointPoiPoint"); + if (poi_point.size() != 3) throw std::runtime_error("dji_kmz: Invalid poi-points."); + + // Placemark options + std::string point_file = toml::find(toml_data, "placemark", "PointFile"); + int global_height = dji_kmz::is_placemark_useGlobalHeight(toml::find(toml_data, "placemark", "useGlobalHeight")); + int global_speed = dji_kmz::is_placemark_useGlobalSpeed(toml::find(toml_data, "placemark", "useGlobalSpeed")); + int global_heading = dji_kmz::is_placemark_useGlobalHeadingParam(toml::find(toml_data, "placemark", "useGlobalHeadingParam")); + int global_turn = dji_kmz::is_placemark_useGlobalTurnParam(toml::find(toml_data, "placemark", "useGlobalTurnParam")); + + // Get waypoints + vector lons, lats, hgts; + exten_name = point_file.substr(point_file.find_last_of('.')); + if (exten_name == ".kml") + { + dji_kmz::get_googleEarth_coorpoints(point_file, lons, lats); + hgts.resize(lons.size(), -1.0); + } + else dji_kmz::read_PointFile(point_file, lons, lats, hgts); + + // Create a template folder + // The following may diff between different OS-systems, improve later + int ret = system("mkdir wpmz"); + if (ret == -1) throw std::runtime_error("dji_kmz: Fail to create working directory."); + + // Write template.kml + ofstream ofile("wpmz/template.kml"); + if (!ofile.is_open()) throw std::runtime_error("dji_kmz: Fail to create working file."); + + ofile << R"( + + +)"; + ofile << " " << author << "\n"; + ofile << " \n"; + ofile << " " << fly_to_wayline << "\n"; + ofile << " " << finish_action << "\n"; + ofile << " " << on_rc_lost << "\n"; + ofile << " " << security_height << "\n"; + ofile << " " << trans_speed << "\n"; + ofile << " " << rth_height << "\n"; + ofile << " \n"; + ofile << " " << drone_enum << "\n"; + ofile << " 0\n"; + ofile << " \n"; + ofile << " \n"; + ofile << " \n"; + ofile << " " << template_type << "\n"; + ofile << " " << template_id << "\n"; + ofile << " \n"; + ofile << " " << coor_mode << "\n"; + ofile << " " << hgt_mode << "\n"; + ofile << " " << posi_type << "\n"; + ofile << " \n"; + ofile << " " << auto_speed << "\n"; + ofile << " " << global_hgt << "\n"; + ofile << " " << cali_flight << "\n"; + ofile << " " << pitch_mode << "\n"; + ofile << " \n"; + ofile << " " << heading_mode << "\n"; + ofile << " " << heading_angle << "\n"; + ofile << " " << poi_point[0] << "," << poi_point[1] << "," << poi_point[2] << "\n"; + ofile << " " << poi_indx << "\n"; + ofile << " \n"; + ofile << " " << global_trun_mode << "\n"; + ofile << " " << use_straight_line << "\n"; + + int hgt; + for (size_t i = 0; i < lons.size(); i++) + { + if (hgts[i] < 0) hgt = global_hgt; + else hgt = hgts[i]; + + dji_kmz::template_placemark(ofile, lons[i], lats[i], hgt, i, global_height, global_speed, global_heading, global_turn); + } + + ofile << R"( + +)"; + ofile.close(); + + // Zip + string cmd = "zip -q -r " + kmz_file + " wpmz && rm -rf wpmz"; + ret = system(cmd.c_str()); + if (ret != 0) throw std::runtime_error("gji_kmz: Fail to compress the .kmz file."); + return 0; +}