diff --git a/integrationtests/python_src/px4_it/mavros/missions/FW_mission_1.plan b/integrationtests/python_src/px4_it/mavros/missions/FW_mission_1.plan
index 67bbed1927..e8dd595993 100644
--- a/integrationtests/python_src/px4_it/mavros/missions/FW_mission_1.plan
+++ b/integrationtests/python_src/px4_it/mavros/missions/FW_mission_1.plan
@@ -14,7 +14,7 @@
         "hoverSpeed": 5,
         "items": [
             {
-                "AMSLAltAboveTerrain": null,
+                "AMSLAltAboveTerrain": 50,
                 "Altitude": 50,
                 "AltitudeMode": 0,
                 "autoContinue": true,
@@ -26,14 +26,14 @@
                     0,
                     0,
                     null,
-                    47.397856910618785,
-                    8.542576533916815,
+                    47.397757116360204,
+                    8.54802567938998,
                     50
                 ],
                 "type": "SimpleItem"
             },
             {
-                "AMSLAltAboveTerrain": null,
+                "AMSLAltAboveTerrain": 50,
                 "Altitude": 50,
                 "AltitudeMode": 0,
                 "autoContinue": true,
@@ -45,8 +45,8 @@
                     0,
                     0,
                     null,
-                    47.39602683906673,
-                    8.539868609985234,
+                    47.395834920202326,
+                    8.548141558196932,
                     50
                 ],
                 "type": "SimpleItem"
@@ -68,7 +68,7 @@
                 "type": "SimpleItem"
             },
             {
-                "AMSLAltAboveTerrain": null,
+                "AMSLAltAboveTerrain": 25,
                 "Altitude": 25,
                 "AltitudeMode": 0,
                 "autoContinue": true,
@@ -80,14 +80,14 @@
                     50,
                     0,
                     1,
-                    47.39537936180079,
-                    8.543743516831427,
+                    47.39580995751514,
+                    8.545613891100459,
                     25
                 ],
                 "type": "SimpleItem"
             },
             {
-                "AMSLAltAboveTerrain": null,
+                "AMSLAltAboveTerrain": 0,
                 "Altitude": 0,
                 "AltitudeMode": 0,
                 "autoContinue": true,
@@ -99,8 +99,8 @@
                     0,
                     0,
                     null,
-                    47.39733554489375,
-                    8.545863573918894,
+                    47.39757125904106,
+                    8.545589014690734,
                     0
                 ],
                 "type": "SimpleItem"
@@ -109,7 +109,7 @@
         "plannedHomePosition": [
             47.397742,
             8.545594,
-            488
+            489
         ],
         "vehicleType": 1,
         "version": 2
diff --git a/integrationtests/python_src/px4_it/mavros/missions/VTOL_mission_1.plan b/integrationtests/python_src/px4_it/mavros/missions/VTOL_mission_1.plan
index 6aee1738b0..fc0f17560b 100644
--- a/integrationtests/python_src/px4_it/mavros/missions/VTOL_mission_1.plan
+++ b/integrationtests/python_src/px4_it/mavros/missions/VTOL_mission_1.plan
@@ -14,7 +14,7 @@
         "hoverSpeed": 5,
         "items": [
             {
-                "AMSLAltAboveTerrain": null,
+                "AMSLAltAboveTerrain": 15,
                 "Altitude": 15,
                 "AltitudeMode": 0,
                 "autoContinue": true,
@@ -26,37 +26,37 @@
                     0,
                     0,
                     null,
-                    47.397731862593744,
-                    8.543269295853861,
+                    47.39773615102264,
+                    8.547064163497595,
                     15
                 ],
                 "type": "SimpleItem"
             },
+            {
+                "AMSLAltAboveTerrain": null,
+                "Altitude": 30,
+                "AltitudeMode": 0,
+                "autoContinue": true,
+                "command": 31,
+                "doJumpId": 2,
+                "frame": 3,
+                "params": [
+                    0,
+                    50,
+                    0,
+                    1,
+                    47.396840504834636,
+                    8.548309212214804,
+                    30
+                ],
+                "type": "SimpleItem"
+            },
             {
                 "AMSLAltAboveTerrain": null,
                 "Altitude": 15,
                 "AltitudeMode": 0,
                 "autoContinue": true,
                 "command": 16,
-                "doJumpId": 2,
-                "frame": 3,
-                "params": [
-                    0,
-                    0,
-                    0,
-                    null,
-                    47.39666467622327,
-                    8.540909537045792,
-                    15
-                ],
-                "type": "SimpleItem"
-            },
-            {
-                "AMSLAltAboveTerrain": null,
-                "Altitude": 0,
-                "AltitudeMode": 0,
-                "autoContinue": true,
-                "command": 85,
                 "doJumpId": 3,
                 "frame": 3,
                 "params": [
@@ -64,8 +64,27 @@
                     0,
                     0,
                     null,
-                    47.39615608503663,
-                    8.54513863086865,
+                    47.39617583578575,
+                    8.546774195044463,
+                    15
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 0,
+                "Altitude": 0,
+                "AltitudeMode": 0,
+                "autoContinue": true,
+                "command": 85,
+                "doJumpId": 4,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.39706525003061,
+                    8.545322355693173,
                     0
                 ],
                 "type": "SimpleItem"
@@ -74,7 +93,7 @@
         "plannedHomePosition": [
             47.397742,
             8.545594,
-            488
+            489
         ],
         "vehicleType": 20,
         "version": 2