diff --git a/src/FirmwarePlugin/APM/MavCmdInfoCommon.json b/src/FirmwarePlugin/APM/MavCmdInfoCommon.json index 77a8d80..8e6320d 100644 --- a/src/FirmwarePlugin/APM/MavCmdInfoCommon.json +++ b/src/FirmwarePlugin/APM/MavCmdInfoCommon.json @@ -2,5 +2,20 @@ "version": 1, "mavCmdInfo": [ + { + "id": 22, + "rawName": "MAV_CMD_NAV_TAKEOFF", + "friendlyName": "Takeoff", + "description": "Take off from the ground.", + "specifiesCoordinate": false, + "friendlyEdit": true, + "category": "Basic", + "param1": { + "label": "Pitch:", + "units": "radians", + "default": 0.26179939, + "decimalPlaces": 2 + } + } ] } diff --git a/src/FirmwarePlugin/APM/MavCmdInfoFixedWing.json b/src/FirmwarePlugin/APM/MavCmdInfoFixedWing.json index 8e6320d..77a8d80 100644 --- a/src/FirmwarePlugin/APM/MavCmdInfoFixedWing.json +++ b/src/FirmwarePlugin/APM/MavCmdInfoFixedWing.json @@ -2,20 +2,5 @@ "version": 1, "mavCmdInfo": [ - { - "id": 22, - "rawName": "MAV_CMD_NAV_TAKEOFF", - "friendlyName": "Takeoff", - "description": "Take off from the ground.", - "specifiesCoordinate": false, - "friendlyEdit": true, - "category": "Basic", - "param1": { - "label": "Pitch:", - "units": "radians", - "default": 0.26179939, - "decimalPlaces": 2 - } - } ] } diff --git a/src/MissionManager/MissionCommands.cc b/src/MissionManager/MissionCommands.cc index e16831c..2266372 100644 --- a/src/MissionManager/MissionCommands.cc +++ b/src/MissionManager/MissionCommands.cc @@ -153,13 +153,13 @@ MavCmdInfo* MissionCommands::getMavCmdInfo(MAV_CMD command, Vehicle* vehicle) co if (_autopilotToMultiRotorMissionCommands[firmwareType]->contains(command)) { mavCmdInfo = _autopilotToMultiRotorMissionCommands[firmwareType]->getMavCmdInfo(command); } - } else { - if (_autopilotToCommonMissionCommands[firmwareType]->contains(command)) { - mavCmdInfo = _autopilotToCommonMissionCommands[firmwareType]->getMavCmdInfo(command); - } } } + if (!mavCmdInfo && _autopilotToCommonMissionCommands[firmwareType]->contains(command)) { + mavCmdInfo = _autopilotToCommonMissionCommands[firmwareType]->getMavCmdInfo(command); + } + if (!mavCmdInfo) { mavCmdInfo = _commonMissionCommands.getMavCmdInfo(command); } diff --git a/src/MissionManager/MissionItem.cc b/src/MissionManager/MissionItem.cc index 3d3dd63..938ae79 100644 --- a/src/MissionManager/MissionItem.cc +++ b/src/MissionManager/MissionItem.cc @@ -813,7 +813,8 @@ void MissionItem::setDefaultsForCommand(void) MAV_CMD command = (MAV_CMD)this->command(); if (_missionCommands->contains(command)) { - foreach (const MavCmdParamInfo* paramInfo, _missionCommands->getMavCmdInfo(command, _vehicle)->paramInfoMap()) { + MavCmdInfo* mavCmdInfo = _missionCommands->getMavCmdInfo(command, _vehicle); + foreach (const MavCmdParamInfo* paramInfo, mavCmdInfo->paramInfoMap()) { Fact* rgParamFacts[7] = { &_param1Fact, &_param2Fact, &_param3Fact, &_param4Fact, &_param5Fact, &_param6Fact, &_param7Fact }; rgParamFacts[paramInfo->param()-1]->setRawValue(paramInfo->defaultValue()); diff --git a/src/MissionManager/MissionManagerTest.cc b/src/MissionManager/MissionManagerTest.cc index 9a37956..6d31f09 100644 --- a/src/MissionManager/MissionManagerTest.cc +++ b/src/MissionManager/MissionManagerTest.cc @@ -31,9 +31,7 @@ const MissionManagerTest::TestCase_t MissionManagerTest::_rgTestCases[] = { { "2\t0\t3\t18\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 2, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_TURNS, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } }, { "3\t0\t3\t19\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 3, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_TIME, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } }, { "4\t0\t3\t21\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 4, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LAND, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } }, - { "5\t0\t3\t22\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 5, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_TAKEOFF, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } }, - { "6\t0\t2\t112\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 6, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_CONDITION_DELAY, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_MISSION } }, - { "7\t0\t2\t177\t3\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 7, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_DO_JUMP, 3, 20.0, 30.0, 40.0, true, false, MAV_FRAME_MISSION } }, + { "6\t0\t2\t112\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 5, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_CONDITION_DELAY, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_MISSION } }, }; const size_t MissionManagerTest::_cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]);