diff --git a/src/AutoPilotPlugins/Common/ESP8266ComponentController.cc b/src/AutoPilotPlugins/Common/ESP8266ComponentController.cc index 576fcc8f2c1b5f7bcbd4d7c2dc10799a009f6886..c2e83626dd2703fd76184b81f1e13f5a4eb288f6 100644 --- a/src/AutoPilotPlugins/Common/ESP8266ComponentController.cc +++ b/src/AutoPilotPlugins/Common/ESP8266ComponentController.cc @@ -48,8 +48,7 @@ ESP8266ComponentController::ESP8266ComponentController() _baudRates.append("460800"); _baudRates.append("921600"); connect(&_timer, &QTimer::timeout, this, &ESP8266ComponentController::_processTimeout); - UASInterface* uas = dynamic_cast(_vehicle->uas()); - connect(uas, &UASInterface::commandAck, this, &ESP8266ComponentController::_commandAck); + connect(_vehicle, &Vehicle::commandLongAck, this, &ESP8266ComponentController::_commandAck); Fact* ssid = getParameterFact(MAV_COMP_ID_UDP_BRIDGE, "WIFI_SSID4"); connect(ssid, &Fact::valueChanged, this, &ESP8266ComponentController::_ssidChanged); Fact* paswd = getParameterFact(MAV_COMP_ID_UDP_BRIDGE, "WIFI_PASSWORD4"); @@ -286,7 +285,7 @@ ESP8266ComponentController::_processTimeout() //----------------------------------------------------------------------------- void -ESP8266ComponentController::_commandAck(UASInterface*, uint8_t compID, uint16_t command, uint8_t result) +ESP8266ComponentController::_commandAck(uint8_t compID, uint16_t command, uint8_t result) { if(compID == MAV_COMP_ID_UDP_BRIDGE) { if(result != MAV_RESULT_ACCEPTED) { diff --git a/src/AutoPilotPlugins/Common/ESP8266ComponentController.h b/src/AutoPilotPlugins/Common/ESP8266ComponentController.h index 72dff94c4fe6c92efc73d804707160a77522e4c4..1256f9bdb6009ec7b5d2099b57c421c855ea6922 100644 --- a/src/AutoPilotPlugins/Common/ESP8266ComponentController.h +++ b/src/AutoPilotPlugins/Common/ESP8266ComponentController.h @@ -86,7 +86,7 @@ signals: private slots: void _processTimeout (); - void _commandAck (UASInterface* uas, uint8_t compID, uint16_t command, uint8_t result); + void _commandAck (uint8_t compID, uint16_t command, uint8_t result); void _ssidChanged (QVariant value); void _passwordChanged (QVariant value); void _baudChanged (QVariant value); diff --git a/src/MissionManager/MavCmdInfoCommon.json b/src/MissionManager/MavCmdInfoCommon.json index ac0d766eb7447f4b5d0abb816367b9f8672138f1..b5d9bf8d28c1a35ff5a10cbe68f1bb7c186e3a87 100644 --- a/src/MissionManager/MavCmdInfoCommon.json +++ b/src/MissionManager/MavCmdInfoCommon.json @@ -149,9 +149,9 @@ "decimalPlaces": 2 } }, - { "id": 23, "rawName": "MAV_CMD_NAV_LAND_LOCAL", "friendlyName": "MAV_CMD_NAV_LAND_LOCAL" }, - { "id": 24, "rawName": "MAV_CMD_NAV_TAKEOFF_LOCAL", "friendlyName": "MAV_CMD_NAV_TAKEOFF_LOCAL" }, - { "id": 25, "rawName": "MAV_CMD_NAV_FOLLOW", "friendlyName": "MAV_CMD_NAV_FOLLOW" }, + { "id": 23, "rawName": "MAV_CMD_NAV_LAND_LOCAL", "friendlyName": "Land local" }, + { "id": 24, "rawName": "MAV_CMD_NAV_TAKEOFF_LOCAL", "friendlyName": "Takeoff local" }, + { "id": 25, "rawName": "MAV_CMD_NAV_FOLLOW", "friendlyName": "Nav follow" }, { "id": 30, "rawName": "MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT", @@ -196,6 +196,8 @@ "decimalPlaces": 2 } }, + { "id": 32, "rawName": "MAV_CMD_DO_FOLLOW", "friendlyName": "Follow Me" }, + { "id": 33, "rawName": "MAV_CMD_DO_FOLLOW_REPOSITION", "friendlyName": "Vehicle reposition" }, { "id": 80, "rawName": "MAV_CMD_NAV_ROI", @@ -267,7 +269,7 @@ "decimalPlaces": 0 } }, - { "id": 83, "rawName": "MAV_CMD_NAV_ALTITUDE_WAIT", "friendlyName": "MAV_CMD_NAV_ALTITUDE_WAIT" }, + { "id": 83, "rawName": "MAV_CMD_NAV_ALTITUDE_WAIT", "friendlyName": "Altitude wait" }, { "id": 84, "rawName": "MAV_CMD_NAV_VTOL_TAKEOFF", @@ -451,7 +453,7 @@ "decimalPlaces": 0 } }, - { "id": 180, "rawName": "MAV_CMD_DO_SET_PARAMETER", "friendlyName": "MAV_CMD_DO_SET_PARAMETER" }, + { "id": 180, "rawName": "MAV_CMD_DO_SET_PARAMETER", "friendlyName": "Set Parameter" }, { "id": 181, "rawName": "MAV_CMD_DO_SET_RELAY", @@ -535,7 +537,7 @@ "decimalPlaces": 0 } }, - { "id": 185, "rawName": "MAV_CMD_DO_FLIGHTTERMINATION", "friendlyName": "MAV_CMD_DO_FLIGHTTERMINATION" }, + { "id": 185, "rawName": "MAV_CMD_DO_FLIGHTTERMINATION", "friendlyName": "Flight termination" }, { "id": 189, "rawName": "MAV_CMD_DO_LAND_START", @@ -546,9 +548,11 @@ "friendlyEdit": true, "category": "Basic" }, - { "id": 190, "rawName": "MAV_CMD_DO_RALLY_LAND", "friendlyName": "MAV_CMD_DO_RALLY_LAND" }, - { "id": 191, "rawName": "MAV_CMD_DO_GO_AROUND", "friendlyName": "MAV_CMD_DO_GO_AROUND" }, - { "id": 200, "rawName": "MAV_CMD_DO_CONTROL_VIDEO", "friendlyName": "MAV_CMD_DO_CONTROL_VIDEO" }, + { "id": 190, "rawName": "MAV_CMD_DO_RALLY_LAND", "friendlyName": "Rally land" }, + { "id": 191, "rawName": "MAV_CMD_DO_GO_AROUND", "friendlyName": "Go around" }, + { "id": 192, "rawName": "MAV_CMD_DO_REPOSITION", "friendlyName": "Reposition" }, + { "id": 193, "rawName": "MAV_CMD_DO_PAUSE_CONTINUE", "friendlyName": "Pause/Continue" }, + { "id": 200, "rawName": "MAV_CMD_DO_CONTROL_VIDEO", "friendlyName": "Control video" }, { "id": 201, "rawName": "MAV_CMD_DO_SET_ROI", @@ -657,7 +661,7 @@ "decimalPlaces": 0 } }, - { "id": 204, "rawName": "MAV_CMD_DO_MOUNT_CONFIGURE", "friendlyName": "MAV_CMD_DO_MOUNT_CONFIGURE" }, + { "id": 204, "rawName": "MAV_CMD_DO_MOUNT_CONFIGURE", "friendlyName": "Mount configure" }, { "id": 205, "rawName": "MAV_CMD_DO_MOUNT_CONTROL", @@ -732,7 +736,7 @@ "decimalPlaces": 0 } }, - { "id": 209, "rawName": "MAV_CMD_DO_MOTOR_TEST", "friendlyName": "MAV_CMD_DO_MOTOR_TEST" }, + { "id": 209, "rawName": "MAV_CMD_DO_MOTOR_TEST", "friendlyName": "Motor test" }, { "id": 210, "rawName": "MAV_CMD_DO_INVERTED_FLIGHT", @@ -802,15 +806,25 @@ "decimalPlaces": 2 } }, - { "id": 252, "rawName": "MAV_CMD_OVERRIDE_GOTO", "friendlyName": "MAV_CMD_OVERRIDE_GOTO" }, - { "id": 300, "rawName": "MAV_CMD_MISSION_START", "friendlyName": "MAV_CMD_MISSION_START" }, + { "id": 241, "rawName": "MAV_CMD_PREFLIGHT_CALIBRATION", "friendlyName": "Calibration" }, + { "id": 242, "rawName": "MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS", "friendlyName": "Set sensor offsets" }, + { "id": 243, "rawName": "MAV_CMD_PREFLIGHT_UAVCAN", "friendlyName": "UAVCAN configure" }, + { "id": 245, "rawName": "MAV_CMD_PREFLIGHT_STORAGE", "friendlyName": "Store parameters" }, + { "id": 246, "rawName": "MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN", "friendlyName": "Reboot/Shutdown vehicle" }, + { "id": 252, "rawName": "MAV_CMD_OVERRIDE_GOTO", "friendlyName": "Override goto" }, + { "id": 300, "rawName": "MAV_CMD_MISSION_START", "friendlyName": "Mission start" }, { "id": 400, "rawName": "MAV_CMD_COMPONENT_ARM_DISARM", "friendlyName": "Arm/Disarm" }, - { "id": 2000, "rawName": "MAV_CMD_IMAGE_START_CAPTURE", "friendlyName": "MAV_CMD_IMAGE_START_CAPTURE" }, - { "id": 2001, "rawName": "MAV_CMD_IMAGE_STOP_CAPTURE", "friendlyName": "MAV_CMD_IMAGE_STOP_CAPTURE" }, - { "id": 2003, "rawName": "MAV_CMD_DO_TRIGGER_CONTROL", "friendlyName": "MAV_CMD_DO_TRIGGER_CONTROL" }, - { "id": 2500, "rawName": "MAV_CMD_VIDEO_START_CAPTURE", "friendlyName": "MAV_CMD_VIDEO_START_CAPTURE" }, - { "id": 2501, "rawName": "MAV_CMD_VIDEO_STOP_CAPTURE", "friendlyName": "MAV_CMD_VIDEO_STOP_CAPTURE" }, - { "id": 2800, "rawName": "MAV_CMD_PANORAMA_CREATE", "friendlyName": "MAV_CMD_PANORAMA_CREATE" }, + { "id": 410, "rawName": "MAV_CMD_GET_HOME_POSITION", "friendlyName": "Get home position" }, + { "id": 500, "rawName": "MAV_CMD_START_RX_PAIR", "friendlyName": "Bind Spektrum receiver" }, + { "id": 510, "rawName": "MAV_CMD_GET_MESSAGE_INTERVAL", "friendlyName": "Get message interval" }, + { "id": 511, "rawName": "MAV_CMD_SET_MESSAGE_INTERVAL", "friendlyName": "Set message interval" }, + { "id": 520, "rawName": "MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES", "friendlyName": "Get capabilities" }, + { "id": 2000, "rawName": "MAV_CMD_IMAGE_START_CAPTURE", "friendlyName": "Start image capture" }, + { "id": 2001, "rawName": "MAV_CMD_IMAGE_STOP_CAPTURE", "friendlyName": "Stop image capture" }, + { "id": 2003, "rawName": "MAV_CMD_DO_TRIGGER_CONTROL", "friendlyName": "Trigger control" }, + { "id": 2500, "rawName": "MAV_CMD_VIDEO_START_CAPTURE", "friendlyName": "Start video capture" }, + { "id": 2501, "rawName": "MAV_CMD_VIDEO_STOP_CAPTURE", "friendlyName": "Stop video cpture" }, + { "id": 2800, "rawName": "MAV_CMD_PANORAMA_CREATE", "friendlyName": "Create panorama" }, { "id": 3000, "rawName": "MAV_CMD_DO_VTOL_TRANSITION", @@ -825,7 +839,7 @@ "enumValues": "3,4" } }, - { "id": 30001, "rawName": "MAV_CMD_PAYLOAD_PREPARE_DEPLOY", "friendlyName": "MAV_CMD_PAYLOAD_PREPARE_DEPLOY" }, - { "id": 30002, "rawName": "MAV_CMD_PAYLOAD_CONTROL_DEPLOY", "friendlyName": "MAV_CMD_PAYLOAD_CONTROL_DEPLOY" } + { "id": 30001, "rawName": "MAV_CMD_PAYLOAD_PREPARE_DEPLOY", "friendlyName": "Payload prepare deploy" }, + { "id": 30002, "rawName": "MAV_CMD_PAYLOAD_CONTROL_DEPLOY", "friendlyName": "Payload control deploy" } ] } diff --git a/src/MissionManager/MissionCommandList.cc b/src/MissionManager/MissionCommandList.cc index d6429ddabaad315f2d476093d623d0126f8989f2..4ccca49119702011608a586f94361fb8140e75bb 100644 --- a/src/MissionManager/MissionCommandList.cc +++ b/src/MissionManager/MissionCommandList.cc @@ -243,7 +243,6 @@ bool MissionCommandList::contains(MAV_CMD command) const MavCmdInfo* MissionCommandList::getMavCmdInfo(MAV_CMD command) const { if (!contains(command)) { - qWarning() << "Unknown command" << command; return NULL; } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 6429f404626b9aafd6b40d5a5288288b98104694..277fd2936d7b2df1430974badbff93b6e4c43ea2 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -426,6 +426,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: _handleExtendedSysState(message); break; + case MAVLINK_MSG_ID_COMMAND_ACK: + _handleCommandAck(message); + break; // Following are ArduPilot dialect messages @@ -439,6 +442,40 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes _uas->receiveMessage(message); } +void Vehicle::_handleCommandAck(mavlink_message_t& message) +{ + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&message, &ack); + + emit commandLongAck(message.compid, ack.command, ack.result); + + QString commandName; + MavCmdInfo* cmdInfo = qgcApp()->toolbox()->missionCommands()->getMavCmdInfo((MAV_CMD)ack.command, this); + if (cmdInfo) { + commandName = cmdInfo->friendlyName(); + } else { + commandName = tr("cmdid %1").arg(ack.command); + } + + switch (ack.result) { + case MAV_RESULT_TEMPORARILY_REJECTED: + qgcApp()->showMessage(tr("%1 command temporarily rejected").arg(commandName)); + break; + case MAV_RESULT_DENIED: + qgcApp()->showMessage(tr("%1 command denied").arg(commandName)); + break; + case MAV_RESULT_UNSUPPORTED: + qgcApp()->showMessage(tr("%1 command not supported").arg(commandName)); + break; + case MAV_RESULT_FAILED: + qgcApp()->showMessage(tr("%1 command failed").arg(commandName)); + break; + default: + // Do nothing + break; + } +} + void Vehicle::_handleExtendedSysState(mavlink_message_t& message) { mavlink_extended_sys_state_t extendedState; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 94d0322198b8cd5320e11658c212e423498a9e7f..f0066eefd5e04b624e60c9d4e170487b531f3f8b 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -542,6 +542,7 @@ signals: void flyingChanged(bool flying); void guidedModeChanged(bool guidedMode); void prearmErrorChanged(const QString& prearmError); + void commandLongAck(uint8_t compID, uint16_t command, uint8_t result); void messagesReceivedChanged (); void messagesSentChanged (); @@ -620,6 +621,7 @@ private: void _handleWind(mavlink_message_t& message); void _handleVibration(mavlink_message_t& message); void _handleExtendedSysState(mavlink_message_t& message); + void _handleCommandAck(mavlink_message_t& message); void _missionManagerError(int errorCode, const QString& errorMsg); void _mapTrajectoryStart(void); void _mapTrajectoryStop(void); diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 9d161c6cf0c9f3b0afb5c33f4bfd99c6aec6a664..5ba7d40a197d6205ba61113200b75140def7a959 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -786,6 +786,7 @@ void MockLink::_handleFTP(const mavlink_message_t& msg) void MockLink::_handleCommandLong(const mavlink_message_t& msg) { mavlink_command_long_t request; + uint8_t commandResult; mavlink_msg_command_long_decode(&msg, &request); @@ -795,7 +796,18 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg) } else { _mavBaseMode |= MAV_MODE_FLAG_SAFETY_ARMED; } + commandResult = MAV_RESULT_ACCEPTED; + } else { + commandResult = MAV_RESULT_UNSUPPORTED; } + + mavlink_message_t commandAck; + mavlink_msg_command_ack_pack(_vehicleSystemId, + _vehicleComponentId, + &commandAck, + request.command, + commandResult); + respondWithMavlinkMessage(commandAck); } void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 74475b0a70d4d1c9c447f6c4470259656edba0ae..1dfa1b48fae541b352b0d92ee5be82bb8ecb49bf 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -607,39 +607,6 @@ void UAS::receiveMessage(mavlink_message_t message) processParamValueMsg(message, parameterName,rawValue,paramVal); } break; - case MAVLINK_MSG_ID_COMMAND_ACK: - { - mavlink_command_ack_t ack; - mavlink_msg_command_ack_decode(&message, &ack); - - emit commandAck(this, message.compid, ack.command, ack.result); - - QString commandName; - MavCmdInfo* cmdInfo = qgcApp()->toolbox()->missionCommands()->getMavCmdInfo((MAV_CMD)ack.command, _vehicle); - if (cmdInfo) { - commandName = cmdInfo->friendlyName(); - } else { - commandName = ack.command; - } - - switch (ack.result) { - case MAV_RESULT_TEMPORARILY_REJECTED: - emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_WARNING, tr("%1 temporarily rejected").arg(commandName)); - break; - case MAV_RESULT_DENIED: - emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_ERROR, tr("%1 denied").arg(commandName)); - break; - case MAV_RESULT_UNSUPPORTED: - emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_WARNING, tr("%1 unsupported").arg(commandName)); - break; - case MAV_RESULT_FAILED: - emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_ERROR, tr("%1 failed").arg(commandName)); - break; - default: - // Do nothing - break; - } - } case MAVLINK_MSG_ID_ATTITUDE_TARGET: { mavlink_attitude_target_t out; diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 1e2e13732c729aacbf5a81e6c7d221b0e7cd100a..84cde7332b5175ac5ebc5aa2291b627083dff4e7 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -322,9 +322,6 @@ signals: // Log Download Signals void logEntry (UASInterface* uas, uint32_t time_utc, uint32_t size, uint16_t id, uint16_t num_logs, uint16_t last_log_num); void logData (UASInterface* uas, uint32_t ofs, uint16_t id, uint8_t count, const uint8_t* data); - - /** @brief Command Ack */ - void commandAck (UASInterface* uas, uint8_t compID, uint16_t command, uint8_t result); }; Q_DECLARE_INTERFACE(UASInterface, "org.qgroundcontrol/1.0")