From 46978c3e00dc124f1bfca44c9f397763ef1e9e56 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Mon, 27 Jul 2020 12:29:52 +0200 Subject: [PATCH] working on DefaultManager.cpp --- Paths/Klingenbach.plan | 697 +++++++++++++++++ Paths/KlingenbachTest.wima | 803 +++----------------- src/Wima/CircularSurveyComplexItem.cc | 12 +- src/Wima/WaypointManager/DefaultManager.cpp | 48 +- src/Wima/WaypointManager/DefaultManager.h | 6 +- src/Wima/WaypointManager/Slicer.cpp | 5 - src/Wima/WaypointManager/Slicer.h | 2 - src/Wima/WaypointManager/Utils.cpp | 105 ++- src/Wima/WaypointManager/Utils.h | 88 ++- src/Wima/WimaController.cc | 92 ++- src/Wima/WimaController.h | 103 +-- 11 files changed, 1039 insertions(+), 922 deletions(-) create mode 100644 Paths/Klingenbach.plan diff --git a/Paths/Klingenbach.plan b/Paths/Klingenbach.plan new file mode 100644 index 000000000..461bb1e65 --- /dev/null +++ b/Paths/Klingenbach.plan @@ -0,0 +1,697 @@ +{ + "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, + 8 + ], + "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": 2, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76826616141194, + 16.530725666402216, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 3, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76825906623652, + 16.53087697971209, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 4, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76825595539324, + 16.531028590198325, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 5, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76825595539324, + 16.531028590198325, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 6, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76816596834739, + 16.53106809506861, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 7, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76816596834739, + 16.53106809506861, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 8, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76816873697609, + 16.530889307913807, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 9, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76817686074449, + 16.530710882254596, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 10, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768190323493215, + 16.530533173006166, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 11, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768190323493215, + 16.530533173006166, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 12, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811851790795, + 16.530350914563478, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 13, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811851790795, + 16.530350914563478, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 14, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768099209120734, + 16.530538775407198, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 15, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76808571028981, + 16.53072775601019, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 16, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76807804981345, + 16.53091745880308, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 17, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76807624380742, + 16.53110748469719, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 18, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76807624380742, + 16.53110748469719, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 19, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76798674683302, + 16.531146065535864, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 20, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76798674683302, + 16.531146065535864, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 21, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76798738725695, + 16.530953664408617, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 22, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.7679938426545, + 16.530761500810428, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 23, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76800609996228, + 16.530569963610645, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 24, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76802413437596, + 16.530379440411014, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 25, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76802413437596, + 16.530379440411014, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 26, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76792911566441, + 16.53041829010208, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 27, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76792911566441, + 16.53041829010208, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 28, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76791032279039, + 16.53064518113584, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 29, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.767899456183414, + 16.530873216172655, + 15 + ], + "type": "SimpleItem" + } + ], + "Refly90Degrees": false, + "TurnAroundDistance": 10, + "VisualTransectPoints": [ + [ + 47.76826616141194, + 16.530725666402216 + ], + [ + 47.76825906623652, + 16.53087697971209 + ], + [ + 47.76825595539324, + 16.531028590198325 + ], + [ + 47.76825595539324, + 16.531028590198325 + ], + [ + 47.76816596834739, + 16.53106809506861 + ], + [ + 47.76816596834739, + 16.53106809506861 + ], + [ + 47.76816873697609, + 16.530889307913807 + ], + [ + 47.76817686074449, + 16.530710882254596 + ], + [ + 47.768190323493215, + 16.530533173006166 + ], + [ + 47.768190323493215, + 16.530533173006166 + ], + [ + 47.76811851790795, + 16.530350914563478 + ], + [ + 47.76811851790795, + 16.530350914563478 + ], + [ + 47.768099209120734, + 16.530538775407198 + ], + [ + 47.76808571028981, + 16.53072775601019 + ], + [ + 47.76807804981345, + 16.53091745880308 + ], + [ + 47.76807624380742, + 16.53110748469719 + ], + [ + 47.76807624380742, + 16.53110748469719 + ], + [ + 47.76798674683302, + 16.531146065535864 + ], + [ + 47.76798674683302, + 16.531146065535864 + ], + [ + 47.76798738725695, + 16.530953664408617 + ], + [ + 47.7679938426545, + 16.530761500810428 + ], + [ + 47.76800609996228, + 16.530569963610645 + ], + [ + 47.76802413437596, + 16.530379440411014 + ], + [ + 47.76802413437596, + 16.530379440411014 + ], + [ + 47.76792911566441, + 16.53041829010208 + ], + [ + 47.76792911566441, + 16.53041829010208 + ], + [ + 47.76791032279039, + 16.53064518113584 + ], + [ + 47.767899456183414, + 16.530873216172655 + ] + ], + "version": 1 + }, + "complexItemType": "circularSurvey", + "deltaAlpha": 3, + "deltaR": 10, + "fixedDirection": false, + "polygon": [ + [ + 47.768115102959555, + 16.530342246669797 + ], + [ + 47.76836640344051, + 16.530980102821747 + ], + [ + 47.76798694569257, + 16.531146687122508 + ], + [ + 47.76777418124312, + 16.530481637145055 + ] + ], + "referencePointAlt": 0, + "referencePointLat": 47.77086091347008, + "referencePointLong": 16.531071041719485, + "reverse": false, + "transectMinLength": 15, + "type": "ComplexItem", + "version": 1 + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 5, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 33, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76783728246855, + 16.530678876245194, + 5 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 0, + "AltitudeMode": 1, + "autoContinue": true, + "command": 21, + "doJumpId": 34, + "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/KlingenbachTest.wima b/Paths/KlingenbachTest.wima index 971447174..b39dbab02 100644 --- a/Paths/KlingenbachTest.wima +++ b/Paths/KlingenbachTest.wima @@ -172,424 +172,8 @@ 0, 0, null, - 47.76821092723909, - 16.531048357836788, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 7, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76821092723909, - 16.531048357836788, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 8, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76821315105435, - 16.530907959982486, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 9, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76821873382109, - 16.53076776901368, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 10, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76822766845702, - 16.530627962776645, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 11, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76822766845702, - 16.530627962776645, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 12, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.768190323493215, - 16.530533173006166, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 13, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.768190323493215, - 16.530533173006166, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 14, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76817686074449, - 16.530710882254596, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 15, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76816873697609, - 16.530889307913807, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 16, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76816596834739, - 16.53106809506861, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 17, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76816596834739, - 16.53106809506861, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 18, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76812107501615, - 16.531087803518982, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 19, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76812107501615, - 16.531087803518982, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 20, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76812279902175, - 16.530925480714274, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 21, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.768128865390075, - 16.530763388754075, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 22, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76813926450051, - 16.530601784699265, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 23, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76815397986116, - 16.530440924836945, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 24, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76815397986116, - 16.530440924836945, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 25, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76811851790795, - 16.530350914563478, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 26, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76811851790795, - 16.530350914563478, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 27, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.768099209120734, - 16.530538775407198, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 28, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76808571028981, - 16.53072775601019, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 29, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76807804981345, - 16.53091745880308, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 30, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76807624380742, - 16.53110748469719, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 31, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76807624380742, - 16.53110748469719, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 32, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76803147152379, - 16.531127140006916, + 47.76816596834739, + 16.53106809506861, 15 ], "type": "SimpleItem" @@ -597,15 +181,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 33, + "doJumpId": 7, "frame": 3, "params": [ 0, 0, 0, null, - 47.76803147152379, - 16.531127140006916, + 47.76816596834739, + 16.53106809506861, 15 ], "type": "SimpleItem" @@ -613,15 +197,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 34, + "doJumpId": 8, "frame": 3, "params": [ 0, 0, 0, null, - 47.76803270870733, - 16.530934504097694, + 47.76816873697609, + 16.530889307913807, 15 ], "type": "SimpleItem" @@ -629,15 +213,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 35, + "doJumpId": 9, "frame": 3, "params": [ 0, 0, 0, null, - 47.76803986710213, - 16.53074215404759, + 47.76817686074449, + 16.530710882254596, 15 ], "type": "SimpleItem" @@ -645,15 +229,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 36, + "doJumpId": 10, "frame": 3, "params": [ 0, 0, 0, null, - 47.76805293172116, - 16.530550492566235, + 47.768190323493215, + 16.530533173006166, 15 ], "type": "SimpleItem" @@ -661,15 +245,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 37, + "doJumpId": 11, "frame": 3, "params": [ 0, 0, 0, null, - 47.768071875211966, - 16.53035992092165, + 47.768190323493215, + 16.530533173006166, 15 ], "type": "SimpleItem" @@ -677,15 +261,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 38, + "doJumpId": 12, "frame": 3, "params": [ 0, 0, 0, null, - 47.768071875211966, - 16.53035992092165, + 47.76811851790795, + 16.530350914563478, 15 ], "type": "SimpleItem" @@ -693,15 +277,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 39, + "doJumpId": 13, "frame": 3, "params": [ 0, 0, 0, null, - 47.76802413437596, - 16.530379440411014, + 47.76811851790795, + 16.530350914563478, 15 ], "type": "SimpleItem" @@ -709,15 +293,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 40, + "doJumpId": 14, "frame": 3, "params": [ 0, 0, 0, null, - 47.76802413437596, - 16.530379440411014, + 47.768099209120734, + 16.530538775407198, 15 ], "type": "SimpleItem" @@ -725,15 +309,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 41, + "doJumpId": 15, "frame": 3, "params": [ 0, 0, 0, null, - 47.76800609996228, - 16.530569963610645, + 47.76808571028981, + 16.53072775601019, 15 ], "type": "SimpleItem" @@ -741,15 +325,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 42, + "doJumpId": 16, "frame": 3, "params": [ 0, 0, 0, null, - 47.7679938426545, - 16.530761500810428, + 47.76807804981345, + 16.53091745880308, 15 ], "type": "SimpleItem" @@ -757,15 +341,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 43, + "doJumpId": 17, "frame": 3, "params": [ 0, 0, 0, null, - 47.76798738725695, - 16.530953664408617, + 47.76807624380742, + 16.53110748469719, 15 ], "type": "SimpleItem" @@ -773,15 +357,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 44, + "doJumpId": 18, "frame": 3, "params": [ 0, 0, 0, null, - 47.76798674683302, - 16.531146065535864, + 47.76807624380742, + 16.53110748469719, 15 ], "type": "SimpleItem" @@ -789,7 +373,7 @@ { "autoContinue": true, "command": 16, - "doJumpId": 45, + "doJumpId": 19, "frame": 3, "params": [ 0, @@ -805,15 +389,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 46, + "doJumpId": 20, "frame": 3, "params": [ 0, 0, 0, null, - 47.76794172285018, - 16.531005331504193, + 47.76798674683302, + 16.531146065535864, 15 ], "type": "SimpleItem" @@ -821,15 +405,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 47, + "doJumpId": 21, "frame": 3, "params": [ 0, 0, 0, null, - 47.76794172285018, - 16.531005331504193, + 47.76798738725695, + 16.530953664408617, 15 ], "type": "SimpleItem" @@ -837,15 +421,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 48, + "doJumpId": 22, "frame": 3, "params": [ 0, 0, 0, null, - 47.76794697499331, - 16.53080245216519, + 47.7679938426545, + 16.530761500810428, 15 ], "type": "SimpleItem" @@ -853,15 +437,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 49, + "doJumpId": 23, "frame": 3, "params": [ 0, 0, 0, null, - 47.767958592743554, - 16.53060015957008, + 47.76800609996228, + 16.530569963610645, 15 ], "type": "SimpleItem" @@ -869,15 +453,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 50, + "doJumpId": 24, "frame": 3, "params": [ 0, 0, 0, null, - 47.7679765507215, - 16.530398895634573, + 47.76802413437596, + 16.530379440411014, 15 ], "type": "SimpleItem" @@ -885,15 +469,15 @@ { "autoContinue": true, "command": 16, - "doJumpId": 51, + "doJumpId": 25, "frame": 3, "params": [ 0, 0, 0, null, - 47.7679765507215, - 16.530398895634573, + 47.76802413437596, + 16.530379440411014, 15 ], "type": "SimpleItem" @@ -901,7 +485,7 @@ { "autoContinue": true, "command": 16, - "doJumpId": 52, + "doJumpId": 26, "frame": 3, "params": [ 0, @@ -917,7 +501,7 @@ { "autoContinue": true, "command": 16, - "doJumpId": 53, + "doJumpId": 27, "frame": 3, "params": [ 0, @@ -933,7 +517,7 @@ { "autoContinue": true, "command": 16, - "doJumpId": 54, + "doJumpId": 28, "frame": 3, "params": [ 0, @@ -949,23 +533,7 @@ { "autoContinue": true, "command": 16, - "doJumpId": 55, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.767899456183414, - 16.530873216172655, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 56, + "doJumpId": 29, "frame": 3, "params": [ 0, @@ -977,70 +545,6 @@ 15 ], "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 57, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76785939920459, - 16.530748007788787, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 58, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76785939920459, - 16.530748007788787, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 59, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76786879171822, - 16.530592526599857, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 60, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76788182125843, - 16.53043762706247, - 15 - ], - "type": "SimpleItem" } ], "Refly90Degrees": false, @@ -1062,46 +566,6 @@ 47.76825595539324, 16.531028590198325 ], - [ - 47.76821092723909, - 16.531048357836788 - ], - [ - 47.76821092723909, - 16.531048357836788 - ], - [ - 47.76821315105435, - 16.530907959982486 - ], - [ - 47.76821873382109, - 16.53076776901368 - ], - [ - 47.76822766845702, - 16.530627962776645 - ], - [ - 47.76822766845702, - 16.530627962776645 - ], - [ - 47.768190323493215, - 16.530533173006166 - ], - [ - 47.768190323493215, - 16.530533173006166 - ], - [ - 47.76817686074449, - 16.530710882254596 - ], - [ - 47.76816873697609, - 16.530889307913807 - ], [ 47.76816596834739, 16.53106809506861 @@ -1111,32 +575,20 @@ 16.53106809506861 ], [ - 47.76812107501615, - 16.531087803518982 - ], - [ - 47.76812107501615, - 16.531087803518982 - ], - [ - 47.76812279902175, - 16.530925480714274 - ], - [ - 47.768128865390075, - 16.530763388754075 + 47.76816873697609, + 16.530889307913807 ], [ - 47.76813926450051, - 16.530601784699265 + 47.76817686074449, + 16.530710882254596 ], [ - 47.76815397986116, - 16.530440924836945 + 47.768190323493215, + 16.530533173006166 ], [ - 47.76815397986116, - 16.530440924836945 + 47.768190323493215, + 16.530533173006166 ], [ 47.76811851790795, @@ -1166,54 +618,6 @@ 47.76807624380742, 16.53110748469719 ], - [ - 47.76803147152379, - 16.531127140006916 - ], - [ - 47.76803147152379, - 16.531127140006916 - ], - [ - 47.76803270870733, - 16.530934504097694 - ], - [ - 47.76803986710213, - 16.53074215404759 - ], - [ - 47.76805293172116, - 16.530550492566235 - ], - [ - 47.768071875211966, - 16.53035992092165 - ], - [ - 47.768071875211966, - 16.53035992092165 - ], - [ - 47.76802413437596, - 16.530379440411014 - ], - [ - 47.76802413437596, - 16.530379440411014 - ], - [ - 47.76800609996228, - 16.530569963610645 - ], - [ - 47.7679938426545, - 16.530761500810428 - ], - [ - 47.76798738725695, - 16.530953664408617 - ], [ 47.76798674683302, 16.531146065535864 @@ -1223,28 +627,24 @@ 16.531146065535864 ], [ - 47.76794172285018, - 16.531005331504193 - ], - [ - 47.76794172285018, - 16.531005331504193 + 47.76798738725695, + 16.530953664408617 ], [ - 47.76794697499331, - 16.53080245216519 + 47.7679938426545, + 16.530761500810428 ], [ - 47.767958592743554, - 16.53060015957008 + 47.76800609996228, + 16.530569963610645 ], [ - 47.7679765507215, - 16.530398895634573 + 47.76802413437596, + 16.530379440411014 ], [ - 47.7679765507215, - 16.530398895634573 + 47.76802413437596, + 16.530379440411014 ], [ 47.76792911566441, @@ -1261,33 +661,13 @@ [ 47.767899456183414, 16.530873216172655 - ], - [ - 47.767899456183414, - 16.530873216172655 - ], - [ - 47.76785939920459, - 16.530748007788787 - ], - [ - 47.76785939920459, - 16.530748007788787 - ], - [ - 47.76786879171822, - 16.530592526599857 - ], - [ - 47.76788182125843, - 16.53043762706247 ] ], "version": 1 }, "complexItemType": "circularSurvey", "deltaAlpha": 3, - "deltaR": 5, + "deltaR": 10, "fixedDirection": false, "polygon": [ [ @@ -1307,7 +687,7 @@ 16.530481637145055 ] ], - "referencePointAlt": null, + "referencePointAlt": 0, "referencePointLat": 47.77086091347008, "referencePointLong": 16.531071041719485, "reverse": false, @@ -1315,13 +695,32 @@ "type": "ComplexItem", "version": 1 }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 5, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 33, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76783728246855, + 16.530678876245194, + 5 + ], + "type": "SimpleItem" + }, { "AMSLAltAboveTerrain": null, "Altitude": 0, "AltitudeMode": 1, "autoContinue": true, "command": 21, - "doJumpId": 64, + "doJumpId": 34, "frame": 3, "params": [ 0, diff --git a/src/Wima/CircularSurveyComplexItem.cc b/src/Wima/CircularSurveyComplexItem.cc index f201e0823..35a3be11a 100644 --- a/src/Wima/CircularSurveyComplexItem.cc +++ b/src/Wima/CircularSurveyComplexItem.cc @@ -237,9 +237,10 @@ void CircularSurveyComplexItem::_buildAndAppendMissionItems(QList& MissionItem* item; int seqNum = _sequenceNumber; - MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT; + MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() + ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT; - for (const QList& transect: _transects) { + for (const QList& transect : _transects) { //bool transectEntry = true; for (const CoordInfo_t& transectCoordInfo: transect) { @@ -392,7 +393,7 @@ void CircularSurveyComplexItem::_rebuildTransectsSlow() if (!_rebuildTransectsInputCheck(surveyPolygon)) return; - // If the transects are getting rebuilt then any previously loaded mission items are now invalid + // If the transects are getting rebuilt then any previously loaded mission items are now invalid. if (_loadedMissionItemsParent) { _loadedMissionItems.clear(); _loadedMissionItemsParent->deleteLater(); @@ -446,7 +447,10 @@ void CircularSurveyComplexItem::_rebuildTransectsSlow() reversePath ^= true; // toggle - optimizedPath.append(connectorPath); + connectorPath.pop_front(); + connectorPath.pop_back(); + if (connectorPath.size() > 0) + optimizedPath.append(connectorPath); optimizedPath.append(currentSection); } if (optimizedPath.size() > _maxWaypoints.rawValue().toInt()) diff --git a/src/Wima/WaypointManager/DefaultManager.cpp b/src/Wima/WaypointManager/DefaultManager.cpp index 186daf041..c9690b29f 100644 --- a/src/Wima/WaypointManager/DefaultManager.cpp +++ b/src/Wima/WaypointManager/DefaultManager.cpp @@ -3,6 +3,8 @@ #include "Wima/Geometry/PolygonCalculus.h" #include "MissionSettingsItem.h" +#include "SimpleMissionItem.h" + WaypointManager::DefaultManager::DefaultManager(Settings &settings, AreaInterface &interface) @@ -67,11 +69,11 @@ bool WaypointManager::DefaultManager::_insertMissionItem(const QGeoCoordinate &c list, _settings->vehicle(), _settings->isFlyView(), - &_currentMissionItems /*parent*/, + &list /*parent*/, doUpdate /*do update*/) ) { - Q_ASSERT(false); qWarning("WaypointManager::DefaultManager::next(): insertMissionItem failed."); + Q_ASSERT(false); return false; } return true; @@ -115,34 +117,37 @@ bool WaypointManager::DefaultManager::_worker() if (_dirty) { _missionItems.clearAndDeleteContents(); _waypointsVariant.clear(); - initialize(_missionItems, _settings->vehicle(), _settings->isFlyView()); + // No initialization of _missionItems, don't need MissionSettingsItem here. for (size_t i = 0; i < size_t(_waypoints.size()); ++i) { - const QGeoCoordinate &c = _waypoints.at(i); + auto &c = _waypoints.at(i); _insertMissionItem(c, _missionItems.count(), _missionItems, false /*update list*/); _waypointsVariant.push_back(QVariant::fromValue(c)); } - Q_ASSERT(_missionItems.value(0) != nullptr); - doUpdate(_missionItems); - + updateHirarchy(_missionItems); + updateSequenceNumbers(_missionItems, 1); // Start with 1, since MissionSettingsItem missing. _dirty = false; } _currentMissionItems.clearAndDeleteContents(); initialize(_currentMissionItems, _settings->vehicle(), _settings->isFlyView()); - // calculate path from home to first waypoint - QVector arrivalPath; + // Precondition. if (!_settings->homePosition().isValid()){ qWarning("WaypointManager::DefaultManager::next(): home position invalid."); Q_ASSERT(false); return false; } + // Calculate path from home to first waypoint. + QVector arrivalPath; if ( !_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(), arrivalPath) ) { qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to first waypoint."); return false; } - arrivalPath.removeFirst(); + if (!arrivalPath.empty()) + arrivalPath.removeFirst(); + if (!arrivalPath.empty()) + arrivalPath.removeLast(); // calculate path from last waypoint to home QVector returnPath; @@ -150,8 +155,10 @@ bool WaypointManager::DefaultManager::_worker() qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to last waypoint."); return false; } - returnPath.removeFirst(); - returnPath.removeLast(); + if (!returnPath.empty()) + returnPath.removeFirst(); + if (!returnPath.empty()) + returnPath.removeLast(); @@ -165,28 +172,25 @@ bool WaypointManager::DefaultManager::_worker() } settingsItem->setCoordinate(_settings->homePosition()); - // Set takeoff position for first mission item (bug). + // First mission item is takeoff command. _insertMissionItem(_settings->homePosition(), 1 /*insertion index*/, false /*do update*/); SimpleMissionItem *takeoffItem = _currentMissionItems.value(1); if (takeoffItem == nullptr) { - Q_ASSERT(false); qWarning("WaypointManager::DefaultManager::next(): nullptr."); + Q_ASSERT(takeoffItem != nullptr); return false; } - takeoffItem->setCoordinate(_settings->homePosition()); + makeTakeOffCmd(takeoffItem, _settings->masterController()->managerVehicle()); // Create change speed item. _insertMissionItem(_settings->homePosition(), 2 /*insertion index*/, false /*do update*/); SimpleMissionItem *speedItem = _currentMissionItems.value(2); if (speedItem == nullptr) { - Q_ASSERT(false); qWarning("WaypointManager::DefaultManager::next(): nullptr."); + Q_ASSERT(speedItem != nullptr); return false; } - speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); - // Set coordinate must be after setCommand (setCommand sets coordinate to zero). - speedItem->setCoordinate(_settings->homePosition()); - speedItem->missionItem().setParam2(_settings->arrivalReturnSpeed()); + makeSpeedCmd(speedItem, _settings->arrivalReturnSpeed()); // insert arrival path for (auto coordinate : arrivalPath) { @@ -204,9 +208,7 @@ bool WaypointManager::DefaultManager::_worker() qWarning("WaypointManager::DefaultManager::next(): nullptr."); return false; } - speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero) - speedItem->setCoordinate(_currentWaypoints.first()); - speedItem->missionItem().setParam2(_settings->flightSpeed()); + makeSpeedCmd(speedItem, _settings->flightSpeed()); // Insert slice. for (auto coordinate : _currentWaypoints) { diff --git a/src/Wima/WaypointManager/DefaultManager.h b/src/Wima/WaypointManager/DefaultManager.h index ff0ee73c1..21b168177 100644 --- a/src/Wima/WaypointManager/DefaultManager.h +++ b/src/Wima/WaypointManager/DefaultManager.h @@ -15,7 +15,11 @@ typedef GenericWaypointManager ManagerBase; - +//! +//! \brief The DefaultManager class is used to manage SimpleMissionItems. +//! +//! @note The \class QmlObjectList returned by \fn missionItems doesn't contain a MissionSettingsItem. +//! This list is supposed to be used for display purposes only. class DefaultManager : public ManagerBase { public: diff --git a/src/Wima/WaypointManager/Slicer.cpp b/src/Wima/WaypointManager/Slicer.cpp index 9a4eebca1..7792e12b3 100644 --- a/src/Wima/WaypointManager/Slicer.cpp +++ b/src/Wima/WaypointManager/Slicer.cpp @@ -38,11 +38,6 @@ int Slicer::startIndex() return _idxStart; } -Slicer &Slicer::operator=(const Slicer &other) -{ - -} - void Slicer::_updateIdx(std::size_t size) { _idxValid = true; diff --git a/src/Wima/WaypointManager/Slicer.h b/src/Wima/WaypointManager/Slicer.h index f8f09f97b..ae0c996fc 100644 --- a/src/Wima/WaypointManager/Slicer.h +++ b/src/Wima/WaypointManager/Slicer.h @@ -46,8 +46,6 @@ public: //! @return Returns the start index. int startIndex (); - Slicer &operator=(const Slicer &other); - private: void _updateIdx(std::size_t size); diff --git a/src/Wima/WaypointManager/Utils.cpp b/src/Wima/WaypointManager/Utils.cpp index 39a17f1bd..b75f96218 100644 --- a/src/Wima/WaypointManager/Utils.cpp +++ b/src/Wima/WaypointManager/Utils.cpp @@ -1,5 +1,8 @@ #include "Utils.h" #include "MissionSettingsItem.h" +#include + +#include template<> @@ -28,25 +31,12 @@ bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate, int sequenceNumber = detail::nextSequenceNumber(list); newItem->setSequenceNumber(sequenceNumber); newItem->setCoordinate(coordinate); - - // Set command (takeoff if first command). newItem->setCommand(MAV_CMD_NAV_WAYPOINT); - if (list.count() == 1 - && ( vehicle->fixedWing() - || vehicle->vtol() - || vehicle->multiRotor() ) ) { - MAV_CMD takeoffCmd = vehicle->vtol() - ? MAV_CMD_NAV_VTOL_TAKEOFF - : MAV_CMD_NAV_TAKEOFF; - if (vehicle->firmwarePlugin()->supportedMissionCommands().contains(takeoffCmd)) { - newItem->setCommand(takeoffCmd); - } - } // Set altitude and altitude mode from previous item. if (newItem->specifiesAltitude()) { - double altitude; - int altitudeMode; + double altitude = 10; + int altitudeMode = QGroundControlQmlGlobal::AltitudeModeRelative; if (detail::previousAltitude(list, index-1, altitude, altitudeMode)) { newItem->altitude()->setRawValue(altitude); @@ -59,9 +49,9 @@ bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate, list.insert(index, newItem); if ( doUpdates ) { - detail::updateSequenceNumbers(list, index); - detail::updateHirarchy(list); - detail::updateHomePosition(list); + updateSequenceNumbers(list); + updateHirarchy(list); + updateHomePosition(list); } } return true; @@ -104,30 +94,27 @@ bool WaypointManager::Utils::detail::previousAltitude(QmlObjectListModel &list, return false; } -bool WaypointManager::Utils::detail::updateSequenceNumbers(QmlObjectListModel &list, size_t startIdx) +bool WaypointManager::Utils::updateSequenceNumbers(QmlObjectListModel &list, size_t firstSeqNumber) { using namespace WaypointManager::Utils; - if ( list.count() < 1 || startIdx >= size_t(list.count()) ) + if ( list.count() < 1) return false; // Setup ascending sequence numbers for all visual items. - VisualMissionItem* startItem = list.value( - std::max(long(startIdx)-1,long(0)) ); - if (list.count() == 1){ - startItem->setSequenceNumber(0); - } else { - int sequenceNumber = startItem->lastSequenceNumber() + 1; - for (int i=startIdx; i < list.count(); ++i) { - VisualMissionItem* item = list.value(i); - item->setSequenceNumber(sequenceNumber); - sequenceNumber = item->lastSequenceNumber() + 1; - } + VisualMissionItem* startItem = list.value(0); + assert(startItem != nullptr); // list empty? + startItem->setSequenceNumber(firstSeqNumber); + int sequenceNumber = startItem->lastSequenceNumber() + 1; + for (int i=1; i < list.count(); ++i) { + VisualMissionItem* item = list.value(i); + item->setSequenceNumber(sequenceNumber); + sequenceNumber = item->lastSequenceNumber() + 1; } return true; } -bool WaypointManager::Utils::detail::updateHirarchy(QmlObjectListModel &list) +bool WaypointManager::Utils::updateHirarchy(QmlObjectListModel &list) { if ( list.count() < 1) return false; @@ -150,7 +137,7 @@ bool WaypointManager::Utils::detail::updateHirarchy(QmlObjectListModel &list) return true; } -bool WaypointManager::Utils::detail::updateHomePosition(QmlObjectListModel &list) +bool WaypointManager::Utils::updateHomePosition(QmlObjectListModel &list) { MissionSettingsItem *settingsItem = list.value(0); assert(settingsItem); // list not initialized? @@ -161,7 +148,7 @@ bool WaypointManager::Utils::detail::updateHomePosition(QmlObjectListModel &list assert(item); if (item->specifiesCoordinate() && item->coordinate().isValid()) { - QGeoCoordinate c = item->coordinate().atDistanceAndAzimuth(30, 0); + QGeoCoordinate c = item->coordinate().atDistanceAndAzimuth(3, 0); c.setAltitude(0); settingsItem->setCoordinate(c); return true; @@ -179,13 +166,55 @@ void WaypointManager::Utils::initialize(QmlObjectListModel &list, list.insert(0, settingsItem); } -bool WaypointManager::Utils::doUpdate(QmlObjectListModel &list) +bool WaypointManager::Utils::updateList(QmlObjectListModel &list) { using namespace WaypointManager::Utils; - bool ret = detail::updateSequenceNumbers(list, 0); - ret = detail::updateHirarchy(list) == false ? false : ret; - (void)detail::updateHomePosition(list); + bool ret = updateSequenceNumbers(list); + ret = updateHirarchy(list) == false ? false : ret; + (void)updateHomePosition(list); return ret; } + +void WaypointManager::Utils::printList(QmlObjectListModel &list) +{ + for (int i = 0; i < list.count(); ++i) { + auto item = list.value(i); + qWarning() << "list index: " << i << "\n" + << "coordinate: " << item->coordinate() << "\n" + << "seq: " << item->sequenceNumber() << "\n" + << "last seq: " << item->lastSequenceNumber() << "\n" + << "is simple item: " << item->isSimpleItem() << "\n\n"; + } +} + +bool WaypointManager::Utils::makeTakeOffCmd(SimpleMissionItem *item, Vehicle *vehicle) +{ + if ( vehicle->fixedWing() + || vehicle->vtol() + || vehicle->multiRotor() + ) + { + MAV_CMD takeoffCmd = vehicle->vtol() + ? MAV_CMD_NAV_VTOL_TAKEOFF + : MAV_CMD_NAV_TAKEOFF; + if (vehicle->firmwarePlugin()->supportedMissionCommands().contains(takeoffCmd)) { + auto c = item->coordinate(); + item->setCommand(takeoffCmd); + item->setCoordinate(c); + return true; + } + } + return false; +} + +void WaypointManager::Utils::makeSpeedCmd(SimpleMissionItem *item, double speed) +{ + assert(speed > 0); + auto c = item->coordinate(); + item->setCommand(MAV_CMD_DO_CHANGE_SPEED); + // Set coordinate must be after setCommand (setCommand sets coordinate to zero). + item->setCoordinate(c); + item->missionItem().setParam2(speed); +} diff --git a/src/Wima/WaypointManager/Utils.h b/src/Wima/WaypointManager/Utils.h index afba6a9eb..be575ef33 100644 --- a/src/Wima/WaypointManager/Utils.h +++ b/src/Wima/WaypointManager/Utils.h @@ -1,10 +1,16 @@ #pragma once -#include "SimpleMissionItem.h" #include "QmlObjectListModel.h" +#include "SimpleMissionItem.h" #include + +class Vehicle; +class QGeoCoordinate; + + + namespace WaypointManager { namespace Utils { @@ -104,6 +110,21 @@ bool extract(const ContainerType &source, return true; } +//! +//! \brief Makes the SimpleMissionItem \p item a takeoff command. +//! \param item SimpleMissionItem +//! \param vehilce Vehicle. +//! \return Returns true if successfull, false either. +//! +bool makeTakeOffCmd(SimpleMissionItem *item, Vehicle *vehicle); + +//! +//! \brief Makes the SimpleMissionItem \p item a MAV_CMD_DO_CHANGE_SPEED item. +//! \param item SimpleMissionItem. +//! \param speed Speed (must be greater zero). +//! +void makeSpeedCmd(SimpleMissionItem *item, double speed); + //! //! \brief Initializes the list \p list. @@ -116,7 +137,7 @@ void initialize(QmlObjectListModel &list, Vehicle *vehicle, bool flyView=true); -//! @brief Creates (from QGeoCoordinate) and Inserts a SimpleMissionItem at the given index to list. +//! @brief Creates (from QGeoCoordinate) and inserts a SimpleMissionItem at the given index to list. //! \param coordinate Coordinate of the SimpleMissionItem. //! \param index Index to insert SimpleMissionItem. //! \param list List of SimpleMissionItems. @@ -137,8 +158,42 @@ bool insertMissionItem(const QGeoCoordinate &coordinate, bool flyView=true, QObject *parent=nullptr, bool doUpdates=true); +//! +//! \brief Performs a list update. +//! +//! Updates the home position of the MissionSettingsItem, the parent child hirarchy and the sequence numbers. +//! \param list List containing VisualMissionItems. +//! \return Returns true if no errors occured, false either. +bool updateList(QmlObjectListModel &list); + +//! @brief Updates the sequence numbers of the VisualMissionItems inside \p list. +//! +//! \param list List containing VisualMissionItems. +//! \param firstSeqNumber The sequence number of the fisrt list entry (defaul = 0). +//! \return Returns true if successfull, false either. +//! +//! \note If the list \p list contains only one VisualMissionItem it's sequence number is +//! set to 0. +bool updateSequenceNumbers(QmlObjectListModel &list, size_t firstSeqNumber = 0); + +//! @brief Update the parent child item hierarchy of the VisualMissionItems inside \p list. +//! +//! \param list List containing VisualMissionItems. +//! \return Returns true if successfull, false either. +//! +//! \note Returns true on success, false either. +bool updateHirarchy(QmlObjectListModel &list); + +//! @brief Updates the home position to the first valid coordinate of the VisualMissionItems inside \p list. +//! +//! \param list List containing VisualMissionItems. +//! \return Returns true if the home position was updated, false either. +bool updateHomePosition(QmlObjectListModel &list); -bool doUpdate(QmlObjectListModel &list); +//! +//! \brief Prints the \p list. +//! \param list List containing VisualMissionItems. +void printList(QmlObjectListModel &list); namespace detail { size_t nextSequenceNumber(QmlObjectListModel &list); @@ -147,33 +202,6 @@ namespace detail { double &altitude, int &altitudeMode); - //! @brief Updates the sequence numbers of the VisualMissionItems inside \p list beginning from \p startIdx. - //! - //! Updates the sequence numbers of the VisualMissionItems inside \p list beginning from \pstartIdx. - //! \param list List containing VisualMissionItems. - //! \param startIdx Start index. - //! \return Returns true if successfull, false either. - //! - //! \note The function returns false immediatelly if the list \p list ist empty or - //! the \p startIdx is out of bounds. - //! \note If the list \p list contains only one VisualMissionItem it's sequence number is - //! set to 0. - bool updateSequenceNumbers(QmlObjectListModel &list, size_t startIdx=0); - - //! @brief Update the parent child item hierarchy of the VisualMissionItems inside \p list. - //! - //! \param list List containing VisualMissionItems. - //! \return Returns true if successfull, false either. - //! - //! \note Returns true on success, false either. - bool updateHirarchy(QmlObjectListModel &list); - - //! @brief Updates the home position to the first valid coordinate of the VisualMissionItems inside \p list. - //! - //! \param list List containing VisualMissionItems. - //! \return Returns true if the home position was updated, false either. - bool updateHomePosition(QmlObjectListModel &list); - } // namespace detail } // namespace Utils } // namespace WaypointManager diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 2fbd1d926..c33dc0594 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -128,6 +128,10 @@ QmlObjectListModel *WimaController::visualItems() { return &_areas; } +WimaDataContainer *WimaController::dataContainer() { + return _container; +} + QmlObjectListModel *WimaController::missionItems() { return const_cast(&_currentManager.missionItems()); } @@ -146,6 +150,42 @@ QVariantList WimaController::currentWaypointPath() return const_cast(_currentManager.currentWaypointsVariant()); } +Fact *WimaController::enableWimaController() { + return &_enableWimaController; +} + +Fact *WimaController::overlapWaypoints() { + return &_overlapWaypoints; +} + +Fact *WimaController::maxWaypointsPerPhase() { + return &_maxWaypointsPerPhase; +} + +Fact *WimaController::startWaypointIndex() { + return &_nextPhaseStartWaypointIndex; +} + +Fact *WimaController::showAllMissionItems() { + return &_showAllMissionItems; +} + +Fact *WimaController::showCurrentMissionItems() { + return &_showCurrentMissionItems; +} + +Fact *WimaController::flightSpeed() { + return &_flightSpeed; +} + +Fact *WimaController::arrivalReturnSpeed() { + return &_arrivalReturnSpeed; +} + +Fact *WimaController::altitude() { + return &_altitude; +} + //QStringList WimaController::loadNameFilters() const //{ // QStringList filters; @@ -170,12 +210,12 @@ bool WimaController::uploadOverrideRequired() const double WimaController::phaseDistance() const { - return _phaseDistance; + return 0.0; } double WimaController::phaseDuration() const { - return _phaseDuration; + return 0.0; } bool WimaController::vehicleHasLowBattery() const @@ -331,7 +371,7 @@ bool WimaController::calcReturnPath() // qgcApp()->showMessage(errorString); // return false; // } -// return true; + return true; } void WimaController::executeSmartRTL() @@ -483,18 +523,19 @@ bool WimaController::_fetchContainerData() // extract mission items QList tempMissionItems = planData.missionItems(); if (tempMissionItems.size() < 1) { - assert(false); + qWarning("WimaController: Mission items from WimaPlaner empty!"); return false; } - for (auto item : tempMissionItems) + for (auto item : tempMissionItems) { _defaultManager.push_back(item.coordinate()); + } _managerSettings.setHomePosition( QGeoCoordinate(_serviceArea.center().latitude(), _serviceArea.center().longitude(), 0) ); - if( !_defaultManager.update() ){ + if( !_defaultManager.reset() ){ assert(false); return false; } @@ -586,13 +627,13 @@ bool WimaController::_calcNextPhase() << "overlap=" << _currentManager.overlap() << "N=" << _currentManager.N() << "startIndex=" << _currentManager.startIndex(); - bool value; - _currentManager.setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)); - Q_ASSERT(value); - (void)value; - qDebug() << "overlap=" << _currentManager.overlap() - << "N=" << _currentManager.N() - << "startIndex=" << _currentManager.startIndex(); +// bool value; +// _currentManager.setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)); +// Q_ASSERT(value); +// (void)value; +// qDebug() << "overlap=" << _currentManager.overlap() +// << "N=" << _currentManager.N() +// << "startIndex=" << _currentManager.startIndex(); if ( !_currentManager.next() ) { assert(false); @@ -755,6 +796,7 @@ void WimaController::_eventTimerHandler() void WimaController::_smartRTLCleanUp(bool flying) { + (void)flying; // if ( !flying) { // vehicle has landed // if (_executingSmartRTL) { @@ -781,20 +823,22 @@ void WimaController::_enableDisableLowBatteryHandling(QVariant enable) void WimaController::_setPhaseDistance(double distance) { - if (!qFuzzyCompare(distance, _phaseDistance)) { - _phaseDistance = distance; + (void)distance; +// if (!qFuzzyCompare(distance, _phaseDistance)) { +// _phaseDistance = distance; - emit phaseDistanceChanged(); - } +// emit phaseDistanceChanged(); +// } } void WimaController::_setPhaseDuration(double duration) { - if (!qFuzzyCompare(duration, _phaseDuration)) { - _phaseDuration = duration; + (void)duration; +// if (!qFuzzyCompare(duration, _phaseDuration)) { +// _phaseDuration = duration; - emit phaseDurationChanged(); - } +// emit phaseDurationChanged(); +// } } bool WimaController::_checkSmartRTLPreCondition(QString &errorString) @@ -977,9 +1021,3 @@ void WimaController::_progressFromJson(JsonDocUPtr pDoc, emit WimaController::nemoProgressChanged(); } -template<> -QVariant getCoordinate(const SimpleMissionItem* item) -{ - return QVariant::fromValue(item->coordinate()); -} - diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 8505cf76c..bf603b043 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -213,20 +213,20 @@ public: // QStringList saveNameFilters (void) const; // QString fileExtension (void) const { return wimaFileExtension; } QGCMapPolygon joinedArea (void) const; - WimaDataContainer* dataContainer (void) { return _container; } + WimaDataContainer* dataContainer (void); QmlObjectListModel* missionItems (void); QmlObjectListModel* currentMissionItems (void); QVariantList waypointPath (void); QVariantList currentWaypointPath (void); - Fact* enableWimaController (void) { return &_enableWimaController; } - Fact* overlapWaypoints (void) { return &_overlapWaypoints; } - Fact* maxWaypointsPerPhase (void) { return &_maxWaypointsPerPhase; } - Fact* startWaypointIndex (void) { return &_nextPhaseStartWaypointIndex; } - Fact* showAllMissionItems (void) { return &_showAllMissionItems; } - Fact* showCurrentMissionItems(void) { return &_showCurrentMissionItems; } - Fact* flightSpeed (void) { return &_flightSpeed; } - Fact* arrivalReturnSpeed (void) { return &_arrivalReturnSpeed; } - Fact* altitude (void) { return &_altitude; } + Fact* enableWimaController (void); + Fact* overlapWaypoints (void); + Fact* maxWaypointsPerPhase (void); + Fact* startWaypointIndex (void); + Fact* showAllMissionItems (void); + Fact* showCurrentMissionItems(void); + Fact* flightSpeed (void); + Fact* arrivalReturnSpeed (void); + Fact* altitude (void); Fact* enableSnake (void) { return &_enableSnake; } Fact* snakeTileWidth (void) { return &_snakeTileWidth;} @@ -260,9 +260,9 @@ public: Q_INVOKABLE bool uploadToVehicle(); Q_INVOKABLE bool forceUploadToVehicle(); Q_INVOKABLE void removeFromVehicle(); - Q_INVOKABLE bool checkSmartRTLPreCondition(); // wrapper for _checkSmartRTLPreCondition(QString &errorString) - Q_INVOKABLE bool calcReturnPath(); // wrapper for _calcReturnPath(QString &errorSring)# - Q_INVOKABLE void executeSmartRTL(); // wrapper for _executeSmartRTL(QString &errorSring) + Q_INVOKABLE bool checkSmartRTLPreCondition(); + Q_INVOKABLE bool calcReturnPath(); + Q_INVOKABLE void executeSmartRTL(); Q_INVOKABLE void initSmartRTL(); Q_INVOKABLE void removeVehicleTrajectoryHistory(); @@ -446,81 +446,4 @@ private: }; -/* - * The following explains the structure of - * _missionController.visualItems(). - * - * Index Description - * -------------------------------------------- - * 0 MissionSettingsItem - * 1 Takeoff Command - * 2 Speed Command: arrivalReturnSpeed - * 3 Arrival Path Waypoint 0 - * ... - * 3+n-1 Arrival Path Waypoint n-1 - * 3+n Speed Command: flightSpeed - * 3+n+1 Circular Survey Waypoint 0 - * ... - * 3+n+m Circular Survey Waypoint m-1 - * 3+n+m+1 Speed Command: arrivalReturnSpeed - * 3+n+m+2 Return Path Waypoint 0 - * ... - * 3+n+m+2+l Return Path Waypoint l-1 - * 3+n+m+2+l+1 Land command - * - * _currentMissionItems is equal to - * _missionController.visualItems() except that it - * is missing the MissionSettingsItem - */ - - - -template -CoordinateType getCoordinate(const SimpleMissionItem* item){ - return item->coordinate(); -} - -template<> -QVariant getCoordinate(const SimpleMissionItem* item); - - -/// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList. -template class ContainerType> -bool getCoordinates(QmlObjectListModel &missionItems, - ContainerType &coordinateList, - std::size_t startIndex, - std::size_t endIndex){ - - if ( startIndex < std::size_t(missionItems.count()) - && endIndex < std::size_t(missionItems.count()) ) { - if (startIndex > endIndex) { - if (!getCoordinates(missionItems, coordinateList, startIndex, missionItems.count()-1)) - return false; - if (!getCoordinates(missionItems, coordinateList, 0, endIndex)) - return false; - } else { - for (std::size_t i = startIndex; i <= endIndex; i++) { - SimpleMissionItem *mItem = missionItems.value(int(i)); - - if (mItem == nullptr) { - coordinateList.clear(); - return false; - } - coordinateList.append(getCoordinate(mItem)); - } - } - } else - return false; - - return true; -} - -/// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList. -template class ContainerType> -bool getCoordinates(QmlObjectListModel &missionItems, - ContainerType &coordinateList){ - - return getCoordinates(missionItems, coordinateList, 0, missionItems.count()); -} - -- 2.22.0