478 lines
22 KiB
C++
478 lines
22 KiB
C++
#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<double> &plon, vector<double> &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 == "<coordinates>")
|
|
{
|
|
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<double> &lons, vector<double> &lats, vector<double> &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<double> &plon, const vector<double> &plat, double lon1, double lat1,
|
|
double lon2, double lat2, double mini_len, double line_space, vector<double> &wlon, vector<double> &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 << " <Placemark>\n";
|
|
ofile << " <Point>\n";
|
|
ofile << " <coordinates>\n";
|
|
ofile << " " << setprecision(16) << lon << "," << lat << "\n";
|
|
ofile << " </coordinates>\n";
|
|
ofile << " </Point>\n";
|
|
ofile << " <wpml:index>" << idx << "</wpml:index>\n";
|
|
ofile << " <wpml:ellipsoidHeight>" << hgt << "</wpml:ellipsoidHeight>\n";
|
|
ofile << " <wpml:height>" << hgt << "</wpml:height>\n";
|
|
ofile << " <wpml:useGlobalHeight>" << global_height << "</wpml:useGlobalHeight>\n";
|
|
ofile << " <wpml:useGlobalSpeed>" << global_speed << "</wpml:useGlobalSpeed>\n";
|
|
ofile << " <wpml:useGlobalHeadingParam>" << global_heading << "</wpml:useGlobalHeadingParam>\n";
|
|
ofile << " <wpml:useGlobalTurnParam>" << global_turn << "</wpml:useGlobalTurnParam>\n";
|
|
ofile << " <wpml:isRisky>0</wpml:isRisky>\n";
|
|
ofile << " </Placemark>\n";
|
|
return;
|
|
}
|
|
};
|
|
|
|
|
|
int main(int argc, char const *argv[])
|
|
{
|
|
if (argc < 2)
|
|
{
|
|
cout << "Usage: dji_kmz [-h] <config.toml>\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] <config.toml>\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<string>(toml_data, "author");
|
|
std::string kmz_file = toml::find<string>(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<string>(toml_data, "missionConfig", "droneType"));
|
|
string fly_to_wayline = dji_kmz::get_flyToWaylineMode(toml::find<string>(toml_data, "missionConfig", "flyToWaylineMode"));
|
|
string finish_action = dji_kmz::get_finishAction(toml::find<string>(toml_data, "missionConfig", "finishAction"));
|
|
string on_rc_lost = dji_kmz::get_exitOnRCLost(toml::find<string>(toml_data, "missionConfig", "exitOnRCLost"));
|
|
float security_height = toml::find<float>(toml_data, "missionConfig", "takeOffSecurityHeight");
|
|
float trans_speed = toml::find<float>(toml_data, "missionConfig", "globalTransitionalSpeed");
|
|
float rth_height = toml::find<float>(toml_data, "missionConfig", "globalRTHHeight");
|
|
|
|
// wayline options
|
|
int use_straight_line = dji_kmz::is_globalUseStraightLine(toml::find<string>(toml_data, "wayline", "globalUseStraightLine"));
|
|
int template_id = toml::find<int>(toml_data, "wayline", "templateId");
|
|
int cali_flight = dji_kmz::is_caliFlightEnable(toml::find<string>(toml_data, "wayline", "caliFlightEnable"));
|
|
float auto_speed = toml::find<float>(toml_data, "wayline", "autoFlightSpeed");
|
|
float global_hgt = toml::find<float>(toml_data, "wayline", "globalHeight");
|
|
string template_type = dji_kmz::get_templateType(toml::find<string>(toml_data, "wayline", "templateType"));
|
|
string pitch_mode = dji_kmz::get_gimbalPitchMode(toml::find<string>(toml_data, "wayline", "gimbalPitchMode"));
|
|
string global_trun_mode = dji_kmz::get_globalWaypointTurnMode(toml::find<string>(toml_data, "wayline", "globalWaypointTurnMode"));
|
|
// wayline waylineCoordinateSysParam
|
|
string coor_mode = dji_kmz::get_wayline_coordinateMode(toml::find<string>(toml_data, "wayline", "waylineCoordinateSysParam", "coordinateMode"));
|
|
string hgt_mode = dji_kmz::get_wayline_heightMode(toml::find<string>(toml_data, "wayline", "waylineCoordinateSysParam", "heightMode"));
|
|
string posi_type = dji_kmz::get_wayline_positioningType(toml::find<string>(toml_data, "wayline", "waylineCoordinateSysParam", "positioningType"));
|
|
// wayline globalWaypointHeadingParam
|
|
int poi_indx = toml::find<int>(toml_data, "wayline", "globalWaypointHeadingParam", "waypointHeadingPoiIndex");
|
|
float heading_angle = toml::find<float>(toml_data, "wayline", "globalWaypointHeadingParam", "waypointHeadingAngle");
|
|
string heading_mode = dji_kmz::get_waypoint_HeadingMode(toml::find<string>(toml_data, "wayline", "globalWaypointHeadingParam", "waypointHeadingMode"));
|
|
std::vector<float> poi_point = toml::find<std::vector<float> >(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<string>(toml_data, "placemark", "PointFile");
|
|
int global_height = dji_kmz::is_placemark_useGlobalHeight(toml::find<string>(toml_data, "placemark", "useGlobalHeight"));
|
|
int global_speed = dji_kmz::is_placemark_useGlobalSpeed(toml::find<string>(toml_data, "placemark", "useGlobalSpeed"));
|
|
int global_heading = dji_kmz::is_placemark_useGlobalHeadingParam(toml::find<string>(toml_data, "placemark", "useGlobalHeadingParam"));
|
|
int global_turn = dji_kmz::is_placemark_useGlobalTurnParam(toml::find<string>(toml_data, "placemark", "useGlobalTurnParam"));
|
|
|
|
// Get waypoints
|
|
vector<double> 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"(<?xml version="1.0" encoding="UTF-8"?>
|
|
<kml xmlns="http://www.opengis.net/kml/2.2" xmlns:wpml="http://www.dji.com/wpmz/1.0.5">
|
|
<Document>
|
|
)";
|
|
ofile << " <wpml:author>" << author << "</wpml:author>\n";
|
|
ofile << " <wpml:missionConfig>\n";
|
|
ofile << " <wpml:flyToWaylineMode>" << fly_to_wayline << "</wpml:flyToWaylineMode>\n";
|
|
ofile << " <wpml:finishAction>" << finish_action << "</wpml:finishAction>\n";
|
|
ofile << " <wpml:exitOnRCLost>" << on_rc_lost << "</wpml:exitOnRCLost>\n";
|
|
ofile << " <wpml:takeOffSecurityHeight>" << security_height << "</wpml:takeOffSecurityHeight>\n";
|
|
ofile << " <wpml:globalTransitionalSpeed>" << trans_speed << "</wpml:globalTransitionalSpeed>\n";
|
|
ofile << " <wpml:globalRTHHeight>" << rth_height << "</wpml:globalRTHHeight>\n";
|
|
ofile << " <wpml:droneInfo>\n";
|
|
ofile << " <wpml:droneEnumValue>" << drone_enum << "</wpml:droneEnumValue>\n";
|
|
ofile << " <wpml:droneSubEnumValue>0</wpml:droneSubEnumValue>\n";
|
|
ofile << " </wpml:droneInfo>\n";
|
|
ofile << " </wpml:missionConfig>\n";
|
|
ofile << " <Folder>\n";
|
|
ofile << " <wpml:templateType>" << template_type << "</wpml:templateType>\n";
|
|
ofile << " <wpml:templateId>" << template_id << "</wpml:templateId>\n";
|
|
ofile << " <wpml:waylineCoordinateSysParam>\n";
|
|
ofile << " <wpml:coordinateMode>" << coor_mode << "</wpml:coordinateMode>\n";
|
|
ofile << " <wpml:heightMode>" << hgt_mode << "</wpml:heightMode>\n";
|
|
ofile << " <wpml:positioningType>" << posi_type << "</wpml:positioningType>\n";
|
|
ofile << " </wpml:waylineCoordinateSysParam>\n";
|
|
ofile << " <wpml:autoFlightSpeed>" << auto_speed << "</wpml:autoFlightSpeed>\n";
|
|
ofile << " <wpml:globalHeight>" << global_hgt << "</wpml:globalHeight>\n";
|
|
ofile << " <wpml:caliFlightEnable>" << cali_flight << "</wpml:caliFlightEnable>\n";
|
|
ofile << " <wpml:gimbalPitchMode>" << pitch_mode << "</wpml:gimbalPitchMode>\n";
|
|
ofile << " <wpml:globalWaypointHeadingParam>\n";
|
|
ofile << " <wpml:waypointHeadingMode>" << heading_mode << "</wpml:waypointHeadingMode>\n";
|
|
ofile << " <wpml:waypointHeadingAngle>" << heading_angle << "</wpml:waypointHeadingAngle>\n";
|
|
ofile << " <wpml:waypointPoiPoint>" << poi_point[0] << "," << poi_point[1] << "," << poi_point[2] << "</wpml:waypointPoiPoint>\n";
|
|
ofile << " <wpml:waypointHeadingPoiIndex>" << poi_indx << "</wpml:waypointHeadingPoiIndex>\n";
|
|
ofile << " </wpml:globalWaypointHeadingParam>\n";
|
|
ofile << " <wpml:globalWaypointTurnMode>" << global_trun_mode << "</wpml:globalWaypointTurnMode>\n";
|
|
ofile << " <wpml:globalUseStraightLine>" << use_straight_line << "</wpml:globalUseStraightLine>\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"( </Folder>
|
|
</Document>
|
|
</kml>)";
|
|
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;
|
|
}
|