diff --git a/src/FirmwarePlugin/APM/MavCmdInfoFixedWing.json b/src/FirmwarePlugin/APM/MavCmdInfoFixedWing.json index 77a8d80..c93a456 100644 --- a/src/FirmwarePlugin/APM/MavCmdInfoFixedWing.json +++ b/src/FirmwarePlugin/APM/MavCmdInfoFixedWing.json @@ -1,6 +1,167 @@ { "version": 1, - "mavCmdInfo": [ + { + "id": 16, + "rawName": "MAV_CMD_NAV_WAYPOINT", + "friendlyName": "Waypoint", + "description": "Travel to a position in 3D space.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "category": "Basic", + "param1": { + "label": "Hold:", + "units": "seconds", + "default": 0, + "decimalPlaces": 0 + }, + "param7": { + "label": "Altitude:", + "units": "meters", + "default": 50, + "decimalPlaces": 0 + } + }, + { + "id": 17, + "rawName": "MAV_CMD_NAV_LOITER_UNLIM", + "friendlyName": "Loiter", + "description": "Travel to a position and Loiter around the specified radius indefinitely.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "category": "Loiter", + "param3": { + "label": "Radius:", + "units": "meters", + "default": 100, + "decimalPlaces": 0 + }, + "param7": { + "label": "Altitude:", + "units": "meters", + "default": 50, + "decimalPlaces": 0 + } + }, + { + "id": 18, + "rawName": "MAV_CMD_NAV_LOITER_TURNS", + "friendlyName": "Loiter (turns)", + "description": "Travel to a position and Loiter around the specified radius for a number of turns.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "category": "Loiter", + "param1": { + "label": "Turns:", + "default": 1, + "decimalPlaces": 0 + }, + "param3": { + "label": "Radius:", + "units": "meters", + "default": 100, + "decimalPlaces": 0 + }, + "param7": { + "label": "Altitude:", + "units": "meters", + "default": 50, + "decimalPlaces": 0 + } + }, + { + "id": 19, + "rawName": "MAV_CMD_NAV_LOITER_TIME", + "friendlyName": "Loiter (time)", + "description": "Travel to a position and Loiter around the specified radius for a number of seconds.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "category": "Loiter", + "param1": { + "label": "Seconds:", + "units": "seconds", + "default": 30, + "decimalPlaces": 1 + }, + "param3": { + "label": "Radius:", + "units": "meters", + "default": 100, + "decimalPlaces": 0 + }, + "param7": { + "label": "Altitude:", + "units": "meters", + "default": 50, + "decimalPlaces": 0 + } + }, + { + "id": 21, + "rawName": "MAV_CMD_NAV_LAND", + "friendlyName": "Land", + "description": "Land vehicle at the specified location.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "category": "Basic", + "param1": { + "label": "Abort Alt:", + "units": "meters", + "default": 25, + "decimalPlaces": 0 + }, + "param4": { + "label": "Heading:", + "units": "radians", + "default": 0, + "decimalPlaces": 2 + }, + "param7": { + "label": "Altitude:", + "units": "meters", + "default": 0, + "decimalPlaces": 1 + } + }, + { + "id": 22, + "rawName": "MAV_CMD_NAV_TAKEOFF", + "friendlyName": "Takeoff", + "description": "Take off from the ground and travel towards the specified position.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "category": "Basic", + "param1": { + "label": "Pitch:", + "units": "degrees", + "default": 0, + "decimalPlaces": 0 + }, + "param4": { + "label": "Heading:", + "units": "radians", + "default": 0, + "decimalPlaces": 1 + }, + "param7": { + "label": "Altitude:", + "units": "meters", + "default": 25, + "decimalPlaces": 0 + } + }, + { + "id": 206, + "rawName": "MAV_CMD_DO_SET_CAM_TRIGG_DIST", + "friendlyName": "Camera trigger distance", + "description": "Set camera trigger distance.", + "category": "Camera", + "param1": { + "label": "Distance:", + "default": 25, + "units": "meters", + "decimalPlaces": 1 + } + } ] } diff --git a/src/FirmwarePlugin/PX4/MavCmdInfoFixedWing.json b/src/FirmwarePlugin/PX4/MavCmdInfoFixedWing.json index 77a8d80..c93a456 100644 --- a/src/FirmwarePlugin/PX4/MavCmdInfoFixedWing.json +++ b/src/FirmwarePlugin/PX4/MavCmdInfoFixedWing.json @@ -1,6 +1,167 @@ { "version": 1, - "mavCmdInfo": [ + { + "id": 16, + "rawName": "MAV_CMD_NAV_WAYPOINT", + "friendlyName": "Waypoint", + "description": "Travel to a position in 3D space.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "category": "Basic", + "param1": { + "label": "Hold:", + "units": "seconds", + "default": 0, + "decimalPlaces": 0 + }, + "param7": { + "label": "Altitude:", + "units": "meters", + "default": 50, + "decimalPlaces": 0 + } + }, + { + "id": 17, + "rawName": "MAV_CMD_NAV_LOITER_UNLIM", + "friendlyName": "Loiter", + "description": "Travel to a position and Loiter around the specified radius indefinitely.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "category": "Loiter", + "param3": { + "label": "Radius:", + "units": "meters", + "default": 100, + "decimalPlaces": 0 + }, + "param7": { + "label": "Altitude:", + "units": "meters", + "default": 50, + "decimalPlaces": 0 + } + }, + { + "id": 18, + "rawName": "MAV_CMD_NAV_LOITER_TURNS", + "friendlyName": "Loiter (turns)", + "description": "Travel to a position and Loiter around the specified radius for a number of turns.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "category": "Loiter", + "param1": { + "label": "Turns:", + "default": 1, + "decimalPlaces": 0 + }, + "param3": { + "label": "Radius:", + "units": "meters", + "default": 100, + "decimalPlaces": 0 + }, + "param7": { + "label": "Altitude:", + "units": "meters", + "default": 50, + "decimalPlaces": 0 + } + }, + { + "id": 19, + "rawName": "MAV_CMD_NAV_LOITER_TIME", + "friendlyName": "Loiter (time)", + "description": "Travel to a position and Loiter around the specified radius for a number of seconds.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "category": "Loiter", + "param1": { + "label": "Seconds:", + "units": "seconds", + "default": 30, + "decimalPlaces": 1 + }, + "param3": { + "label": "Radius:", + "units": "meters", + "default": 100, + "decimalPlaces": 0 + }, + "param7": { + "label": "Altitude:", + "units": "meters", + "default": 50, + "decimalPlaces": 0 + } + }, + { + "id": 21, + "rawName": "MAV_CMD_NAV_LAND", + "friendlyName": "Land", + "description": "Land vehicle at the specified location.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "category": "Basic", + "param1": { + "label": "Abort Alt:", + "units": "meters", + "default": 25, + "decimalPlaces": 0 + }, + "param4": { + "label": "Heading:", + "units": "radians", + "default": 0, + "decimalPlaces": 2 + }, + "param7": { + "label": "Altitude:", + "units": "meters", + "default": 0, + "decimalPlaces": 1 + } + }, + { + "id": 22, + "rawName": "MAV_CMD_NAV_TAKEOFF", + "friendlyName": "Takeoff", + "description": "Take off from the ground and travel towards the specified position.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "category": "Basic", + "param1": { + "label": "Pitch:", + "units": "degrees", + "default": 0, + "decimalPlaces": 0 + }, + "param4": { + "label": "Heading:", + "units": "radians", + "default": 0, + "decimalPlaces": 1 + }, + "param7": { + "label": "Altitude:", + "units": "meters", + "default": 25, + "decimalPlaces": 0 + } + }, + { + "id": 206, + "rawName": "MAV_CMD_DO_SET_CAM_TRIGG_DIST", + "friendlyName": "Camera trigger distance", + "description": "Set camera trigger distance.", + "category": "Camera", + "param1": { + "label": "Distance:", + "default": 25, + "units": "meters", + "decimalPlaces": 1 + } + } ] } diff --git a/src/MissionManager/MavCmdInfoCommon.json b/src/MissionManager/MavCmdInfoCommon.json index b5d9bf8..69bf14f 100644 --- a/src/MissionManager/MavCmdInfoCommon.json +++ b/src/MissionManager/MavCmdInfoCommon.json @@ -409,7 +409,7 @@ }, "param2": { "label": "Repeat:", - "default": 0, + "default": 10, "decimalPlaces": 0 } }, diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 5938dfa..4c6001e 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -987,11 +987,18 @@ bool MissionController::_findLastAltitude(double* lastAltitude) // Don't use home position for (int i=1; i<_visualItems->count(); i++) { - VisualMissionItem* item = qobject_cast(_visualItems->get(i)); + VisualMissionItem* visualItem = qobject_cast(_visualItems->get(i)); - if (item->specifiesCoordinate() && !item->isStandaloneCoordinate()) { - foundAltitude = item->exitCoordinate().altitude(); + if (visualItem->specifiesCoordinate() && !visualItem->isStandaloneCoordinate()) { + foundAltitude = visualItem->exitCoordinate().altitude(); found = true; + + if (visualItem->isSimpleItem()) { + SimpleMissionItem* simpleItem = qobject_cast(visualItem); + if ((MAV_CMD)simpleItem->command() == MAV_CMD_NAV_TAKEOFF) { + found = false; + } + } } }