Commit 0d0bc748 authored by Don Gagne's avatar Don Gagne

Merge pull request #3194 from DonLakeFlyer/CommandResponse

Better feedback for command failures
parents 10c782f9 5ddd82f5
...@@ -48,8 +48,7 @@ ESP8266ComponentController::ESP8266ComponentController() ...@@ -48,8 +48,7 @@ ESP8266ComponentController::ESP8266ComponentController()
_baudRates.append("460800"); _baudRates.append("460800");
_baudRates.append("921600"); _baudRates.append("921600");
connect(&_timer, &QTimer::timeout, this, &ESP8266ComponentController::_processTimeout); connect(&_timer, &QTimer::timeout, this, &ESP8266ComponentController::_processTimeout);
UASInterface* uas = dynamic_cast<UASInterface*>(_vehicle->uas()); connect(_vehicle, &Vehicle::commandLongAck, this, &ESP8266ComponentController::_commandAck);
connect(uas, &UASInterface::commandAck, this, &ESP8266ComponentController::_commandAck);
Fact* ssid = getParameterFact(MAV_COMP_ID_UDP_BRIDGE, "WIFI_SSID4"); Fact* ssid = getParameterFact(MAV_COMP_ID_UDP_BRIDGE, "WIFI_SSID4");
connect(ssid, &Fact::valueChanged, this, &ESP8266ComponentController::_ssidChanged); connect(ssid, &Fact::valueChanged, this, &ESP8266ComponentController::_ssidChanged);
Fact* paswd = getParameterFact(MAV_COMP_ID_UDP_BRIDGE, "WIFI_PASSWORD4"); Fact* paswd = getParameterFact(MAV_COMP_ID_UDP_BRIDGE, "WIFI_PASSWORD4");
...@@ -286,7 +285,7 @@ ESP8266ComponentController::_processTimeout() ...@@ -286,7 +285,7 @@ ESP8266ComponentController::_processTimeout()
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void 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(compID == MAV_COMP_ID_UDP_BRIDGE) {
if(result != MAV_RESULT_ACCEPTED) { if(result != MAV_RESULT_ACCEPTED) {
......
...@@ -86,7 +86,7 @@ signals: ...@@ -86,7 +86,7 @@ signals:
private slots: private slots:
void _processTimeout (); 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 _ssidChanged (QVariant value);
void _passwordChanged (QVariant value); void _passwordChanged (QVariant value);
void _baudChanged (QVariant value); void _baudChanged (QVariant value);
......
...@@ -149,9 +149,9 @@ ...@@ -149,9 +149,9 @@
"decimalPlaces": 2 "decimalPlaces": 2
} }
}, },
{ "id": 23, "rawName": "MAV_CMD_NAV_LAND_LOCAL", "friendlyName": "MAV_CMD_NAV_LAND_LOCAL" }, { "id": 23, "rawName": "MAV_CMD_NAV_LAND_LOCAL", "friendlyName": "Land local" },
{ "id": 24, "rawName": "MAV_CMD_NAV_TAKEOFF_LOCAL", "friendlyName": "MAV_CMD_NAV_TAKEOFF_LOCAL" }, { "id": 24, "rawName": "MAV_CMD_NAV_TAKEOFF_LOCAL", "friendlyName": "Takeoff local" },
{ "id": 25, "rawName": "MAV_CMD_NAV_FOLLOW", "friendlyName": "MAV_CMD_NAV_FOLLOW" }, { "id": 25, "rawName": "MAV_CMD_NAV_FOLLOW", "friendlyName": "Nav follow" },
{ {
"id": 30, "id": 30,
"rawName": "MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT", "rawName": "MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT",
...@@ -196,6 +196,8 @@ ...@@ -196,6 +196,8 @@
"decimalPlaces": 2 "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, "id": 80,
"rawName": "MAV_CMD_NAV_ROI", "rawName": "MAV_CMD_NAV_ROI",
...@@ -267,7 +269,7 @@ ...@@ -267,7 +269,7 @@
"decimalPlaces": 0 "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, "id": 84,
"rawName": "MAV_CMD_NAV_VTOL_TAKEOFF", "rawName": "MAV_CMD_NAV_VTOL_TAKEOFF",
...@@ -451,7 +453,7 @@ ...@@ -451,7 +453,7 @@
"decimalPlaces": 0 "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, "id": 181,
"rawName": "MAV_CMD_DO_SET_RELAY", "rawName": "MAV_CMD_DO_SET_RELAY",
...@@ -535,7 +537,7 @@ ...@@ -535,7 +537,7 @@
"decimalPlaces": 0 "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, "id": 189,
"rawName": "MAV_CMD_DO_LAND_START", "rawName": "MAV_CMD_DO_LAND_START",
...@@ -546,9 +548,11 @@ ...@@ -546,9 +548,11 @@
"friendlyEdit": true, "friendlyEdit": true,
"category": "Basic" "category": "Basic"
}, },
{ "id": 190, "rawName": "MAV_CMD_DO_RALLY_LAND", "friendlyName": "MAV_CMD_DO_RALLY_LAND" }, { "id": 190, "rawName": "MAV_CMD_DO_RALLY_LAND", "friendlyName": "Rally land" },
{ "id": 191, "rawName": "MAV_CMD_DO_GO_AROUND", "friendlyName": "MAV_CMD_DO_GO_AROUND" }, { "id": 191, "rawName": "MAV_CMD_DO_GO_AROUND", "friendlyName": "Go around" },
{ "id": 200, "rawName": "MAV_CMD_DO_CONTROL_VIDEO", "friendlyName": "MAV_CMD_DO_CONTROL_VIDEO" }, { "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, "id": 201,
"rawName": "MAV_CMD_DO_SET_ROI", "rawName": "MAV_CMD_DO_SET_ROI",
...@@ -657,7 +661,7 @@ ...@@ -657,7 +661,7 @@
"decimalPlaces": 0 "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, "id": 205,
"rawName": "MAV_CMD_DO_MOUNT_CONTROL", "rawName": "MAV_CMD_DO_MOUNT_CONTROL",
...@@ -732,7 +736,7 @@ ...@@ -732,7 +736,7 @@
"decimalPlaces": 0 "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, "id": 210,
"rawName": "MAV_CMD_DO_INVERTED_FLIGHT", "rawName": "MAV_CMD_DO_INVERTED_FLIGHT",
...@@ -802,15 +806,25 @@ ...@@ -802,15 +806,25 @@
"decimalPlaces": 2 "decimalPlaces": 2
} }
}, },
{ "id": 252, "rawName": "MAV_CMD_OVERRIDE_GOTO", "friendlyName": "MAV_CMD_OVERRIDE_GOTO" }, { "id": 241, "rawName": "MAV_CMD_PREFLIGHT_CALIBRATION", "friendlyName": "Calibration" },
{ "id": 300, "rawName": "MAV_CMD_MISSION_START", "friendlyName": "MAV_CMD_MISSION_START" }, { "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": 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": 410, "rawName": "MAV_CMD_GET_HOME_POSITION", "friendlyName": "Get home position" },
{ "id": 2001, "rawName": "MAV_CMD_IMAGE_STOP_CAPTURE", "friendlyName": "MAV_CMD_IMAGE_STOP_CAPTURE" }, { "id": 500, "rawName": "MAV_CMD_START_RX_PAIR", "friendlyName": "Bind Spektrum receiver" },
{ "id": 2003, "rawName": "MAV_CMD_DO_TRIGGER_CONTROL", "friendlyName": "MAV_CMD_DO_TRIGGER_CONTROL" }, { "id": 510, "rawName": "MAV_CMD_GET_MESSAGE_INTERVAL", "friendlyName": "Get message interval" },
{ "id": 2500, "rawName": "MAV_CMD_VIDEO_START_CAPTURE", "friendlyName": "MAV_CMD_VIDEO_START_CAPTURE" }, { "id": 511, "rawName": "MAV_CMD_SET_MESSAGE_INTERVAL", "friendlyName": "Set message interval" },
{ "id": 2501, "rawName": "MAV_CMD_VIDEO_STOP_CAPTURE", "friendlyName": "MAV_CMD_VIDEO_STOP_CAPTURE" }, { "id": 520, "rawName": "MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES", "friendlyName": "Get capabilities" },
{ "id": 2800, "rawName": "MAV_CMD_PANORAMA_CREATE", "friendlyName": "MAV_CMD_PANORAMA_CREATE" }, { "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, "id": 3000,
"rawName": "MAV_CMD_DO_VTOL_TRANSITION", "rawName": "MAV_CMD_DO_VTOL_TRANSITION",
...@@ -825,7 +839,7 @@ ...@@ -825,7 +839,7 @@
"enumValues": "3,4" "enumValues": "3,4"
} }
}, },
{ "id": 30001, "rawName": "MAV_CMD_PAYLOAD_PREPARE_DEPLOY", "friendlyName": "MAV_CMD_PAYLOAD_PREPARE_DEPLOY" }, { "id": 30001, "rawName": "MAV_CMD_PAYLOAD_PREPARE_DEPLOY", "friendlyName": "Payload prepare deploy" },
{ "id": 30002, "rawName": "MAV_CMD_PAYLOAD_CONTROL_DEPLOY", "friendlyName": "MAV_CMD_PAYLOAD_CONTROL_DEPLOY" } { "id": 30002, "rawName": "MAV_CMD_PAYLOAD_CONTROL_DEPLOY", "friendlyName": "Payload control deploy" }
] ]
} }
...@@ -243,7 +243,6 @@ bool MissionCommandList::contains(MAV_CMD command) const ...@@ -243,7 +243,6 @@ bool MissionCommandList::contains(MAV_CMD command) const
MavCmdInfo* MissionCommandList::getMavCmdInfo(MAV_CMD command) const MavCmdInfo* MissionCommandList::getMavCmdInfo(MAV_CMD command) const
{ {
if (!contains(command)) { if (!contains(command)) {
qWarning() << "Unknown command" << command;
return NULL; return NULL;
} }
......
...@@ -426,6 +426,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes ...@@ -426,6 +426,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
_handleExtendedSysState(message); _handleExtendedSysState(message);
break; break;
case MAVLINK_MSG_ID_COMMAND_ACK:
_handleCommandAck(message);
break;
// Following are ArduPilot dialect messages // Following are ArduPilot dialect messages
...@@ -439,6 +442,40 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes ...@@ -439,6 +442,40 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
_uas->receiveMessage(message); _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) void Vehicle::_handleExtendedSysState(mavlink_message_t& message)
{ {
mavlink_extended_sys_state_t extendedState; mavlink_extended_sys_state_t extendedState;
......
...@@ -542,6 +542,7 @@ signals: ...@@ -542,6 +542,7 @@ signals:
void flyingChanged(bool flying); void flyingChanged(bool flying);
void guidedModeChanged(bool guidedMode); void guidedModeChanged(bool guidedMode);
void prearmErrorChanged(const QString& prearmError); void prearmErrorChanged(const QString& prearmError);
void commandLongAck(uint8_t compID, uint16_t command, uint8_t result);
void messagesReceivedChanged (); void messagesReceivedChanged ();
void messagesSentChanged (); void messagesSentChanged ();
...@@ -620,6 +621,7 @@ private: ...@@ -620,6 +621,7 @@ private:
void _handleWind(mavlink_message_t& message); void _handleWind(mavlink_message_t& message);
void _handleVibration(mavlink_message_t& message); void _handleVibration(mavlink_message_t& message);
void _handleExtendedSysState(mavlink_message_t& message); void _handleExtendedSysState(mavlink_message_t& message);
void _handleCommandAck(mavlink_message_t& message);
void _missionManagerError(int errorCode, const QString& errorMsg); void _missionManagerError(int errorCode, const QString& errorMsg);
void _mapTrajectoryStart(void); void _mapTrajectoryStart(void);
void _mapTrajectoryStop(void); void _mapTrajectoryStop(void);
......
...@@ -786,16 +786,32 @@ void MockLink::_handleFTP(const mavlink_message_t& msg) ...@@ -786,16 +786,32 @@ void MockLink::_handleFTP(const mavlink_message_t& msg)
void MockLink::_handleCommandLong(const mavlink_message_t& msg) void MockLink::_handleCommandLong(const mavlink_message_t& msg)
{ {
mavlink_command_long_t request; mavlink_command_long_t request;
uint8_t commandResult = MAV_RESULT_UNSUPPORTED;
mavlink_msg_command_long_decode(&msg, &request); 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) { if (request.param1 == 0.0f) {
_mavBaseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED; _mavBaseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
} else { } else {
_mavBaseMode |= MAV_MODE_FLAG_SAFETY_ARMED; _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) void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode)
......
...@@ -607,39 +607,6 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -607,39 +607,6 @@ void UAS::receiveMessage(mavlink_message_t message)
processParamValueMsg(message, parameterName,rawValue,paramVal); processParamValueMsg(message, parameterName,rawValue,paramVal);
} }
break; 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: case MAVLINK_MSG_ID_ATTITUDE_TARGET:
{ {
mavlink_attitude_target_t out; mavlink_attitude_target_t out;
......
...@@ -322,9 +322,6 @@ signals: ...@@ -322,9 +322,6 @@ signals:
// Log Download 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 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); 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") Q_DECLARE_INTERFACE(UASInterface, "org.qgroundcontrol/1.0")
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment