Skip to content

Q
qgroundcontrol
Commit 0d0bc748 authored by Don Gagne's avatar Don Gagne
Browse files
Options

Merge pull request #3194 from DonLakeFlyer/CommandResponse 

Better feedback for command failures
parents 10c782f9 5ddd82f5
Hide whitespace changes
Inline Side-by-side
Showing with 94 additions and 63 deletions
src/AutoPilotPlugins/Common/ESP8266ComponentController.cc
View file @ 0d0bc748
......@@ -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
View file @ 0d0bc748
......@@ -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
View file @ 0d0bc748
......@@ -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
View file @ 0d0bc748
......@@ -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
View file @ 0d0bc748
......@@ -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
View file @ 0d0bc748
......@@ -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
View file @ 0d0bc748
......@@ -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
View file @ 0d0bc748
......@@ -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
View file @ 0d0bc748
......@@ -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")
......
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