@lumikmz/kmz 0.2.0
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- package/LICENSE +21 -0
- package/README.md +540 -0
- package/dist/index.d.mts +789 -0
- package/dist/index.mjs +779 -0
- package/package.json +24 -0
- package/src/index.ts +3 -0
- package/src/plan/geometry.ts +109 -0
- package/src/plan/index.ts +31 -0
- package/src/plan/plan-mapping-strip.ts +84 -0
- package/src/plan/plan-mapping2d.ts +128 -0
- package/src/plan/plan-mapping3d.ts +188 -0
- package/src/plan/plan-waypoint.ts +80 -0
- package/src/plan/plan.test.ts +96 -0
- package/src/types/action.ts +253 -0
- package/src/types/branded.ts +20 -0
- package/src/types/drone-payload.ts +59 -0
- package/src/types/enums.ts +97 -0
- package/src/types/errors.ts +19 -0
- package/src/types/index.ts +10 -0
- package/src/types/mission-config.ts +58 -0
- package/src/types/placemark.ts +96 -0
- package/src/types/template.ts +170 -0
- package/src/types/waylines.ts +32 -0
- package/src/types/waypoint-params.ts +78 -0
- package/src/validate/index.ts +25 -0
- package/src/validate/validate-mission-config.ts +48 -0
- package/src/validate/validate-template.ts +101 -0
- package/src/validate/validate-waylines.ts +59 -0
- package/tsconfig.json +4 -0
- package/vite.config.ts +9 -0
package/package.json
ADDED
|
@@ -0,0 +1,24 @@
|
|
|
1
|
+
{
|
|
2
|
+
"name": "@lumikmz/kmz",
|
|
3
|
+
"version": "0.2.0",
|
|
4
|
+
"description": "DJI KMZ/WPML domain model + plan() algorithm + validate(), spec-aligned types mirroring docs/kmz/",
|
|
5
|
+
"license": "MIT",
|
|
6
|
+
"type": "module",
|
|
7
|
+
"main": "./dist/index.mjs",
|
|
8
|
+
"types": "./dist/index.d.mts",
|
|
9
|
+
"exports": {
|
|
10
|
+
".": {
|
|
11
|
+
"types": "./dist/index.d.mts",
|
|
12
|
+
"import": "./dist/index.mjs",
|
|
13
|
+
"default": "./dist/index.mjs"
|
|
14
|
+
},
|
|
15
|
+
"./package.json": "./package.json"
|
|
16
|
+
},
|
|
17
|
+
"devDependencies": {
|
|
18
|
+
"vite-plus": "latest",
|
|
19
|
+
"vitest": "npm:@voidzero-dev/vite-plus-test@^0.1.20"
|
|
20
|
+
},
|
|
21
|
+
"scripts": {
|
|
22
|
+
"build": "vp pack"
|
|
23
|
+
}
|
|
24
|
+
}
|
package/src/index.ts
ADDED
|
@@ -0,0 +1,109 @@
|
|
|
1
|
+
import type { LngLat } from "../types/branded.js";
|
|
2
|
+
|
|
3
|
+
const EARTH_RADIUS = 6_371_000;
|
|
4
|
+
const DEG2RAD = Math.PI / 180;
|
|
5
|
+
const RAD2DEG = 180 / Math.PI;
|
|
6
|
+
|
|
7
|
+
export type EnuPoint = readonly [number, number];
|
|
8
|
+
|
|
9
|
+
/** Project lng/lat to local ENU meters around `origin`. */
|
|
10
|
+
export function lngLatToEnu(point: LngLat, origin: LngLat): EnuPoint {
|
|
11
|
+
const dLat = (point.lat - origin.lat) * DEG2RAD;
|
|
12
|
+
const dLng = (point.lng - origin.lng) * DEG2RAD;
|
|
13
|
+
const avgLat = ((point.lat + origin.lat) / 2) * DEG2RAD;
|
|
14
|
+
return [dLng * EARTH_RADIUS * Math.cos(avgLat), dLat * EARTH_RADIUS];
|
|
15
|
+
}
|
|
16
|
+
|
|
17
|
+
/** Inverse of `lngLatToEnu`. */
|
|
18
|
+
export function enuToLngLat([x, y]: EnuPoint, origin: LngLat): LngLat {
|
|
19
|
+
const avgLat = origin.lat * DEG2RAD;
|
|
20
|
+
return {
|
|
21
|
+
lng: origin.lng + (x / (EARTH_RADIUS * Math.cos(avgLat))) * RAD2DEG,
|
|
22
|
+
lat: origin.lat + (y / EARTH_RADIUS) * RAD2DEG,
|
|
23
|
+
};
|
|
24
|
+
}
|
|
25
|
+
|
|
26
|
+
export function centroid(points: readonly LngLat[]): LngLat {
|
|
27
|
+
let sumLng = 0;
|
|
28
|
+
let sumLat = 0;
|
|
29
|
+
for (const p of points) {
|
|
30
|
+
sumLng += p.lng;
|
|
31
|
+
sumLat += p.lat;
|
|
32
|
+
}
|
|
33
|
+
return { lng: sumLng / points.length, lat: sumLat / points.length };
|
|
34
|
+
}
|
|
35
|
+
|
|
36
|
+
/**
|
|
37
|
+
* Bearing from `from` → `to` in degrees clockwise from North.
|
|
38
|
+
* ENU axes: x=east, y=north.
|
|
39
|
+
*/
|
|
40
|
+
export function bearingDeg(from: EnuPoint, to: EnuPoint): number {
|
|
41
|
+
return Math.atan2(to[0] - from[0], to[1] - from[1]) * RAD2DEG;
|
|
42
|
+
}
|
|
43
|
+
|
|
44
|
+
/** Clip a horizontal scan line `y = const` against a polygon; returns `[x_enter, x_exit]` pairs. */
|
|
45
|
+
function clipHorizontal(y: number, polygon: readonly EnuPoint[]): [number, number][] {
|
|
46
|
+
const intersections: number[] = [];
|
|
47
|
+
const n = polygon.length;
|
|
48
|
+
for (let i = 0; i < n; i++) {
|
|
49
|
+
const [x1, y1] = polygon[i]!;
|
|
50
|
+
const [x2, y2] = polygon[(i + 1) % n]!;
|
|
51
|
+
if ((y1 <= y && y < y2) || (y2 <= y && y < y1)) {
|
|
52
|
+
const t = (y - y1) / (y2 - y1);
|
|
53
|
+
intersections.push(x1 + t * (x2 - x1));
|
|
54
|
+
}
|
|
55
|
+
}
|
|
56
|
+
intersections.sort((a, b) => a - b);
|
|
57
|
+
const out: [number, number][] = [];
|
|
58
|
+
for (let i = 0; i + 1 < intersections.length; i += 2) {
|
|
59
|
+
out.push([intersections[i]!, intersections[i + 1]!]);
|
|
60
|
+
}
|
|
61
|
+
return out;
|
|
62
|
+
}
|
|
63
|
+
|
|
64
|
+
/** Serpentine boustrophedon scan over a polygon; returns ENU waypoints. */
|
|
65
|
+
export function scanWaypoints(polygon: readonly EnuPoint[], lineSpacing: number): EnuPoint[] {
|
|
66
|
+
let minY = Infinity;
|
|
67
|
+
let maxY = -Infinity;
|
|
68
|
+
for (const [, y] of polygon) {
|
|
69
|
+
if (y < minY) minY = y;
|
|
70
|
+
if (y > maxY) maxY = y;
|
|
71
|
+
}
|
|
72
|
+
|
|
73
|
+
const waypoints: EnuPoint[] = [];
|
|
74
|
+
let leftToRight = true;
|
|
75
|
+
for (let y = minY; y <= maxY + lineSpacing * 0.5; y += lineSpacing) {
|
|
76
|
+
const segments = clipHorizontal(y, polygon);
|
|
77
|
+
for (const [x1, x2] of segments) {
|
|
78
|
+
if (leftToRight) {
|
|
79
|
+
waypoints.push([x1, y], [x2, y]);
|
|
80
|
+
} else {
|
|
81
|
+
waypoints.push([x2, y], [x1, y]);
|
|
82
|
+
}
|
|
83
|
+
}
|
|
84
|
+
leftToRight = !leftToRight;
|
|
85
|
+
}
|
|
86
|
+
return waypoints;
|
|
87
|
+
}
|
|
88
|
+
|
|
89
|
+
/**
|
|
90
|
+
* Parse a KML coordinates string like `"lon,lat lon,lat lon,lat"` or `"lon,lat,alt ..."`
|
|
91
|
+
* into LngLat array (altitude dropped).
|
|
92
|
+
*/
|
|
93
|
+
export function parseCoordinatesString(s: string): LngLat[] {
|
|
94
|
+
return s
|
|
95
|
+
.trim()
|
|
96
|
+
.split(/\s+/)
|
|
97
|
+
.map((tuple) => {
|
|
98
|
+
const [lng, lat] = tuple.split(",").map(Number) as [number, number];
|
|
99
|
+
return { lng, lat };
|
|
100
|
+
});
|
|
101
|
+
}
|
|
102
|
+
|
|
103
|
+
/** Encode a polygon ring back to `"lon,lat,alt lon,lat,alt"` format (alt defaults to 0). */
|
|
104
|
+
export function encodePolygonCoordinates(
|
|
105
|
+
ring: readonly LngLat[],
|
|
106
|
+
altitudeMeters: number = 0,
|
|
107
|
+
): string {
|
|
108
|
+
return ring.map((p) => `${p.lng},${p.lat},${altitudeMeters}`).join(" ");
|
|
109
|
+
}
|
|
@@ -0,0 +1,31 @@
|
|
|
1
|
+
import { PlannerError } from "../types/errors.js";
|
|
2
|
+
import type {
|
|
3
|
+
Mapping2dTemplate,
|
|
4
|
+
Mapping3dTemplate,
|
|
5
|
+
MappingStripTemplate,
|
|
6
|
+
Template,
|
|
7
|
+
WaypointTemplate,
|
|
8
|
+
} from "../types/template.js";
|
|
9
|
+
import type { Waylines } from "../types/waylines.js";
|
|
10
|
+
import { planMapping2d } from "./plan-mapping2d.js";
|
|
11
|
+
import { planMapping3d } from "./plan-mapping3d.js";
|
|
12
|
+
import { planMappingStrip } from "./plan-mapping-strip.js";
|
|
13
|
+
import { planWaypoint } from "./plan-waypoint.js";
|
|
14
|
+
|
|
15
|
+
const isWaypoint = (t: Template): t is WaypointTemplate => t.Folder.templateType === "waypoint";
|
|
16
|
+
const isMapping2d = (t: Template): t is Mapping2dTemplate => t.Folder.templateType === "mapping2d";
|
|
17
|
+
const isMapping3d = (t: Template): t is Mapping3dTemplate => t.Folder.templateType === "mapping3d";
|
|
18
|
+
const isMappingStrip = (t: Template): t is MappingStripTemplate =>
|
|
19
|
+
t.Folder.templateType === "mappingStrip";
|
|
20
|
+
|
|
21
|
+
/**
|
|
22
|
+
* Compile a high-level `Template` into executable `Waylines`.
|
|
23
|
+
* Dispatch is by `template.Folder.templateType` per the DJI WPML spec.
|
|
24
|
+
*/
|
|
25
|
+
export function plan(template: Template): Waylines {
|
|
26
|
+
if (isWaypoint(template)) return planWaypoint(template);
|
|
27
|
+
if (isMapping2d(template)) return planMapping2d(template);
|
|
28
|
+
if (isMapping3d(template)) return planMapping3d(template);
|
|
29
|
+
if (isMappingStrip(template)) return planMappingStrip(template);
|
|
30
|
+
throw new PlannerError(`Unknown templateType: ${(template as Template).Folder.templateType}`);
|
|
31
|
+
}
|
|
@@ -0,0 +1,84 @@
|
|
|
1
|
+
import { degrees } from "../types/branded.js";
|
|
2
|
+
import type { ExecuteHeightMode } from "../types/enums.js";
|
|
3
|
+
import type { MappingStripFolder, MappingStripTemplate } from "../types/template.js";
|
|
4
|
+
import type { Waylines, WaylinesFolder } from "../types/waylines.js";
|
|
5
|
+
import type { WaylinesPlacemark } from "../types/placemark.js";
|
|
6
|
+
import { bearingDeg, centroid, lngLatToEnu } from "./geometry.js";
|
|
7
|
+
|
|
8
|
+
/**
|
|
9
|
+
* Compile a `mappingStrip` template into a single wayline that follows the
|
|
10
|
+
* supplied LineString. Each LineString vertex becomes one waypoint.
|
|
11
|
+
*
|
|
12
|
+
* When `stripUseTemplateAltitude` is true the altitude in each LineString
|
|
13
|
+
* triple is used as the executeHeight; otherwise the folder's `height` is used.
|
|
14
|
+
*/
|
|
15
|
+
export function planMappingStrip(template: MappingStripTemplate): Waylines {
|
|
16
|
+
const folder = template.Folder;
|
|
17
|
+
const coords = parseTriples(folder.Placemark.LineString.coordinates);
|
|
18
|
+
const origin = centroid(coords.map((c) => ({ lng: c.lng, lat: c.lat })));
|
|
19
|
+
const enuPath = coords.map((c) => lngLatToEnu({ lng: c.lng, lat: c.lat }, origin));
|
|
20
|
+
|
|
21
|
+
const placemarks: WaylinesPlacemark[] = coords.map((c, index) => {
|
|
22
|
+
const isLast = index === coords.length - 1;
|
|
23
|
+
const headingAngle = isLast ? 0 : bearingDeg(enuPath[index]!, enuPath[index + 1]!);
|
|
24
|
+
const executeHeight =
|
|
25
|
+
folder.stripUseTemplateAltitude && c.alt !== undefined
|
|
26
|
+
? (c.alt as ReturnType<typeof folder.height extends infer T ? () => T : never>)
|
|
27
|
+
: folder.height;
|
|
28
|
+
|
|
29
|
+
return {
|
|
30
|
+
Point: { coordinates: { lng: c.lng, lat: c.lat } },
|
|
31
|
+
index,
|
|
32
|
+
executeHeight,
|
|
33
|
+
waypointSpeed: folder.autoFlightSpeed,
|
|
34
|
+
waypointHeadingParam: {
|
|
35
|
+
waypointHeadingMode: "followWayline",
|
|
36
|
+
waypointHeadingAngle: degrees(headingAngle),
|
|
37
|
+
waypointHeadingPathMode: "followBadArc",
|
|
38
|
+
},
|
|
39
|
+
waypointTurnParam: { waypointTurnMode: "toPointAndPassWithContinuityCurvature" },
|
|
40
|
+
};
|
|
41
|
+
});
|
|
42
|
+
|
|
43
|
+
const waylineFolder: WaylinesFolder = {
|
|
44
|
+
templateId: folder.templateId,
|
|
45
|
+
waylineId: 0,
|
|
46
|
+
executeHeightMode: heightModeToExecuteMode(folder),
|
|
47
|
+
autoFlightSpeed: folder.autoFlightSpeed,
|
|
48
|
+
Placemark: placemarks,
|
|
49
|
+
};
|
|
50
|
+
|
|
51
|
+
return { missionConfig: template.missionConfig, Folder: [waylineFolder] };
|
|
52
|
+
}
|
|
53
|
+
|
|
54
|
+
interface Triple {
|
|
55
|
+
lng: number;
|
|
56
|
+
lat: number;
|
|
57
|
+
alt?: number;
|
|
58
|
+
}
|
|
59
|
+
|
|
60
|
+
function parseTriples(s: string): Triple[] {
|
|
61
|
+
return s
|
|
62
|
+
.trim()
|
|
63
|
+
.split(/\s+/)
|
|
64
|
+
.map((tuple) => {
|
|
65
|
+
const parts = tuple.split(",").map(Number);
|
|
66
|
+
return {
|
|
67
|
+
lng: parts[0]!,
|
|
68
|
+
lat: parts[1]!,
|
|
69
|
+
...(parts[2] !== undefined ? { alt: parts[2]! } : {}),
|
|
70
|
+
};
|
|
71
|
+
});
|
|
72
|
+
}
|
|
73
|
+
|
|
74
|
+
function heightModeToExecuteMode(folder: MappingStripFolder): ExecuteHeightMode {
|
|
75
|
+
switch (folder.waylineCoordinateSysParam.heightMode) {
|
|
76
|
+
case "EGM96":
|
|
77
|
+
return "WGS84";
|
|
78
|
+
case "relativeToStartPoint":
|
|
79
|
+
return "relativeToStartPoint";
|
|
80
|
+
case "aboveGroundLevel":
|
|
81
|
+
case "realTimeFollowSurface":
|
|
82
|
+
return "realTimeFollowSurface";
|
|
83
|
+
}
|
|
84
|
+
}
|
|
@@ -0,0 +1,128 @@
|
|
|
1
|
+
import { degrees, metersPerSecond } from "../types/branded.js";
|
|
2
|
+
import type { ActionGroup } from "../types/action.js";
|
|
3
|
+
import type { ExecuteHeightMode } from "../types/enums.js";
|
|
4
|
+
import type { Mapping2dTemplate, Mapping2dFolder } from "../types/template.js";
|
|
5
|
+
import type { Waylines, WaylinesFolder } from "../types/waylines.js";
|
|
6
|
+
import type { WaylinesPlacemark } from "../types/placemark.js";
|
|
7
|
+
import {
|
|
8
|
+
bearingDeg,
|
|
9
|
+
centroid,
|
|
10
|
+
enuToLngLat,
|
|
11
|
+
lngLatToEnu,
|
|
12
|
+
parseCoordinatesString,
|
|
13
|
+
scanWaypoints,
|
|
14
|
+
} from "./geometry.js";
|
|
15
|
+
|
|
16
|
+
/** Default HFOV (degrees) used to estimate ground footprint when payload-specific FOV unknown. */
|
|
17
|
+
const DEFAULT_HFOV_DEG = 84;
|
|
18
|
+
|
|
19
|
+
export function planMapping2d(template: Mapping2dTemplate): Waylines {
|
|
20
|
+
const folder = template.Folder;
|
|
21
|
+
const ring = parseCoordinatesString(
|
|
22
|
+
folder.Placemark.Polygon.outerBoundaryIs.LinearRing.coordinates,
|
|
23
|
+
);
|
|
24
|
+
const origin = centroid(ring);
|
|
25
|
+
const enuPolygon = ring.map((p) => lngLatToEnu(p, origin));
|
|
26
|
+
|
|
27
|
+
const hfovRad = (DEFAULT_HFOV_DEG / 2) * (Math.PI / 180);
|
|
28
|
+
const footprintWidth = 2 * folder.height * Math.tan(hfovRad);
|
|
29
|
+
const sideOverlap = folder.overlap.orthoCameraOverlapW ?? 70;
|
|
30
|
+
const lineSpacing = footprintWidth * (1 - sideOverlap / 100);
|
|
31
|
+
|
|
32
|
+
const enuWps = scanWaypoints(enuPolygon, lineSpacing);
|
|
33
|
+
const lngLatWps = enuWps.map((p) => enuToLngLat(p, origin));
|
|
34
|
+
|
|
35
|
+
let actionGroupId = 0;
|
|
36
|
+
const placemarks: WaylinesPlacemark[] = lngLatWps.map((coord, index) => {
|
|
37
|
+
const isLast = index === lngLatWps.length - 1;
|
|
38
|
+
const headingAngle = isLast ? 0 : bearingDeg(enuWps[index]!, enuWps[index + 1]!);
|
|
39
|
+
|
|
40
|
+
const actionGroup: ActionGroup[] = isLast
|
|
41
|
+
? []
|
|
42
|
+
: [
|
|
43
|
+
{
|
|
44
|
+
actionGroupId: actionGroupId++,
|
|
45
|
+
actionGroupStartIndex: index,
|
|
46
|
+
actionGroupEndIndex: index + 1,
|
|
47
|
+
actionGroupMode: "sequence",
|
|
48
|
+
actionTrigger: { actionTriggerType: "betweenAdjacentPoints" },
|
|
49
|
+
action: [
|
|
50
|
+
{
|
|
51
|
+
actionId: 0,
|
|
52
|
+
actionActuatorFunc: "takePhoto",
|
|
53
|
+
actionActuatorFuncParam: {
|
|
54
|
+
payloadPositionIndex: 0,
|
|
55
|
+
payloadLensIndex: ["wide"],
|
|
56
|
+
useGlobalPayloadLensIndex: false,
|
|
57
|
+
},
|
|
58
|
+
},
|
|
59
|
+
],
|
|
60
|
+
},
|
|
61
|
+
];
|
|
62
|
+
|
|
63
|
+
return {
|
|
64
|
+
Point: { coordinates: coord },
|
|
65
|
+
index,
|
|
66
|
+
executeHeight: folder.height,
|
|
67
|
+
waypointSpeed: folder.autoFlightSpeed,
|
|
68
|
+
waypointHeadingParam: {
|
|
69
|
+
waypointHeadingMode: "followWayline",
|
|
70
|
+
waypointHeadingAngle: degrees(headingAngle),
|
|
71
|
+
waypointHeadingPathMode: "followBadArc",
|
|
72
|
+
},
|
|
73
|
+
waypointTurnParam: { waypointTurnMode: "toPointAndPassWithContinuityCurvature" },
|
|
74
|
+
...(actionGroup.length > 0 ? { actionGroup } : {}),
|
|
75
|
+
};
|
|
76
|
+
});
|
|
77
|
+
|
|
78
|
+
const waylineFolder: WaylinesFolder = {
|
|
79
|
+
templateId: folder.templateId,
|
|
80
|
+
waylineId: 0,
|
|
81
|
+
executeHeightMode: heightModeToExecuteMode(folder),
|
|
82
|
+
autoFlightSpeed: folder.autoFlightSpeed,
|
|
83
|
+
startActionGroup: {
|
|
84
|
+
actionGroupId: 65535,
|
|
85
|
+
actionGroupStartIndex: 0,
|
|
86
|
+
actionGroupEndIndex: 0,
|
|
87
|
+
actionGroupMode: "sequence",
|
|
88
|
+
actionTrigger: { actionTriggerType: "reachPoint" },
|
|
89
|
+
action: [
|
|
90
|
+
{
|
|
91
|
+
actionId: 0,
|
|
92
|
+
actionActuatorFunc: "gimbalRotate",
|
|
93
|
+
actionActuatorFuncParam: {
|
|
94
|
+
payloadPositionIndex: 0,
|
|
95
|
+
gimbalHeadingYawBase: "north",
|
|
96
|
+
gimbalRotateMode: "absoluteAngle",
|
|
97
|
+
gimbalPitchRotateEnable: true,
|
|
98
|
+
gimbalPitchRotateAngle: degrees(-90),
|
|
99
|
+
gimbalRollRotateEnable: false,
|
|
100
|
+
gimbalRollRotateAngle: degrees(0),
|
|
101
|
+
gimbalYawRotateEnable: true,
|
|
102
|
+
gimbalYawRotateAngle: degrees(0),
|
|
103
|
+
gimbalRotateTimeEnable: false,
|
|
104
|
+
gimbalRotateTime: 10,
|
|
105
|
+
},
|
|
106
|
+
},
|
|
107
|
+
],
|
|
108
|
+
},
|
|
109
|
+
Placemark: placemarks,
|
|
110
|
+
};
|
|
111
|
+
|
|
112
|
+
return { missionConfig: template.missionConfig, Folder: [waylineFolder] };
|
|
113
|
+
}
|
|
114
|
+
|
|
115
|
+
function heightModeToExecuteMode(folder: Mapping2dFolder): ExecuteHeightMode {
|
|
116
|
+
switch (folder.waylineCoordinateSysParam.heightMode) {
|
|
117
|
+
case "EGM96":
|
|
118
|
+
return "WGS84";
|
|
119
|
+
case "relativeToStartPoint":
|
|
120
|
+
return "relativeToStartPoint";
|
|
121
|
+
case "aboveGroundLevel":
|
|
122
|
+
case "realTimeFollowSurface":
|
|
123
|
+
return "realTimeFollowSurface";
|
|
124
|
+
}
|
|
125
|
+
}
|
|
126
|
+
|
|
127
|
+
// Suppress unused import warnings for re-exports
|
|
128
|
+
void metersPerSecond;
|
|
@@ -0,0 +1,188 @@
|
|
|
1
|
+
import { degrees } from "../types/branded.js";
|
|
2
|
+
import type { Action, ActionGroup, GimbalRotateAction } from "../types/action.js";
|
|
3
|
+
import type { ExecuteHeightMode } from "../types/enums.js";
|
|
4
|
+
import type { Mapping3dFolder, Mapping3dTemplate } from "../types/template.js";
|
|
5
|
+
import type { Waylines, WaylinesFolder } from "../types/waylines.js";
|
|
6
|
+
import type { WaylinesPlacemark } from "../types/placemark.js";
|
|
7
|
+
import {
|
|
8
|
+
centroid,
|
|
9
|
+
enuToLngLat,
|
|
10
|
+
type EnuPoint,
|
|
11
|
+
lngLatToEnu,
|
|
12
|
+
parseCoordinatesString,
|
|
13
|
+
scanWaypoints,
|
|
14
|
+
} from "./geometry.js";
|
|
15
|
+
|
|
16
|
+
const DEFAULT_HFOV_DEG = 84;
|
|
17
|
+
|
|
18
|
+
/**
|
|
19
|
+
* Compile a `mapping3d` (oblique photography) template into 5 waylines:
|
|
20
|
+
* one nadir (ortho) pass + four oblique passes (front/back/left/right).
|
|
21
|
+
*/
|
|
22
|
+
export function planMapping3d(template: Mapping3dTemplate): Waylines {
|
|
23
|
+
const folder = template.Folder;
|
|
24
|
+
const ring = parseCoordinatesString(
|
|
25
|
+
folder.Placemark.Polygon.outerBoundaryIs.LinearRing.coordinates,
|
|
26
|
+
);
|
|
27
|
+
const origin = centroid(ring);
|
|
28
|
+
const enuPolygon = ring.map((p) => lngLatToEnu(p, origin));
|
|
29
|
+
|
|
30
|
+
const hfovRad = (DEFAULT_HFOV_DEG / 2) * (Math.PI / 180);
|
|
31
|
+
const footprintWidth = 2 * folder.height * Math.tan(hfovRad);
|
|
32
|
+
const sideOverlap = folder.overlap.orthoCameraOverlapW ?? 70;
|
|
33
|
+
const lineSpacing = footprintWidth * (1 - sideOverlap / 100);
|
|
34
|
+
|
|
35
|
+
const enuWps = scanWaypoints(enuPolygon, lineSpacing);
|
|
36
|
+
|
|
37
|
+
const passes = [
|
|
38
|
+
{ headingAngle: 0, gimbalPitch: -90, minShootInterval: 2.0 },
|
|
39
|
+
{ headingAngle: 0, gimbalPitch: folder.inclinedGimbalPitch, minShootInterval: 2.5 },
|
|
40
|
+
{ headingAngle: 180, gimbalPitch: folder.inclinedGimbalPitch, minShootInterval: 2.5 },
|
|
41
|
+
{ headingAngle: 90, gimbalPitch: folder.inclinedGimbalPitch, minShootInterval: 2.5 },
|
|
42
|
+
{ headingAngle: -90, gimbalPitch: folder.inclinedGimbalPitch, minShootInterval: 2.5 },
|
|
43
|
+
];
|
|
44
|
+
|
|
45
|
+
const Folder: WaylinesFolder[] = passes.map((pass, i) =>
|
|
46
|
+
buildObliquePass(
|
|
47
|
+
folder,
|
|
48
|
+
i,
|
|
49
|
+
origin,
|
|
50
|
+
enuWps,
|
|
51
|
+
pass.headingAngle,
|
|
52
|
+
pass.gimbalPitch,
|
|
53
|
+
pass.minShootInterval,
|
|
54
|
+
),
|
|
55
|
+
);
|
|
56
|
+
|
|
57
|
+
return { missionConfig: template.missionConfig, Folder };
|
|
58
|
+
}
|
|
59
|
+
|
|
60
|
+
function buildObliquePass(
|
|
61
|
+
folder: Mapping3dFolder,
|
|
62
|
+
waylineId: number,
|
|
63
|
+
origin: { lng: number; lat: number },
|
|
64
|
+
enuWps: readonly EnuPoint[],
|
|
65
|
+
headingAngle: number,
|
|
66
|
+
gimbalPitch: number,
|
|
67
|
+
minShootInterval: number,
|
|
68
|
+
): WaylinesFolder {
|
|
69
|
+
const lastIndex = enuWps.length - 1;
|
|
70
|
+
|
|
71
|
+
const placemarks: WaylinesPlacemark[] = enuWps.map((enu, index) => {
|
|
72
|
+
const coord = enuToLngLat(enu, origin);
|
|
73
|
+
const placemark: WaylinesPlacemark = {
|
|
74
|
+
Point: { coordinates: coord },
|
|
75
|
+
index,
|
|
76
|
+
executeHeight: folder.height,
|
|
77
|
+
waypointSpeed: folder.autoFlightSpeed,
|
|
78
|
+
waypointHeadingParam: {
|
|
79
|
+
waypointHeadingMode: "fixed",
|
|
80
|
+
waypointHeadingAngle: degrees(headingAngle),
|
|
81
|
+
waypointHeadingPathMode: "followBadArc",
|
|
82
|
+
},
|
|
83
|
+
waypointTurnParam: { waypointTurnMode: "toPointAndPassWithContinuityCurvature" },
|
|
84
|
+
};
|
|
85
|
+
|
|
86
|
+
const groups = waypointActionGroups(index, lastIndex, gimbalPitch, minShootInterval);
|
|
87
|
+
if (groups.length > 0) placemark.actionGroup = groups;
|
|
88
|
+
return placemark;
|
|
89
|
+
});
|
|
90
|
+
|
|
91
|
+
return {
|
|
92
|
+
templateId: folder.templateId,
|
|
93
|
+
waylineId,
|
|
94
|
+
executeHeightMode: heightModeToExecuteMode(folder.waylineCoordinateSysParam.heightMode),
|
|
95
|
+
autoFlightSpeed: folder.autoFlightSpeed,
|
|
96
|
+
startActionGroup: startGimbalRotateGroup(gimbalPitch),
|
|
97
|
+
Placemark: placemarks,
|
|
98
|
+
};
|
|
99
|
+
}
|
|
100
|
+
|
|
101
|
+
function waypointActionGroups(
|
|
102
|
+
index: number,
|
|
103
|
+
lastIndex: number,
|
|
104
|
+
gimbalPitch: number,
|
|
105
|
+
minShootInterval: number,
|
|
106
|
+
): ActionGroup[] {
|
|
107
|
+
if (index === 0) {
|
|
108
|
+
return [
|
|
109
|
+
{
|
|
110
|
+
actionGroupId: 0,
|
|
111
|
+
actionGroupStartIndex: 0,
|
|
112
|
+
actionGroupEndIndex: lastIndex,
|
|
113
|
+
actionGroupMode: "sequence",
|
|
114
|
+
actionTrigger: { actionTriggerType: "reachPoint" },
|
|
115
|
+
action: [gimbalRotate(0, gimbalPitch)],
|
|
116
|
+
},
|
|
117
|
+
{
|
|
118
|
+
actionGroupId: 1,
|
|
119
|
+
actionGroupStartIndex: 0,
|
|
120
|
+
actionGroupEndIndex: lastIndex,
|
|
121
|
+
actionGroupMode: "sequence",
|
|
122
|
+
actionTrigger: {
|
|
123
|
+
actionTriggerType: "multipleTiming",
|
|
124
|
+
actionTriggerParam: minShootInterval,
|
|
125
|
+
},
|
|
126
|
+
action: [takePhoto(0)],
|
|
127
|
+
},
|
|
128
|
+
];
|
|
129
|
+
}
|
|
130
|
+
return [];
|
|
131
|
+
}
|
|
132
|
+
|
|
133
|
+
function startGimbalRotateGroup(gimbalPitch: number): ActionGroup {
|
|
134
|
+
return {
|
|
135
|
+
actionGroupId: 65535,
|
|
136
|
+
actionGroupStartIndex: 0,
|
|
137
|
+
actionGroupEndIndex: 0,
|
|
138
|
+
actionGroupMode: "sequence",
|
|
139
|
+
actionTrigger: { actionTriggerType: "reachPoint" },
|
|
140
|
+
action: [gimbalRotate(0, gimbalPitch)],
|
|
141
|
+
};
|
|
142
|
+
}
|
|
143
|
+
|
|
144
|
+
function gimbalRotate(actionId: number, pitch: number): GimbalRotateAction {
|
|
145
|
+
return {
|
|
146
|
+
actionId,
|
|
147
|
+
actionActuatorFunc: "gimbalRotate",
|
|
148
|
+
actionActuatorFuncParam: {
|
|
149
|
+
payloadPositionIndex: 0,
|
|
150
|
+
gimbalHeadingYawBase: "north",
|
|
151
|
+
gimbalRotateMode: "absoluteAngle",
|
|
152
|
+
gimbalPitchRotateEnable: true,
|
|
153
|
+
gimbalPitchRotateAngle: degrees(pitch),
|
|
154
|
+
gimbalRollRotateEnable: false,
|
|
155
|
+
gimbalRollRotateAngle: degrees(0),
|
|
156
|
+
gimbalYawRotateEnable: false,
|
|
157
|
+
gimbalYawRotateAngle: degrees(0),
|
|
158
|
+
gimbalRotateTimeEnable: false,
|
|
159
|
+
gimbalRotateTime: 0,
|
|
160
|
+
},
|
|
161
|
+
};
|
|
162
|
+
}
|
|
163
|
+
|
|
164
|
+
function takePhoto(actionId: number): Action {
|
|
165
|
+
return {
|
|
166
|
+
actionId,
|
|
167
|
+
actionActuatorFunc: "takePhoto",
|
|
168
|
+
actionActuatorFuncParam: {
|
|
169
|
+
payloadPositionIndex: 0,
|
|
170
|
+
payloadLensIndex: ["wide"],
|
|
171
|
+
useGlobalPayloadLensIndex: false,
|
|
172
|
+
},
|
|
173
|
+
};
|
|
174
|
+
}
|
|
175
|
+
|
|
176
|
+
function heightModeToExecuteMode(
|
|
177
|
+
mode: Mapping3dFolder["waylineCoordinateSysParam"]["heightMode"],
|
|
178
|
+
): ExecuteHeightMode {
|
|
179
|
+
switch (mode) {
|
|
180
|
+
case "EGM96":
|
|
181
|
+
return "WGS84";
|
|
182
|
+
case "relativeToStartPoint":
|
|
183
|
+
return "relativeToStartPoint";
|
|
184
|
+
case "aboveGroundLevel":
|
|
185
|
+
case "realTimeFollowSurface":
|
|
186
|
+
return "realTimeFollowSurface";
|
|
187
|
+
}
|
|
188
|
+
}
|
|
@@ -0,0 +1,80 @@
|
|
|
1
|
+
import { degrees } from "../types/branded.js";
|
|
2
|
+
import type { ActionGroup } from "../types/action.js";
|
|
3
|
+
import type { ExecuteHeightMode } from "../types/enums.js";
|
|
4
|
+
import type { WaypointTemplate, WaypointFolder } from "../types/template.js";
|
|
5
|
+
import type { Waylines, WaylinesFolder } from "../types/waylines.js";
|
|
6
|
+
import type { WaylinesPlacemark } from "../types/placemark.js";
|
|
7
|
+
import type { WaypointHeadingParam, WaypointTurnParam } from "../types/waypoint-params.js";
|
|
8
|
+
|
|
9
|
+
/**
|
|
10
|
+
* Compile a `waypoint` template directly: each Placemark in the template becomes
|
|
11
|
+
* one waypoint in the wayline, resolving `useGlobalXxx` flags against folder defaults.
|
|
12
|
+
*/
|
|
13
|
+
export function planWaypoint(template: WaypointTemplate): Waylines {
|
|
14
|
+
const folder: WaypointFolder = template.Folder;
|
|
15
|
+
|
|
16
|
+
const placemarks: WaylinesPlacemark[] = folder.Placemark.map((p, i) => {
|
|
17
|
+
const heading: WaypointHeadingParam =
|
|
18
|
+
p.useGlobalHeadingParam || p.waypointHeadingParam === undefined
|
|
19
|
+
? folder.globalWaypointHeadingParam
|
|
20
|
+
: p.waypointHeadingParam;
|
|
21
|
+
|
|
22
|
+
const turn: WaypointTurnParam =
|
|
23
|
+
p.useGlobalTurnParam || p.waypointTurnParam === undefined
|
|
24
|
+
? { waypointTurnMode: folder.globalWaypointTurnMode }
|
|
25
|
+
: p.waypointTurnParam;
|
|
26
|
+
|
|
27
|
+
const executeHeight =
|
|
28
|
+
p.useGlobalHeight || p.height === undefined ? folder.globalHeight : p.height;
|
|
29
|
+
|
|
30
|
+
const waypointSpeed =
|
|
31
|
+
p.useGlobalSpeed || p.waypointSpeed === undefined ? folder.autoFlightSpeed : p.waypointSpeed;
|
|
32
|
+
|
|
33
|
+
const placemark: WaylinesPlacemark = {
|
|
34
|
+
Point: p.Point,
|
|
35
|
+
index: i,
|
|
36
|
+
executeHeight,
|
|
37
|
+
waypointSpeed,
|
|
38
|
+
waypointHeadingParam: heading,
|
|
39
|
+
waypointTurnParam: turn,
|
|
40
|
+
...(p.useStraightLine !== undefined ? { useStraightLine: p.useStraightLine } : {}),
|
|
41
|
+
...(p.isRisky !== undefined ? { isRisky: p.isRisky } : {}),
|
|
42
|
+
...(p.actionGroup !== undefined ? { actionGroup: p.actionGroup } : {}),
|
|
43
|
+
};
|
|
44
|
+
return placemark;
|
|
45
|
+
});
|
|
46
|
+
|
|
47
|
+
const waylineFolder: WaylinesFolder = {
|
|
48
|
+
templateId: folder.templateId,
|
|
49
|
+
waylineId: 0,
|
|
50
|
+
executeHeightMode: heightModeToExecuteMode(folder.waylineCoordinateSysParam.heightMode),
|
|
51
|
+
autoFlightSpeed: folder.autoFlightSpeed,
|
|
52
|
+
Placemark: placemarks,
|
|
53
|
+
};
|
|
54
|
+
|
|
55
|
+
return { missionConfig: template.missionConfig, Folder: [waylineFolder] };
|
|
56
|
+
}
|
|
57
|
+
|
|
58
|
+
/**
|
|
59
|
+
* Map a template heightMode to a waylines executeHeightMode.
|
|
60
|
+
* - `EGM96` → `WGS84` (waylines spec doesn't expose EGM96; runtime resolves it).
|
|
61
|
+
* - `aboveGroundLevel` → `realTimeFollowSurface` (closest executable analog).
|
|
62
|
+
*/
|
|
63
|
+
function heightModeToExecuteMode(
|
|
64
|
+
mode: WaypointFolder["waylineCoordinateSysParam"]["heightMode"],
|
|
65
|
+
): ExecuteHeightMode {
|
|
66
|
+
switch (mode) {
|
|
67
|
+
case "EGM96":
|
|
68
|
+
return "WGS84";
|
|
69
|
+
case "relativeToStartPoint":
|
|
70
|
+
return "relativeToStartPoint";
|
|
71
|
+
case "aboveGroundLevel":
|
|
72
|
+
case "realTimeFollowSurface":
|
|
73
|
+
return "realTimeFollowSurface";
|
|
74
|
+
}
|
|
75
|
+
}
|
|
76
|
+
|
|
77
|
+
// Re-export degrees so dependent algorithms can use it consistently.
|
|
78
|
+
export { degrees };
|
|
79
|
+
// Re-export ActionGroup so we don't need to import it across plan files.
|
|
80
|
+
export type { ActionGroup };
|