Merge pull request #3194 from DonLakeFlyer/CommandResponse

Better feedback for command failures

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<UASInterface*>(_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) {

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);

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" }
     ]
 }

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;
     }
 

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;

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);

src/comm/MockLink.cc

@@ -786,16 +786,32 @@ void MockLink::_handleFTP(const mavlink_message_t& msg)
 void MockLink::_handleCommandLong(const mavlink_message_t& msg)
 {
     mavlink_command_long_t request;
+    uint8_t commandResult = MAV_RESULT_UNSUPPORTED;
 
     mavlink_msg_command_long_decode(&msg, &request);
 
-    if (request.command == MAV_CMD_COMPONENT_ARM_DISARM) {
+    switch (request.command) {
+    case MAV_CMD_COMPONENT_ARM_DISARM:
         if (request.param1 == 0.0f) {
             _mavBaseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
         } else {
             _mavBaseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
         }
+        commandResult = MAV_RESULT_ACCEPTED;
+        break;
+    case MAV_CMD_PREFLIGHT_CALIBRATION:
+    case MAV_CMD_PREFLIGHT_STORAGE:
+        commandResult = MAV_RESULT_ACCEPTED;
+        break;
     }
+
+    mavlink_message_t commandAck;
+    mavlink_msg_command_ack_pack(_vehicleSystemId,
+                                 _vehicleComponentId,
+                                 &commandAck,
+                                 request.command,
+                                 commandResult);
+    respondWithMavlinkMessage(commandAck);
 }
 
 void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode)

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;

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")