diff --git a/Paths/KlingenbachTest.wima b/Paths/KlingenbachTest.wima new file mode 100644 index 0000000000000000000000000000000000000000..72a54bed289fd60461d25e37f97ba38ebda7539c --- /dev/null +++ b/Paths/KlingenbachTest.wima @@ -0,0 +1,1111 @@ +{ + "AreaItems": [ + { + "AreaType": "Measurement Area", + "BorderPolygonOffset": 6, + "BottomLayerAltitude": 5, + "LayerDistance": 1, + "NumberOfLayers": 1, + "ShowBorderPolygon": 1, + "maxAltitude": 30, + "polygon": [ + [ + 47.768115102959555, + 16.530342246669797 + ], + [ + 47.76836640344051, + 16.530980102821747 + ], + [ + 47.76798694569257, + 16.531146687122508 + ], + [ + 47.76777418124312, + 16.530481637145055 + ] + ] + }, + { + "AreaType": "Service Area", + "BorderPolygonOffset": 6, + "ShowBorderPolygon": 0, + "maxAltitude": 30, + "polygon": [ + [ + 47.76777317830783, + 16.530413969584316 + ], + [ + 47.76785089069791, + 16.530667046992818 + ], + [ + 47.76775614544286, + 16.5307494063876 + ], + [ + 47.76766140001518, + 16.530447230100037 + ] + ] + } + ], + "MissionItems": { + "fileType": "Plan", + "geoFence": { + "circles": [ + ], + "polygons": [ + ], + "version": 2 + }, + "groundStation": "QGroundControl", + "mission": { + "cruiseSpeed": 15, + "firmwareType": 3, + "hoverSpeed": 1, + "items": [ + { + "autoContinue": true, + "command": 22, + "doJumpId": 1, + "frame": 3, + "params": [ + 15, + 0, + 0, + null, + 47.76776040370392, + 16.530569413178814, + 15 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 15, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 2, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76783724337357, + 16.530679065020234, + 15 + ], + "type": "SimpleItem" + }, + { + "TransectStyleComplexItem": { + "CameraCalc": { + "AdjustedFootprintFrontal": 25, + "AdjustedFootprintSide": 25, + "CameraName": "Manual (no camera specs)", + "DistanceToSurface": 15, + "DistanceToSurfaceRelative": true, + "version": 1 + }, + "CameraShots": 0, + "CameraTriggerInTurnAround": true, + "FollowTerrain": false, + "HoverAndCapture": false, + "Items": [ + { + "autoContinue": true, + "command": 16, + "doJumpId": 3, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76806730587214, + 16.53111140850613, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 4, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76799491160373, + 16.531015220669012, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 5, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76791720460454, + 16.530928693424983, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 6, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.767880803232615, + 16.53081491158012, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 7, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76796453066004, + 16.530896418037717, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 8, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768043261182015, + 16.530988288482693, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 9, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811641683406, + 16.531089848489778, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 10, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768164991593466, + 16.531068523869646, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 11, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76809332863841, + 16.53096470827869, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 12, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76801615908482, + 16.530870032021927, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 13, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76793400789035, + 16.530785139149, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 14, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76784743390098, + 16.530710607156596, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 15, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76781621267059, + 16.530613017175664, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 16, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76790432462996, + 16.53068068570622, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 17, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76798857940534, + 16.530758471720823, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 18, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76806845322607, + 16.53084589166193, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 19, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76814344955573, + 16.530942402082584, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 20, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768213102178876, + 16.531047403024775, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 21, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76826080774556, + 16.53102645998563, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 22, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76819322114901, + 16.53092073665582, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 23, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76812047969944, + 16.530822878314783, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 24, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768042995738575, + 16.5307334396817, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 25, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76796120849152, + 16.530652927747852, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 26, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.767875581576945, + 16.530581798902674, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 27, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76778660037905, + 16.5305204563467, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 28, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76781420191909, + 16.530465274147996, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 29, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76790732788206, + 16.53053097339251, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 30, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76799680624691, + 16.530607070163562, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 31, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768082113528585, + 16.530693119263905, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 32, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76816275064456, + 16.530788617271075, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 33, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768238245834695, + 16.530893005482643, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 34, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76830815742123, + 16.531005673184843, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 35, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76835519240977, + 16.530985024533656, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 36, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.7682740802549, + 16.530851859564116, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 37, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768185515888376, + 16.53072965466372, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 38, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76809016582257, + 16.530619329514728, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 39, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76798874763725, + 16.530521714395498, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 40, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76788202457927, + 16.530437543931995, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 41, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76794696095184, + 16.530410993813895, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 42, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76803801984642, + 16.53048783494969, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 43, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76812507788592, + 16.53057433311072, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 44, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768207673228986, + 16.5306700294258, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 45, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.7682853677088, + 16.53077441622769, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 46, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768212497930946, + 16.530589456629045, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 47, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811387228977, + 16.530481178891698, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 48, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76800957067493, + 16.530385394978044, + 15 + ], + "type": "SimpleItem" + } + ], + "Refly90Degrees": false, + "TurnAroundDistance": 10, + "VisualTransectPoints": [ + [ + 47.76806730587214, + 16.53111140850613 + ], + [ + 47.76799491160373, + 16.531015220669012 + ], + [ + 47.76791720460454, + 16.530928693424983 + ], + [ + 47.767880803232615, + 16.53081491158012 + ], + [ + 47.76796453066004, + 16.530896418037717 + ], + [ + 47.768043261182015, + 16.530988288482693 + ], + [ + 47.76811641683406, + 16.531089848489778 + ], + [ + 47.768164991593466, + 16.531068523869646 + ], + [ + 47.76809332863841, + 16.53096470827869 + ], + [ + 47.76801615908482, + 16.530870032021927 + ], + [ + 47.76793400789035, + 16.530785139149 + ], + [ + 47.76784743390098, + 16.530710607156596 + ], + [ + 47.76781621267059, + 16.530613017175664 + ], + [ + 47.76790432462996, + 16.53068068570622 + ], + [ + 47.76798857940534, + 16.530758471720823 + ], + [ + 47.76806845322607, + 16.53084589166193 + ], + [ + 47.76814344955573, + 16.530942402082584 + ], + [ + 47.768213102178876, + 16.531047403024775 + ], + [ + 47.76826080774556, + 16.53102645998563 + ], + [ + 47.76819322114901, + 16.53092073665582 + ], + [ + 47.76812047969944, + 16.530822878314783 + ], + [ + 47.768042995738575, + 16.5307334396817 + ], + [ + 47.76796120849152, + 16.530652927747852 + ], + [ + 47.767875581576945, + 16.530581798902674 + ], + [ + 47.76778660037905, + 16.5305204563467 + ], + [ + 47.76781420191909, + 16.530465274147996 + ], + [ + 47.76790732788206, + 16.53053097339251 + ], + [ + 47.76799680624691, + 16.530607070163562 + ], + [ + 47.768082113528585, + 16.530693119263905 + ], + [ + 47.76816275064456, + 16.530788617271075 + ], + [ + 47.768238245834695, + 16.530893005482643 + ], + [ + 47.76830815742123, + 16.531005673184843 + ], + [ + 47.76835519240977, + 16.530985024533656 + ], + [ + 47.7682740802549, + 16.530851859564116 + ], + [ + 47.768185515888376, + 16.53072965466372 + ], + [ + 47.76809016582257, + 16.530619329514728 + ], + [ + 47.76798874763725, + 16.530521714395498 + ], + [ + 47.76788202457927, + 16.530437543931995 + ], + [ + 47.76794696095184, + 16.530410993813895 + ], + [ + 47.76803801984642, + 16.53048783494969 + ], + [ + 47.76812507788592, + 16.53057433311072 + ], + [ + 47.768207673228986, + 16.5306700294258 + ], + [ + 47.7682853677088, + 16.53077441622769 + ], + [ + 47.768212497930946, + 16.530589456629045 + ], + [ + 47.76811387228977, + 16.530481178891698 + ], + [ + 47.76800957067493, + 16.530385394978044 + ] + ], + "version": 1 + }, + "complexItemType": "circularSurvey", + "deltaAlpha": 5, + "deltaR": 5, + "isSnakePath": true, + "polygon": [ + [ + 47.768115102959555, + 16.530342246669797 + ], + [ + 47.76836640344051, + 16.530980102821747 + ], + [ + 47.76798694569257, + 16.531146687122508 + ], + [ + 47.76777418124312, + 16.530481637145055 + ] + ], + "referencePointAlt": 0, + "referencePointLat": 47.76728385046134, + "referencePointLong": 16.532308204002987, + "transectMinLength": 15, + "type": "ComplexItem", + "version": 1 + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 0, + "AltitudeMode": 1, + "autoContinue": true, + "command": 21, + "doJumpId": 60, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76776040370392, + 16.530569413178814, + 0 + ], + "type": "SimpleItem" + } + ], + "plannedHomePosition": [ + 47.76776040370392, + 16.530569413178814, + 178 + ], + "vehicleType": 2, + "version": 2 + }, + "rallyPoints": { + "points": [ + ], + "version": 2 + }, + "version": 1 + } +} diff --git a/Paths/ttt.plan b/Paths/ttt.plan new file mode 100644 index 0000000000000000000000000000000000000000..2c113f377db18b677ba90a5c589c0d588061f377 --- /dev/null +++ b/Paths/ttt.plan @@ -0,0 +1,1182 @@ +{ + "fileType": "Plan", + "geoFence": { + "circles": [ + ], + "polygons": [ + ], + "version": 2 + }, + "groundStation": "QGroundControl", + "mission": { + "cruiseSpeed": 15, + "firmwareType": 3, + "hoverSpeed": 1, + "items": [ + { + "TransectStyleComplexItem": { + "CameraCalc": { + "AdjustedFootprintFrontal": 25, + "AdjustedFootprintSide": 25, + "CameraName": "Manual (no camera specs)", + "DistanceToSurface": 15, + "DistanceToSurfaceRelative": true, + "version": 1 + }, + "CameraShots": 0, + "CameraTriggerInTurnAround": true, + "FollowTerrain": false, + "HoverAndCapture": false, + "Items": [ + { + "autoContinue": true, + "command": 16, + "doJumpId": 1, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76830996168149, + 16.531078080607077, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 2, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76829804974075, + 16.53102392492446, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 3, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76828308011502, + 16.530971475941232, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 4, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76826516164709, + 16.530921115009285, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 5, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76824442462058, + 16.530873208298473, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 6, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76822101981255, + 16.53082810413424, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 7, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768195117397276, + 16.530786130464946, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 8, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76816690570892, + 16.53074759247741, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 9, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76813658987215, + 16.53071277037791, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 10, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76810439031073, + 16.53068191735484, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 11, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76807054114481, + 16.530655257737784, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 12, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768035288488655, + 16.530632985366445, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 13, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.7679988886612, + 16.530615262181257, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 14, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76796160632235, + 16.530602217045924, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 15, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76792371254868, + 16.530593944810462, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 16, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76788548286245, + 16.53059050562156, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 17, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76789401336053, + 16.530457125409807, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 18, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76794037215665, + 16.530462221653437, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 19, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76798626339304, + 16.530473241938182, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 20, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768031345720765, + 16.530490104292777, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 21, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76807528380773, + 16.530512683291395, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 22, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811775083296, + 16.530540810986558, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 23, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768158430917545, + 16.530574278158397, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 24, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76819702147422, + 16.53061283587086, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 25, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.7682332354581, + 16.53065619732335, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 26, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76826680350174, + 16.530704039984016, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 27, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768297475918786, + 16.530756007988817, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 28, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76832502456117, + 16.530811714788502, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 29, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768349244516145, + 16.530870746023865, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 30, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76836995563045, + 16.530932662607825, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 31, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76838700385034, + 16.530997003991452, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 32, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76840026236748, + 16.531063291589636, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 33, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76843011128268, + 16.530812190772934, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 34, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76840412057789, + 16.53074736405803, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 35, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76837470177482, + 16.53068586573183, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 36, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76834204638334, + 16.530628096134794, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 37, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76830636698282, + 16.530574431334184, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 38, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768267895838235, + 16.530525220675944, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 39, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.7682268833882, + 16.530480784510544, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 40, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76818359661467, + 16.53044141210756, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 41, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76813831730493, + 16.530407359772603, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 42, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76809134021725, + 16.530378849178813, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 43, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76804297116204, + 16.530356065923836, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 44, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76799352501113, + 16.530339158321603, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 45, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76794332364798, + 16.53032823643686, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 46, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768111593768836, + 16.530247756350924, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 47, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76816244597949, + 16.530277077539925, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 48, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76821165723565, + 16.530312051720227, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 49, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76825894412527, + 16.530352477472384, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 50, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76830403431889, + 16.530398121980845, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 51, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76834666813801, + 16.530448722374775, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 52, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76838660005064, + 16.53050398724195, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 53, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76835630723104, + 16.53028941381972, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 54, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76830077310924, + 16.530233806252745, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 55, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.7682423373786, + 16.530185224411827, + 15 + ], + "type": "SimpleItem" + } + ], + "Refly90Degrees": false, + "TurnAroundDistance": 10, + "VisualTransectPoints": [ + [ + 47.76830996168149, + 16.531078080607077 + ], + [ + 47.76829804974075, + 16.53102392492446 + ], + [ + 47.76828308011502, + 16.530971475941232 + ], + [ + 47.76826516164709, + 16.530921115009285 + ], + [ + 47.76824442462058, + 16.530873208298473 + ], + [ + 47.76822101981255, + 16.53082810413424 + ], + [ + 47.768195117397276, + 16.530786130464946 + ], + [ + 47.76816690570892, + 16.53074759247741 + ], + [ + 47.76813658987215, + 16.53071277037791 + ], + [ + 47.76810439031073, + 16.53068191735484 + ], + [ + 47.76807054114481, + 16.530655257737784 + ], + [ + 47.768035288488655, + 16.530632985366445 + ], + [ + 47.7679988886612, + 16.530615262181257 + ], + [ + 47.76796160632235, + 16.530602217045924 + ], + [ + 47.76792371254868, + 16.530593944810462 + ], + [ + 47.76788548286245, + 16.53059050562156 + ], + [ + 47.76789401336053, + 16.530457125409807 + ], + [ + 47.76794037215665, + 16.530462221653437 + ], + [ + 47.76798626339304, + 16.530473241938182 + ], + [ + 47.768031345720765, + 16.530490104292777 + ], + [ + 47.76807528380773, + 16.530512683291395 + ], + [ + 47.76811775083296, + 16.530540810986558 + ], + [ + 47.768158430917545, + 16.530574278158397 + ], + [ + 47.76819702147422, + 16.53061283587086 + ], + [ + 47.7682332354581, + 16.53065619732335 + ], + [ + 47.76826680350174, + 16.530704039984016 + ], + [ + 47.768297475918786, + 16.530756007988817 + ], + [ + 47.76832502456117, + 16.530811714788502 + ], + [ + 47.768349244516145, + 16.530870746023865 + ], + [ + 47.76836995563045, + 16.530932662607825 + ], + [ + 47.76838700385034, + 16.530997003991452 + ], + [ + 47.76840026236748, + 16.531063291589636 + ], + [ + 47.76843011128268, + 16.530812190772934 + ], + [ + 47.76840412057789, + 16.53074736405803 + ], + [ + 47.76837470177482, + 16.53068586573183 + ], + [ + 47.76834204638334, + 16.530628096134794 + ], + [ + 47.76830636698282, + 16.530574431334184 + ], + [ + 47.768267895838235, + 16.530525220675944 + ], + [ + 47.7682268833882, + 16.530480784510544 + ], + [ + 47.76818359661467, + 16.53044141210756 + ], + [ + 47.76813831730493, + 16.530407359772603 + ], + [ + 47.76809134021725, + 16.530378849178813 + ], + [ + 47.76804297116204, + 16.530356065923836 + ], + [ + 47.76799352501113, + 16.530339158321603 + ], + [ + 47.76794332364798, + 16.53032823643686 + ], + [ + 47.768111593768836, + 16.530247756350924 + ], + [ + 47.76816244597949, + 16.530277077539925 + ], + [ + 47.76821165723565, + 16.530312051720227 + ], + [ + 47.76825894412527, + 16.530352477472384 + ], + [ + 47.76830403431889, + 16.530398121980845 + ], + [ + 47.76834666813801, + 16.530448722374775 + ], + [ + 47.76838660005064, + 16.53050398724195 + ], + [ + 47.76835630723104, + 16.53028941381972 + ], + [ + 47.76830077310924, + 16.530233806252745 + ], + [ + 47.7682423373786, + 16.530185224411827 + ] + ], + "version": 1 + }, + "complexItemType": "circularSurvey", + "deltaAlpha": 5, + "deltaR": 10, + "isSnakePath": true, + "polygon": [ + [ + 47.76833531973578, + 16.530140752891526 + ], + [ + 47.76846408526058, + 16.53105283897935 + ], + [ + 47.76784947606115, + 16.53115349675477 + ], + [ + 47.76790096078066, + 16.530348497711913 + ] + ], + "referencePointAlt": 0, + "referencePointLat": 47.76787751303391, + "referencePointLong": 16.531258654708665, + "transectMinLength": 0.3, + "type": "ComplexItem", + "version": 1 + } + ], + "plannedHomePosition": [ + 47.76825152951995, + 16.530647241895288, + 178 + ], + "vehicleType": 2, + "version": 2 + }, + "rallyPoints": { + "points": [ + ], + "version": 2 + }, + "version": 1 +} diff --git a/src/FlightDisplay/FlightDisplayWimaMenu.qml b/src/FlightDisplay/FlightDisplayWimaMenu.qml index 0c0670e5c84bcb5bd234e750cbab1343618ace08..23243c39293dd3c2ea9b31f407380805659d9357 100644 --- a/src/FlightDisplay/FlightDisplayWimaMenu.qml +++ b/src/FlightDisplay/FlightDisplayWimaMenu.qml @@ -19,7 +19,7 @@ import QGroundControl.FactControls 1.0 Item { id: _root - height: 500 + height: 700 width: 300 property var wimaController // must be provided by the user @@ -39,6 +39,15 @@ Item { } } + function getTime(time) { + // time is a double + if(isNaN(time)) { + return "00:00:00" + } + var t = new Date(0, 0, 0, 0, 0, Number(time)) + return Qt.formatTime(t, 'hh:mm:ss') + } + // box containing all items Rectangle { anchors.left: parent.left @@ -139,6 +148,18 @@ Item { anchors.right: parent.right visible: missionHeader.checked + QGCLabel { text: qsTr("Speed") } + FactTextField { + fact: wimaController.flightSpeed + Layout.fillWidth: true + } + + QGCLabel { text: qsTr("Altitude") } + FactTextField { + fact: wimaController.altitude + Layout.fillWidth: true + } + // Buttons QGCButton { id: buttonPreviousMissionPhase @@ -228,6 +249,22 @@ Item { wrapMode: Text.WordWrap font.pointSize: ScreenTools.smallFontPointSize } + QGCLabel { + text: wimaController.phaseDistance >= 0 ? wimaController.phaseDistance.toFixed(2) + " m": "" + wrapMode: Text.WordWrap + font.pointSize: ScreenTools.smallFontPointSize + } + + QGCLabel { + text: qsTr("Phase Duration: ") + wrapMode: Text.WordWrap + font.pointSize: ScreenTools.smallFontPointSize + } + QGCLabel { + text: wimaController.phaseDuration >= 0 ? getTime(wimaController.phaseDuration) : "" + wrapMode: Text.WordWrap + font.pointSize: ScreenTools.smallFontPointSize + } } } } diff --git a/src/Wima/Circle.cc b/src/Wima/Circle.cc index b12319ab2806a8d5ef72ce7bf7b1771e54a33837..5e13522049c4897737fd38fa315b3353ae0db9dd 100644 --- a/src/Wima/Circle.cc +++ b/src/Wima/Circle.cc @@ -107,9 +107,11 @@ QList Circle::approximate(double angleDiscretisation) const QList Circle::approximateSektor(int numberOfCorners, double alpha1, double alpha2) const { - if ( numberOfCorners < 2) + if ( numberOfCorners < 2) { + qWarning("numberOfCorners < 2"); return QList(); - return approximateSektor((alpha2-alpha1)/double(numberOfCorners-1), alpha1, alpha2); + } + return approximateSektor(PlanimetryCalculus::truncateAngle(alpha2-alpha1)/double(numberOfCorners-1), alpha1, alpha2); } QList Circle::approximateSektor(double angleDiscretisation, double alpha1, double alpha2) const diff --git a/src/Wima/CircularSurveyComplexItem.cc b/src/Wima/CircularSurveyComplexItem.cc index 41ca97fa7a332f1eb5d15eedd0acab9c97282826..fcdeb1ccc2d4c9a2b1e34d48ea0d9b06c25f5062 100644 --- a/src/Wima/CircularSurveyComplexItem.cc +++ b/src/Wima/CircularSurveyComplexItem.cc @@ -27,7 +27,8 @@ CircularSurveyComplexItem::CircularSurveyComplexItem(Vehicle *vehicle, bool flyV , _deltaAlpha (settingsGroup, _metaDataMap[deltaAlphaName]) , _transectMinLength (settingsGroup, _metaDataMap[transectMinLengthName]) , _isSnakePath (settingsGroup, _metaDataMap[isSnakePathName]) - , _autoGenerated (false) + , _isInitialized (false) + , _updateCounter (0) { _editorQml = "qrc:/qml/CircularSurveyItemEditor.qml"; connect(&_deltaR, &Fact::valueChanged, this, &CircularSurveyComplexItem::_rebuildTransects); @@ -53,12 +54,12 @@ void CircularSurveyComplexItem::setRefPoint(const QGeoCoordinate &refPt) } } -void CircularSurveyComplexItem::setAutoGenerated(bool autoGen) +void CircularSurveyComplexItem::setIsInitialized(bool isInitialized) { - if (autoGen != _autoGenerated) { - _autoGenerated = autoGen; + if (isInitialized != _isInitialized) { + _isInitialized = isInitialized; - emit autoGeneratedChanged(); + emit isInitializedChanged(); } } @@ -77,9 +78,9 @@ Fact *CircularSurveyComplexItem::deltaAlpha() return &_deltaAlpha; } -bool CircularSurveyComplexItem::autoGenerated() +bool CircularSurveyComplexItem::isInitialized() { - return _autoGenerated; + return _isInitialized; } bool CircularSurveyComplexItem::load(const QJsonObject &complexObject, int sequenceNumber, QString &errorString) @@ -142,7 +143,7 @@ bool CircularSurveyComplexItem::load(const QJsonObject &complexObject, int seque _referencePoint.setLatitude (complexObject[jsonReferencePointLatKey].toDouble()); _referencePoint.setAltitude (complexObject[jsonReferencePointAltKey].toDouble()); _isSnakePath.setRawValue (complexObject[jsonIsSnakePathKey].toBool()); - _autoGenerated = true; + setIsInitialized(true); _ignoreRecalc = false; @@ -349,6 +350,11 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1() using namespace PolygonCalculus; using namespace PlanimetryCalculus; + if (!_isInitialized) + return; + + _updateCounter++; + // If the transects are getting rebuilt then any previously loaded mission items are now invalid if (_loadedMissionItemsParent) { _loadedMissionItems.clear(); @@ -437,20 +443,31 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1() QPointFList exitPoints; // determine entryPoints and exit Points for (int j = 0; j < intersectPoints.size(); j++) { - QList intersects = intersectPoints[j]; + QList intersects = intersectPoints[j]; // one pt = tangent, two pt = sekant QPointF p1 = surveyPolygon[neighbourList[j].first]; QPointF p2 = surveyPolygon[neighbourList[j].second]; QLineF intersetLine(p1, p2); - double lineAngle = truncateAngle(angle(intersetLine)); + double lineAngle = angle(intersetLine); + +// int n = 16; +// for (int i = -n; i <= n; i++) { +// double alpha = 2*M_PI*double(i)/double(n); +// qDebug() << i << " " << alpha << " " << truncateAngle(alpha); +// } for (QPointF ipt : intersects) { - double circleTangentAngle = truncateAngle(angle(ipt)+M_PI_2); + double circleTangentAngle = angle(ipt)+M_PI_2; // compare line angle and circle tangent at intersection point // to determine between exit and entry point - if ( !qFuzzyCompare(lineAngle, circleTangentAngle) - && !qFuzzyCompare(lineAngle, truncateAngle(circleTangentAngle + M_PI))) { - if (truncateAngle(lineAngle - circleTangentAngle) < M_PI) { +// qDebug() << "lineAngle" << lineAngle*180/M_PI; +// qDebug() << "circleTangentAngle" << circleTangentAngle*180/M_PI; +// qDebug() << "!qFuzzyIsNull(truncateAngle(lineAngle - circleTangentAngle): " << !qFuzzyIsNull(truncateAngle(lineAngle - circleTangentAngle)); +// qDebug() << "!qFuzzyIsNull(truncateAngle(lineAngle - circleTangentAngle - M_PI): " << !qFuzzyIsNull(truncateAngle(lineAngle - circleTangentAngle - M_PI)); + if ( !qFuzzyIsNull(truncateAngle(lineAngle - circleTangentAngle)) + && !qFuzzyIsNull(truncateAngle(lineAngle - circleTangentAngle - M_PI) )) + { + if (truncateAngle(circleTangentAngle - lineAngle) > M_PI) { entryPoints.append(ipt); } else { exitPoints.append(ipt); @@ -479,11 +496,16 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1() } } + // generate circle sectors for (int k = 0; k < entryPoints.size(); k++) { double alpha1 = angle(entryPoints[k]); double alpha2 = angle(exitPoints[(k+offset) % entryPoints.size()]); - double dAlpha = alpha2-alpha1; + double dAlpha = truncateAngle(alpha2-alpha1); int numNodes = int(ceil(dAlpha/dalpha)) + 1; +// qDebug() << "alpha1" << alpha1; +// qDebug() << "alpha2" << alpha2; +// qDebug() << "dAlpha" << dAlpha; +// qDebug() << "numNodes" << numNodes; QList sectorPath = circle.approximateSektor(numNodes, alpha1, alpha2); // use shortestPath() here if necessary, could be a problem if dr >> @@ -568,6 +590,8 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1() _transects.append(transectList); } + qDebug() << "CircularSurveyComplexItem::_rebuildTransectsPhase1(): " << _updateCounter; + } void CircularSurveyComplexItem::_recalcComplexDistance() diff --git a/src/Wima/CircularSurveyComplexItem.h b/src/Wima/CircularSurveyComplexItem.h index b075390e7dc6c2eeffa69ed51f6bc9101f2a55f3..ed82ee04ee4482a1d8034377ca8c95ad5c6e0484 100644 --- a/src/Wima/CircularSurveyComplexItem.h +++ b/src/Wima/CircularSurveyComplexItem.h @@ -18,19 +18,19 @@ public: /// @param kmlOrShpFile Polygon comes from this file, empty for default polygon CircularSurveyComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlOrShpFile, QObject* parent); - Q_PROPERTY(QGeoCoordinate refPoint READ refPoint WRITE setRefPoint NOTIFY refPointChanged) - Q_PROPERTY(Fact* deltaR READ deltaR CONSTANT) - Q_PROPERTY(Fact* deltaAlpha READ deltaAlpha CONSTANT) - Q_PROPERTY(Fact* transectMinLength READ transectMinLength CONSTANT) - Q_PROPERTY(Fact* isSnakePath READ isSnakePath CONSTANT) - Q_PROPERTY(bool autoGenerated READ autoGenerated NOTIFY autoGeneratedChanged) + Q_PROPERTY(QGeoCoordinate refPoint READ refPoint WRITE setRefPoint NOTIFY refPointChanged) + Q_PROPERTY(Fact* deltaR READ deltaR CONSTANT) + Q_PROPERTY(Fact* deltaAlpha READ deltaAlpha CONSTANT) + Q_PROPERTY(Fact* transectMinLength READ transectMinLength CONSTANT) + Q_PROPERTY(Fact* isSnakePath READ isSnakePath CONSTANT) + Q_PROPERTY(bool isInitialized READ isInitialized WRITE setIsInitialized NOTIFY isInitializedChanged) Q_INVOKABLE void resetReference(void); // Property setters void setRefPoint(const QGeoCoordinate &refPt); // Set this to true if survey was automatically generated, prevents initialisation from gui. - void setAutoGenerated(bool autoGen); + void setIsInitialized(bool isInitialized); // Property getters QGeoCoordinate refPoint() const; @@ -39,7 +39,7 @@ public: Fact *transectMinLength(); Fact *isSnakePath(); // Is true if survey was automatically generated, prevents initialisation from gui. - bool autoGenerated(); + bool isInitialized(); // Overrides from ComplexMissionItem bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final; @@ -76,7 +76,7 @@ public: signals: void refPointChanged(); - void autoGeneratedChanged(); + void isInitializedChanged(); private slots: // Overrides from TransectStyleComplexItem @@ -103,7 +103,11 @@ private: QTimer _updateTimer; - bool _autoGenerated; // set to true if survey was automatically generated, prevents initialisation from gui + bool _isInitialized; // indicates if the polygon and refpoint etc. are initialized, prevents reinitialisation from gui and execution of _rebuildTransectsPhase1 during init from gui + + int _updateCounter; + + }; diff --git a/src/Wima/PlanimetryCalculus.cc b/src/Wima/PlanimetryCalculus.cc index 93f396268a6ebc4e54864359ee140f9e73fd4d8e..adc32b08be0fdc2adce03a66000f99b3f166d20a 100644 --- a/src/Wima/PlanimetryCalculus.cc +++ b/src/Wima/PlanimetryCalculus.cc @@ -620,7 +620,7 @@ angle double angle(const QPointF &p1) { - return qAtan2(p1.y(), p1.x()); + return truncateAngle(qAtan2(p1.y(), p1.x())); } bool intersects(const Circle &circle, const QPolygonF &polygon, QList &intersectionPoints, NeighbourList &neighbourList, IntersectList &typeList) @@ -698,6 +698,8 @@ angle + + } // end namespace PlanimetryCalculus diff --git a/src/Wima/WimaController.SettingsGroup.json b/src/Wima/WimaController.SettingsGroup.json index 34bb8620199c059f85bdcc3a8dda82030e5170ba..e807483e8ed7f88ea83717a446a4571c246b4caf 100644 --- a/src/Wima/WimaController.SettingsGroup.json +++ b/src/Wima/WimaController.SettingsGroup.json @@ -36,5 +36,20 @@ "shortDescription": "Determines whether the mission items of the current mission phase are displayed or not.", "type": "bool", "defaultValue": 1 +}, +{ + "name": "FlightSpeed", + "shortDescription": "The mission flight speed.", + "type": "double", + "min" : 0.3, + "max" : 5, + "defaultValue": 2 +}, +{ + "name": "Altitude", + "shortDescription": "The mission altitude.", + "type": "double", + "min" : 1, + "defaultValue": 5 } ] diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index df7db349d4146f1b2fa9167221c4b31a755f8dc4..2a99bf256121925933765956af481e32521dec63 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -10,6 +10,8 @@ const char* WimaController::maxWaypointsPerPhaseName = "MaxWaypointsPerPhase" const char* WimaController::startWaypointIndexName = "StartWaypointIndex"; const char* WimaController::showAllMissionItemsName = "ShowAllMissionItems"; const char* WimaController::showCurrentMissionItemsName = "ShowCurrentMissionItems"; +const char* WimaController::flightSpeedName = "FlightSpeed"; +const char* WimaController::altitudeName = "Altitude"; WimaController::WimaController(QObject *parent) : QObject (parent) @@ -25,10 +27,14 @@ WimaController::WimaController(QObject *parent) , _maxWaypointsPerPhase (settingsGroup, _metaDataMap[maxWaypointsPerPhaseName]) , _nextPhaseStartWaypointIndex (settingsGroup, _metaDataMap[startWaypointIndexName]) , _showAllMissionItems (settingsGroup, _metaDataMap[showAllMissionItemsName]) - , _showCurrentMissionItems (settingsGroup, _metaDataMap[showCurrentMissionItemsName]) + , _showCurrentMissionItems (settingsGroup, _metaDataMap[showCurrentMissionItemsName]) + , _flightSpeed (settingsGroup, _metaDataMap[flightSpeedName]) + , _altitude (settingsGroup, _metaDataMap[altitudeName]) , _startWaypointIndex (0) , _lastMissionPhaseReached (false) , _uploadOverrideRequired (false) + , _phaseDistance(-1) + , _phaseDuration(-1) { _nextPhaseStartWaypointIndex.setRawValue(int(1)); @@ -37,6 +43,8 @@ WimaController::WimaController(QObject *parent) connect(&_overlapWaypoints, &Fact::rawValueChanged, this, &WimaController::updateNextWaypoint); connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this, &WimaController::recalcCurrentPhase); connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::calcNextPhase); + connect(&_flightSpeed, &Fact::rawValueChanged, this, &WimaController::updateSpeed); + connect(&_altitude, &Fact::rawValueChanged, this, &WimaController::updateAltitude); } QmlObjectListModel* WimaController::visualItems() @@ -111,11 +119,31 @@ Fact *WimaController::showCurrentMissionItems() return &_showCurrentMissionItems; } +Fact *WimaController::flightSpeed() +{ + return &_flightSpeed; +} + +Fact *WimaController::altitude() +{ + return &_altitude; +} + bool WimaController::uploadOverrideRequired() const { return _uploadOverrideRequired; } +double WimaController::phaseDistance() const +{ + return _phaseDistance; +} + +double WimaController::phaseDuration() const +{ + return _phaseDuration; +} + Fact *WimaController::startWaypointIndex() { return &_nextPhaseStartWaypointIndex; @@ -317,6 +345,12 @@ bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVa bool retValue = extractCoordinateList(missionItems, geoCoordintateList, startIndex, endIndex); + for (int i = 0; i < geoCoordintateList.size(); i++) { + QGeoCoordinate vertex = geoCoordintateList[i]; + if (qFuzzyIsNull(vertex.latitude()) && qFuzzyIsNull(vertex.longitude())) + geoCoordintateList.removeAt(i); + } + if (!retValue) return false; @@ -539,6 +573,17 @@ bool WimaController::calcNextPhase() } takeoffItem->setCoordinate(_takeoffLandPostion); + // create change speed item + _missionController->insertSimpleMissionItem(_takeoffLandPostion, 2); + SimpleMissionItem *speedItem = missionControllerVisuals->value(2); + if (speedItem == nullptr) { + qWarning("WimaController::calcNextPhase(): nullptr"); + return false; + } + speedItem->setCoordinate(_takeoffLandPostion); + speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); + speedItem->missionItem().setParam2(_flightSpeed.rawValue().toDouble()); + // set land command for last mission item SimpleMissionItem *landItem = missionControllerVisuals->value(missionControllerVisuals->count()-1); @@ -561,7 +606,10 @@ bool WimaController::calcNextPhase() _currentMissionItems.append(visualItemCopy); } + _setPhaseDistance(_missionController->missionDistance()); + _setPhaseDuration(_missionController->missionDistance()/_flightSpeed.rawValue().toDouble()); _missionController->removeAll(); // remove items from _missionController, will be added on upload + updateAltitude(); updateCurrentPath(); emit currentMissionItemsChanged(); @@ -611,5 +659,47 @@ bool WimaController::setTakeoffLandPosition() return true; } +void WimaController::updateSpeed() +{ + SimpleMissionItem *item = _currentMissionItems.value(1); // speed item + if (item != nullptr && item->command() == MAV_CMD_DO_CHANGE_SPEED) { + item->missionItem().setParam2(_flightSpeed.rawValue().toDouble()); + _setPhaseDuration(_phaseDistance/_flightSpeed.rawValue().toDouble()); + } + else { + qWarning("WimaController::updateSpeed(): internal error."); + } +} + +void WimaController::updateAltitude() +{ + for (int i = 0; i < _currentMissionItems.count(); i++) { + SimpleMissionItem *item = _currentMissionItems.value(i); + if (item == nullptr) { + qWarning("WimaController::updateAltitude(): nullptr"); + return; + } + item->altitude()->setRawValue(_altitude.rawValue().toDouble()); + } +} + +void WimaController::_setPhaseDistance(double distance) +{ + if (!qFuzzyCompare(distance, _phaseDistance)) { + _phaseDistance = distance; + + emit phaseDistanceChanged(); + } +} + +void WimaController::_setPhaseDuration(double duration) +{ + if (!qFuzzyCompare(duration, _phaseDuration)) { + _phaseDuration = duration; + + emit phaseDurationChanged(); + } +} + diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index c7ed1481a135e0b55ecd103052a088eb985fbc20..879a70a8e2676685636c057a479d9607b9a5a937 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -51,7 +51,11 @@ public: Q_PROPERTY(Fact* startWaypointIndex READ startWaypointIndex CONSTANT) Q_PROPERTY(Fact* showAllMissionItems READ showAllMissionItems CONSTANT) Q_PROPERTY(Fact* showCurrentMissionItems READ showCurrentMissionItems CONSTANT) + Q_PROPERTY(Fact* flightSpeed READ flightSpeed CONSTANT) + Q_PROPERTY(Fact* altitude READ altitude CONSTANT) Q_PROPERTY(bool uploadOverrideRequired READ uploadOverrideRequired WRITE setUploadOverrideRequired NOTIFY uploadOverrideRequiredChanged) + Q_PROPERTY(double phaseDistance READ phaseDistance NOTIFY phaseDistanceChanged) + Q_PROPERTY(double phaseDuration READ phaseDuration NOTIFY phaseDurationChanged) @@ -75,7 +79,11 @@ public: Fact* startWaypointIndex (void); Fact* showAllMissionItems (void); Fact* showCurrentMissionItems(void); + Fact* flightSpeed (void); + Fact* altitude (void); bool uploadOverrideRequired (void) const; + double phaseDistance (void) const; + double phaseDuration (void) const; // Property setters @@ -110,6 +118,8 @@ public: static const char* startWaypointIndexName; static const char* showAllMissionItemsName; static const char* showCurrentMissionItemsName; + static const char* flightSpeedName; + static const char* altitudeName; // Member Methodes QJsonDocument saveToJson(FileType fileType); @@ -136,6 +146,8 @@ signals: void waypointPathChanged (void); void currentWaypointPathChanged (void); void uploadOverrideRequiredChanged (void); + void phaseDistanceChanged (void); + void phaseDurationChanged (void); private slots: bool fetchContainerData(); @@ -144,7 +156,13 @@ private slots: void updateCurrentPath (void); void updateNextWaypoint (void); void recalcCurrentPhase (void); - bool setTakeoffLandPosition(void); + bool setTakeoffLandPosition (void); + void updateSpeed (void); + void updateAltitude (void); + +private: + void _setPhaseDistance(double distance); + void _setPhaseDuration(double duration); private: @@ -174,6 +192,8 @@ private: // defining the first element of the next phase SettingsFact _showAllMissionItems; // bool value, Determines whether the mission items of the overall mission are displayed or not. SettingsFact _showCurrentMissionItems; // bool value, Determines whether the mission items of the current mission phase are displayed or not. + SettingsFact _flightSpeed; // mission flight speed + SettingsFact _altitude; // mission altitude int _endWaypointIndex; // indes of the mission item stored in _missionItems defining the last element // (which is not part of the return path) of _currentMissionItem @@ -182,4 +202,8 @@ private: bool _lastMissionPhaseReached; bool _uploadOverrideRequired; // Is set to true if uploadToVehicle() did not suceed because the vehicle is not inside the service area. // The user can override the upload lock with a slider, this will reset this variable to false. + double _phaseDistance; // the lenth in meters of the current phase + double _phaseDuration; // the phase duration in seconds }; + + diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index 3ea41a0e067a56db0fc0860fa37eddf6abd060d1..4d2d67d057aa719ff597010d9c110ce4f77f83d9 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -231,7 +231,7 @@ bool WimaPlaner::updateMission() _circularSurvey->setRefPoint(_measurementArea.center()); _lastSurveyRefPoint = _measurementArea.center(); _surveyRefChanging = false; - _circularSurvey->setAutoGenerated(true); // prevents reinitialisation from gui + _circularSurvey->setIsInitialized(true); // prevents reinitialisation from gui connect(_circularSurvey->deltaR(), &Fact::rawValueChanged, this, &WimaPlaner::calcArrivalAndReturnPath); connect(_circularSurvey->deltaAlpha(), &Fact::rawValueChanged, this, &WimaPlaner::calcArrivalAndReturnPath); connect(_circularSurvey->isSnakePath(), &Fact::rawValueChanged, this, &WimaPlaner::calcArrivalAndReturnPath); @@ -433,7 +433,7 @@ bool WimaPlaner::loadFromFile(const QString &filename) _lastSurveyRefPoint = _circularSurvey->refPoint(); _surveyRefChanging = false; - _circularSurvey->setAutoGenerated(true); // prevents reinitialisation from gui + _circularSurvey->setIsInitialized(true); // prevents reinitialisation from gui connect(_circularSurvey->deltaR(), &Fact::rawValueChanged, this, &WimaPlaner::calcArrivalAndReturnPath); connect(_circularSurvey->deltaAlpha(), &Fact::rawValueChanged, this, &WimaPlaner::calcArrivalAndReturnPath); connect(_circularSurvey->isSnakePath(), &Fact::rawValueChanged, this, &WimaPlaner::calcArrivalAndReturnPath); @@ -553,7 +553,7 @@ bool WimaPlaner::calcArrivalAndReturnPath() qgcApp()->showMessage( QString(tr("Not able to calculate the path from takeoff position to measurement area.")).toLocal8Bit().data()); return false; } - _arrivalPathLength = path.size()-1; // -1: last item is first measurement point + _arrivalPathLength = path.size()-1; int sequenceNumber = 0; for (int i = 1; i < path.count()-1; i++) { sequenceNumber = _missionController->insertSimpleMissionItem(path.value(i), missionItems->count()-1); @@ -596,6 +596,10 @@ bool WimaPlaner::calcArrivalAndReturnPath() bool WimaPlaner::recalcJoinedArea() { setJoinedAreaValid(false); + // check if at least service area and measurement area are available + if (_visualItems.indexOf(&_serviceArea) == -1 || _visualItems.indexOf(&_measurementArea) == -1) + return false; + // check if area paths form simple polygons if ( !_serviceArea.isSimplePolygon() ) { qgcApp()->showMessage(tr("Service area is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n")); @@ -744,7 +748,10 @@ void WimaPlaner::updateTimerSlot() // Check if parameter has changed, wait until it stops changing, update mission // circular survey reference point - if (_circularSurvey != nullptr) { + if (_missionController != nullptr + && _missionController->visualItems()->indexOf(_circularSurvey) != -1 + && _circularSurvey != nullptr) + { if (_surveyRefChanging) { if (_circularSurvey->refPoint() == _lastSurveyRefPoint) { // is it still changing? calcArrivalAndReturnPath(); @@ -791,7 +798,9 @@ void WimaPlaner::updateTimerSlot() // update old values - if (_circularSurvey != nullptr) + if (_missionController != nullptr + && _missionController->visualItems()->indexOf(_circularSurvey) != -1 + && _circularSurvey != nullptr) _lastSurveyRefPoint = _circularSurvey->refPoint(); _lastMeasurementAreaPath = _measurementArea.path(); diff --git a/src/WimaView/SphericalSurveyMapVisual.qml b/src/WimaView/SphericalSurveyMapVisual.qml index 8c6683fab927e244277ab26cda046c5310fca6a4..38bfcfc8a04a1302a7589494e4ff74ea46e8eef6 100644 --- a/src/WimaView/SphericalSurveyMapVisual.qml +++ b/src/WimaView/SphericalSurveyMapVisual.qml @@ -90,8 +90,9 @@ Item { } Component.onCompleted: { - if ( !_missionItem.autoGenerated ) { - _addInitialPolygon() + if ( !_missionItem.isInitialized ) { + _addInitialPolygon() + _missionItem.isInitialized = true // set isInitialized to true, to trigger _rebuildTransectsPhase1 in the last line _setRefPoint() } _addVisualElements()