From d84dd7e42da6aeb91b9b6068896e689ef921bd52 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 4 Dec 2016 16:17:14 -0800 Subject: [PATCH] New Vehicle::sendMavCommand - Queued MAV_CMD with retry --- src/AutoPilotPlugins/APM/APMCompassCal.cc | 5 +- .../Common/ESP8266ComponentController.cc | 18 +- .../Common/ESP8266ComponentController.h | 2 +- .../PX4/AirframeComponentController.cc | 2 +- .../APM/ArduCopterFirmwarePlugin.cc | 28 +-- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 177 +++++---------- src/Vehicle/MAVLinkLogManager.cc | 66 +----- src/Vehicle/MAVLinkLogManager.h | 5 +- src/Vehicle/Vehicle.cc | 214 +++++++++--------- src/Vehicle/Vehicle.h | 32 ++- .../CustomCommandWidgetController.cc | 18 +- .../CustomCommandWidgetController.h | 3 +- src/uas/UAS.cc | 27 --- src/uas/UAS.h | 3 - src/uas/UASInterface.h | 4 - 15 files changed, 244 insertions(+), 360 deletions(-) diff --git a/src/AutoPilotPlugins/APM/APMCompassCal.cc b/src/AutoPilotPlugins/APM/APMCompassCal.cc index 42939faee..88dd8a67b 100644 --- a/src/AutoPilotPlugins/APM/APMCompassCal.cc +++ b/src/AutoPilotPlugins/APM/APMCompassCal.cc @@ -145,7 +145,10 @@ CalWorkerThread::calibrate_return CalWorkerThread::calibrate(void) sensorId = 6.0f; } if (sensorId != 0.0f) { - _vehicle->doCommandLong(_vehicle->defaultComponentId(), MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS, sensorId, -sphere_x[cur_mag], -sphere_y[cur_mag], -sphere_z[cur_mag]); + _vehicle->sendMavCommand(_vehicle->defaultComponentId(), + MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS, + true, /* showErrors */ + sensorId, -sphere_x[cur_mag], -sphere_y[cur_mag], -sphere_z[cur_mag]); } } } diff --git a/src/AutoPilotPlugins/Common/ESP8266ComponentController.cc b/src/AutoPilotPlugins/Common/ESP8266ComponentController.cc index 206b54bcd..c454a6120 100644 --- a/src/AutoPilotPlugins/Common/ESP8266ComponentController.cc +++ b/src/AutoPilotPlugins/Common/ESP8266ComponentController.cc @@ -38,7 +38,7 @@ ESP8266ComponentController::ESP8266ComponentController() _baudRates.append("460800"); _baudRates.append("921600"); connect(&_timer, &QTimer::timeout, this, &ESP8266ComponentController::_processTimeout); - connect(_vehicle, &Vehicle::commandLongAck, this, &ESP8266ComponentController::_commandAck); + connect(_vehicle, &Vehicle::mavCommandResult, this, &ESP8266ComponentController::_mavCommandResult); 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"); @@ -381,21 +381,23 @@ ESP8266ComponentController::_processTimeout() //----------------------------------------------------------------------------- void -ESP8266ComponentController::_commandAck(uint8_t compID, uint16_t command, uint8_t result) +ESP8266ComponentController::_mavCommandResult(int vehicleId, int component, MAV_CMD command, MAV_RESULT result, bool noReponseFromVehicle) { - if(compID == MAV_COMP_ID_UDP_BRIDGE) { - if(result != MAV_RESULT_ACCEPTED) { + Q_UNUSED(vehicleId); + Q_UNUSED(noReponseFromVehicle); + + if (component == MAV_COMP_ID_UDP_BRIDGE) { + if (result != MAV_RESULT_ACCEPTED) { qWarning() << "ESP8266ComponentController command" << command << "rejected."; return; } - if((_waitType == WAIT_FOR_REBOOT && command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN) || - (_waitType == WAIT_FOR_RESTORE && command == MAV_CMD_PREFLIGHT_STORAGE)) - { + if ((_waitType == WAIT_FOR_REBOOT && command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN) || + (_waitType == WAIT_FOR_RESTORE && command == MAV_CMD_PREFLIGHT_STORAGE)) { _timer.stop(); _waitType = WAIT_FOR_NOTHING; emit busyChanged(); qCDebug(ESP8266ComponentControllerLog) << "_commandAck for" << command; - if(command == MAV_CMD_PREFLIGHT_STORAGE) { + if (command == MAV_CMD_PREFLIGHT_STORAGE) { _vehicle->parameterManager()->refreshAllParameters(MAV_COMP_ID_UDP_BRIDGE); } } diff --git a/src/AutoPilotPlugins/Common/ESP8266ComponentController.h b/src/AutoPilotPlugins/Common/ESP8266ComponentController.h index d0b82db40..64c831fb1 100644 --- a/src/AutoPilotPlugins/Common/ESP8266ComponentController.h +++ b/src/AutoPilotPlugins/Common/ESP8266ComponentController.h @@ -83,7 +83,7 @@ signals: private slots: void _processTimeout (); - void _commandAck (uint8_t compID, uint16_t command, uint8_t result); + void _mavCommandResult(int vehicleId, int component, MAV_CMD command, MAV_RESULT result, bool noReponseFromVehicle); void _ssidChanged (QVariant value); void _passwordChanged (QVariant value); void _baudChanged (QVariant value); diff --git a/src/AutoPilotPlugins/PX4/AirframeComponentController.cc b/src/AutoPilotPlugins/PX4/AirframeComponentController.cc index d656a998c..bbcb97b47 100644 --- a/src/AutoPilotPlugins/PX4/AirframeComponentController.cc +++ b/src/AutoPilotPlugins/PX4/AirframeComponentController.cc @@ -117,7 +117,7 @@ void AirframeComponentController::_waitParamWriteSignal(QVariant value) void AirframeComponentController::_rebootAfterStackUnwind(void) { - _uas->executeCommand(MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0); + _vehicle->sendMavCommand(_vehicle->defaultComponentId(), MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, true /* showError */, 1.0f); qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents); for (unsigned i = 0; i < 2000; i++) { QGC::SLEEP::usleep(500); diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index d9bc3b4e8..dd9350fa6 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -125,29 +125,11 @@ void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitu return; } - mavlink_message_t msg; - mavlink_command_long_t cmd; - - cmd.command = (uint16_t)MAV_CMD_NAV_TAKEOFF; - cmd.confirmation = 0; - cmd.param1 = 0.0f; - cmd.param2 = 0.0f; - cmd.param3 = 0.0f; - cmd.param4 = 0.0f; - cmd.param5 = 0.0f; - cmd.param6 = 0.0f; - cmd.param7 = vehicle->altitudeAMSL()->rawValue().toFloat() + altitudeRel; // AMSL meters - cmd.target_system = vehicle->id(); - cmd.target_component = vehicle->defaultComponentId(); - - MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - mavlink_msg_command_long_encode_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - vehicle->priorityLink()->mavlinkChannel(), - &msg, - &cmd); - - vehicle->sendMessageOnLink(vehicle->priorityLink(), msg); + vehicle->sendMavCommand(vehicle->defaultComponentId(), + MAV_CMD_NAV_TAKEOFF, + true, // show error + 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, + vehicle->altitudeAMSL()->rawValue().toFloat() + altitudeRel); // AMSL meters } void ArduCopterFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 9644e8a83..16d04d301 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -283,30 +283,16 @@ QObject* PX4FirmwarePlugin::loadParameterMetaData(const QString& metaDataFile) void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle) { - // then tell it to loiter at the current position - mavlink_message_t msg; - mavlink_command_long_t cmd; - - cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION; - cmd.confirmation = 0; - cmd.param1 = -1.0f; - cmd.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; - cmd.param3 = 0.0f; - cmd.param4 = NAN; - cmd.param5 = NAN; - cmd.param6 = NAN; - cmd.param7 = NAN; - cmd.target_system = vehicle->id(); - cmd.target_component = vehicle->defaultComponentId(); - - MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - mavlink_msg_command_long_encode_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - vehicle->priorityLink()->mavlinkChannel(), - &msg, - &cmd); - - vehicle->sendMessageOnLink(vehicle->priorityLink(), msg); + vehicle->sendMavCommand(vehicle->defaultComponentId(), + MAV_CMD_DO_REPOSITION, + true, // show error if failed + -1.0f, + MAV_DO_REPOSITION_FLAGS_CHANGE_MODE, + 0.0f, + NAN, + NAN, + NAN, + NAN); } void PX4FirmwarePlugin::guidedModeRTL(Vehicle* vehicle) @@ -321,131 +307,82 @@ void PX4FirmwarePlugin::guidedModeLand(Vehicle* vehicle) void PX4FirmwarePlugin::guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude) { - //-- If not in "guided" mode, make it so. - if(!isGuidedMode(vehicle)) + if (!isGuidedMode(vehicle)) { setGuidedMode(vehicle, true); - MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - mavlink_message_t msg; - mavlink_command_long_t cmd; - cmd.command = (uint16_t)MAV_CMD_SET_GUIDED_SUBMODE_CIRCLE; - cmd.confirmation = 0; - cmd.param1 = radius; - cmd.param2 = velocity; - cmd.param3 = altitude; - cmd.param4 = NAN; - cmd.param5 = centerCoord.isValid() ? centerCoord.latitude() : NAN; - cmd.param6 = centerCoord.isValid() ? centerCoord.longitude() : NAN; - cmd.param7 = centerCoord.isValid() ? centerCoord.altitude() : NAN; - cmd.target_system = vehicle->id(); - cmd.target_component = vehicle->defaultComponentId(); - mavlink_msg_command_long_encode_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - vehicle->priorityLink()->mavlinkChannel(), - &msg, - &cmd); - vehicle->sendMessageOnLink(vehicle->priorityLink(), msg); + } + + vehicle->sendMavCommand(vehicle->defaultComponentId(), + MAV_CMD_SET_GUIDED_SUBMODE_CIRCLE, + true, // show error if fails + radius, + velocity, + altitude, + NAN, + centerCoord.isValid() ? centerCoord.latitude() : NAN, + centerCoord.isValid() ? centerCoord.longitude() : NAN, + centerCoord.isValid() ? centerCoord.altitude() : NAN); } void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) { Q_UNUSED(altitudeRel); if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) { - qgcApp()->showMessage(QStringLiteral("Unable to takeoff, vehicle position not known.")); + qgcApp()->showMessage(tr("Unable to takeoff, vehicle position not known.")); return; } - MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - - // Set destination altitude - mavlink_message_t msg; - mavlink_command_long_t cmd; - - cmd.command = (uint16_t)MAV_CMD_NAV_TAKEOFF; - cmd.confirmation = 0; - cmd.param1 = -1.0f; - cmd.param2 = 0.0f; - cmd.param3 = 0.0f; - cmd.param4 = NAN; - cmd.param5 = NAN; - cmd.param6 = NAN; - cmd.param7 = vehicle->altitudeAMSL()->rawValue().toDouble() + altitudeRel; - cmd.target_system = vehicle->id(); - cmd.target_component = vehicle->defaultComponentId(); - - mavlink_msg_command_long_encode_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - vehicle->priorityLink()->mavlinkChannel(), - &msg, - &cmd); - vehicle->sendMessageOnLink(vehicle->priorityLink(), msg); + vehicle->sendMavCommand(vehicle->defaultComponentId(), + MAV_CMD_NAV_TAKEOFF, + true, // show error is fails + -1.0f, + 0.0f, + 0.0f, + NAN, + NAN, + NAN, + vehicle->altitudeAMSL()->rawValue().toDouble() + altitudeRel); } void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) { if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) { - qgcApp()->showMessage(QStringLiteral("Unable to go to location, vehicle position not known.")); + qgcApp()->showMessage(tr("Unable to go to location, vehicle position not known.")); return; } - mavlink_message_t msg; - mavlink_command_long_t cmd; - - cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION; - cmd.confirmation = 0; - cmd.param1 = -1.0f; - cmd.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; - cmd.param3 = 0.0f; - cmd.param4 = NAN; - cmd.param5 = gotoCoord.latitude(); - cmd.param6 = gotoCoord.longitude(); - cmd.param7 = vehicle->altitudeAMSL()->rawValue().toDouble(); - cmd.target_system = vehicle->id(); - cmd.target_component = vehicle->defaultComponentId(); - - MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - mavlink_msg_command_long_encode_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - vehicle->priorityLink()->mavlinkChannel(), - &msg, - &cmd); - - vehicle->sendMessageOnLink(vehicle->priorityLink(), msg); + vehicle->sendMavCommand(vehicle->defaultComponentId(), + MAV_CMD_DO_REPOSITION, + true, // show error is fails + -1.0f, + MAV_DO_REPOSITION_FLAGS_CHANGE_MODE, + 0.0f, + NAN, + gotoCoord.latitude(), + gotoCoord.longitude(), + vehicle->altitudeAMSL()->rawValue().toFloat()); } void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) { if (!vehicle->homePositionAvailable()) { - qgcApp()->showMessage(QStringLiteral("Unable to change altitude, home position unknown.")); + qgcApp()->showMessage(tr("Unable to change altitude, home position unknown.")); return; } if (qIsNaN(vehicle->homePosition().altitude())) { - qgcApp()->showMessage(QStringLiteral("Unable to change altitude, home position altitude unknown.")); + qgcApp()->showMessage(tr("Unable to change altitude, home position altitude unknown.")); return; } - mavlink_message_t msg; - mavlink_command_long_t cmd; - - cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION; - cmd.confirmation = 0; - cmd.param1 = -1.0f; - cmd.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; - cmd.param3 = 0.0f; - cmd.param4 = NAN; - cmd.param5 = NAN; - cmd.param6 = NAN; - cmd.param7 = vehicle->homePosition().altitude() + altitudeRel; - cmd.target_system = vehicle->id(); - cmd.target_component = vehicle->defaultComponentId(); - - MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - mavlink_msg_command_long_encode_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - vehicle->priorityLink()->mavlinkChannel(), - &msg, - &cmd); - - vehicle->sendMessageOnLink(vehicle->priorityLink(), msg); + vehicle->sendMavCommand(vehicle->defaultComponentId(), + MAV_CMD_DO_REPOSITION, + true, // show error is fails + -1.0f, + MAV_DO_REPOSITION_FLAGS_CHANGE_MODE, + 0.0f, + NAN, + NAN, + NAN, + vehicle->homePosition().altitude() + altitudeRel); } void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode) diff --git a/src/Vehicle/MAVLinkLogManager.cc b/src/Vehicle/MAVLinkLogManager.cc index 72e2d6e6e..5014150e0 100644 --- a/src/Vehicle/MAVLinkLogManager.cc +++ b/src/Vehicle/MAVLinkLogManager.cc @@ -19,8 +19,6 @@ #include #include -#define kTimeOutMilliseconds 1000 - QGC_LOGGING_CATEGORY(MAVLinkLogManagerLog, "MAVLinkLogManagerLog") static const char* kEmailAddressKey = "MAVLinkLogEmail"; @@ -301,7 +299,6 @@ MAVLinkLogManager::MAVLinkLogManager(QGCApplication* app) , _loggingDisabled(false) , _logProcessor(NULL) , _deleteAfterUpload(false) - , _loggingCmdTryCount(0) { //-- Get saved settings QSettings settings; @@ -347,7 +344,6 @@ MAVLinkLogManager::setToolbox(QGCToolbox* toolbox) qmlRegisterUncreatableType("QGroundControl.MAVLinkLogManager", 1, 0, "MAVLinkLogManager", "Reference only"); if(!_loggingDisabled) { connect(toolbox->multiVehicleManager(), &MultiVehicleManager::activeVehicleChanged, this, &MAVLinkLogManager::_activeVehicleChanged); - connect(&_ackTimer, &QTimer::timeout, this, &MAVLinkLogManager::_processCmdAck); } } @@ -540,8 +536,6 @@ MAVLinkLogManager::startLogging() if(_createNewLog()) { _vehicle->startMavlinkLog(); _logRunning = true; - _loggingCmdTryCount = 0; - _ackTimer.start(kTimeOutMilliseconds); emit logRunningChanged(); } } @@ -570,11 +564,6 @@ MAVLinkLogManager::stopLogging() delete _logProcessor; _logProcessor = NULL; _logRunning = false; - if(_vehicle) { - //-- Setup a timer to make sure vehicle received the command - _loggingCmdTryCount = 0; - _ackTimer.start(kTimeOutMilliseconds); - } emit logRunningChanged(); } } @@ -742,9 +731,9 @@ MAVLinkLogManager::_activeVehicleChanged(Vehicle* vehicle) // For now, we only handle one log download at a time. // Disconnect the previous one (if any) if(_vehicle) { - disconnect(_vehicle, &Vehicle::armedChanged, this, &MAVLinkLogManager::_armedChanged); - disconnect(_vehicle, &Vehicle::mavlinkLogData, this, &MAVLinkLogManager::_mavlinkLogData); - disconnect(_vehicle, &Vehicle::commandLongAck, this, &MAVLinkLogManager::_commandLongAck); + disconnect(_vehicle, &Vehicle::armedChanged, this, &MAVLinkLogManager::_armedChanged); + disconnect(_vehicle, &Vehicle::mavlinkLogData, this, &MAVLinkLogManager::_mavlinkLogData); + disconnect(_vehicle, &Vehicle::mavCommandResult, this, &MAVLinkLogManager::_mavCommandResult); _vehicle = NULL; //-- Stop logging (if that's the case) stopLogging(); @@ -753,51 +742,17 @@ MAVLinkLogManager::_activeVehicleChanged(Vehicle* vehicle) // Connect new system if(vehicle) { _vehicle = vehicle; - connect(_vehicle, &Vehicle::armedChanged, this, &MAVLinkLogManager::_armedChanged); - connect(_vehicle, &Vehicle::mavlinkLogData, this, &MAVLinkLogManager::_mavlinkLogData); - connect(_vehicle, &Vehicle::commandLongAck, this, &MAVLinkLogManager::_commandLongAck); + connect(_vehicle, &Vehicle::armedChanged, this, &MAVLinkLogManager::_armedChanged); + connect(_vehicle, &Vehicle::mavlinkLogData, this, &MAVLinkLogManager::_mavlinkLogData); + connect(_vehicle, &Vehicle::mavCommandResult, this, &MAVLinkLogManager::_mavCommandResult); emit canStartLogChanged(); } } -//----------------------------------------------------------------------------- -void -MAVLinkLogManager::_processCmdAck() -{ - if(_loggingCmdTryCount++ > 3) { - _ackTimer.stop(); - //-- Give up - if(_logRunning) { - qCWarning(MAVLinkLogManagerLog) << "Start MAVLink log command had no response."; - _discardLog(); - } else { - qCWarning(MAVLinkLogManagerLog) << "Stop MAVLink log command had no response."; - } - } else { - if(_vehicle) { - if(_logRunning) { - _vehicle->startMavlinkLog(); - qCWarning(MAVLinkLogManagerLog) << "Start MAVLink log command sent again."; - } else { - _vehicle->stopMavlinkLog(); - qCWarning(MAVLinkLogManagerLog) << "Stop MAVLink log command sent again."; - } - _ackTimer.start(kTimeOutMilliseconds); - } else { - //-- Vehicle went away on us - _ackTimer.stop(); - } - } -} - //----------------------------------------------------------------------------- void MAVLinkLogManager::_mavlinkLogData(Vehicle* /*vehicle*/, uint8_t /*target_system*/, uint8_t /*target_component*/, uint16_t sequence, uint8_t first_message, QByteArray data, bool /*acked*/) { - //-- Disable timer if we got a message before an ACK for the start command - if(_logRunning) { - _ackTimer.stop(); - } if(_logProcessor && _logProcessor->valid()) { if(!_logProcessor->processStreamData(sequence, first_message, data)) { qCCritical(MAVLinkLogManagerLog) << "Error writing MAVLink log file:" << _logProcessor->fileName(); @@ -814,12 +769,15 @@ MAVLinkLogManager::_mavlinkLogData(Vehicle* /*vehicle*/, uint8_t /*target_system //----------------------------------------------------------------------------- void -MAVLinkLogManager::_commandLongAck(uint8_t /*compID*/, uint16_t command, uint8_t result) +MAVLinkLogManager::_mavCommandResult(int vehicleId, int component, MAV_CMD command, MAV_RESULT result, bool noReponseFromVehicle) { + Q_UNUSED(vehicleId); + Q_UNUSED(component); + Q_UNUSED(noReponseFromVehicle) + if(command == MAV_CMD_LOGGING_START || command == MAV_CMD_LOGGING_STOP) { - _ackTimer.stop(); //-- Did it fail? - if(result) { + if(result != MAV_RESULT_ACCEPTED) { if(command == MAV_CMD_LOGGING_STOP) { //-- Not that it could happen but... qCWarning(MAVLinkLogManagerLog) << "Stop MAVLink log command failed."; diff --git a/src/Vehicle/MAVLinkLogManager.h b/src/Vehicle/MAVLinkLogManager.h index f0319502b..ae8604121 100644 --- a/src/Vehicle/MAVLinkLogManager.h +++ b/src/Vehicle/MAVLinkLogManager.h @@ -172,8 +172,7 @@ private slots: void _activeVehicleChanged (Vehicle* vehicle); void _mavlinkLogData (Vehicle* vehicle, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t first_message, QByteArray data, bool acked); void _armedChanged (bool armed); - void _commandLongAck (uint8_t compID, uint16_t command, uint8_t result); - void _processCmdAck (); + void _mavCommandResult (int vehicleId, int component, MAV_CMD command, MAV_RESULT result, bool noReponseFromVehicle); private: bool _sendLog (const QString& logFile); @@ -200,8 +199,6 @@ private: bool _loggingDisabled; MAVLinkLogProcessor* _logProcessor; bool _deleteAfterUpload; - int _loggingCmdTryCount; - QTimer _ackTimer; }; #endif diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 90ad8003d..76ec74bd4 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -174,12 +174,18 @@ Vehicle::Vehicle(LinkInterface* link, _prearmErrorTimer.setInterval(_prearmErrorTimeoutMSecs); _prearmErrorTimer.setSingleShot(true); - // Connection Lost time - _connectionLostTimer.setInterval(Vehicle::_connectionLostTimeoutMSecs); + // Connection Lost timer + _connectionLostTimer.setInterval(_connectionLostTimeoutMSecs); _connectionLostTimer.setSingleShot(false); _connectionLostTimer.start(); connect(&_connectionLostTimer, &QTimer::timeout, this, &Vehicle::_connectionLostTimeout); + // Send MAV_CMD ack timer + _mavCommandAckTimer.setSingleShot(true); + _mavCommandAckTimer.setInterval(_mavCommandAckTimeoutMSecs); + _mavCommandAckTimer.setSingleShot(false); + connect(&_mavCommandAckTimer, &QTimer::timeout, this, &Vehicle::_sendMavCommandAgain); + _mav = uas(); // Listen for system messages @@ -212,23 +218,11 @@ Vehicle::Vehicle(LinkInterface* link, _rallyPointManager = _firmwarePlugin->newRallyPointManager(this); connect(_rallyPointManager, &RallyPointManager::error, this, &Vehicle::_rallyPointManagerError); - // Ask the vehicle for firmware version info. This must be MAV_COMP_ID_ALL since we don't know default component id yet. - - mavlink_message_t versionMsg; - mavlink_command_long_t versionCmd; - - versionCmd.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES; - versionCmd.confirmation = 0; - versionCmd.param1 = 1; // Request firmware version - versionCmd.param2 = versionCmd.param3 = versionCmd.param4 = versionCmd.param5 = versionCmd.param6 = versionCmd.param7 = 0; - versionCmd.target_system = id(); - versionCmd.target_component = MAV_COMP_ID_ALL; - mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(), - _mavlink->getComponentId(), - priorityLink()->mavlinkChannel(), - &versionMsg, - &versionCmd); - sendMessageMultiple(versionMsg); + // Ask the vehicle for firmware version info. + sendMavCommand(MAV_COMP_ID_ALL, // Don't know default component id yet. + MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, + false, // No error shown if fails + 1); // Request firmware version _firmwarePlugin->initializeVehicle(this); @@ -564,40 +558,41 @@ void Vehicle::_handleHilActuatorControls(mavlink_message_t &message) void Vehicle::_handleCommandAck(mavlink_message_t& message) { + bool showError = true; + mavlink_command_ack_t ack; mavlink_msg_command_ack_decode(&message, &ack); - emit commandLongAck(message.compid, ack.command, ack.result); + if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) { + showError = _mavCommandQueue[0].showError; + _mavCommandQueue.removeFirst(); + } + + emit mavCommandResult(_id, message.compid, (MAV_CMD)ack.command, (MAV_RESULT)ack.result, false /* noResponsefromVehicle */); + + if (showError) { + QString commandName = qgcApp()->toolbox()->missionCommandTree()->friendlyName((MAV_CMD)ack.command); - // Disregard failures for these (handled above) - switch (ack.command) { - case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: - case MAV_CMD_LOGGING_START: - case MAV_CMD_LOGGING_STOP: - return; + 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; + } } - QString commandName = qgcApp()->toolbox()->missionCommandTree()->friendlyName((MAV_CMD)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; - } + _sendNextQueuedMavCommand(); } void Vehicle::_handleExtendedSysState(mavlink_message_t& message) @@ -1303,29 +1298,10 @@ QGeoCoordinate Vehicle::homePosition(void) void Vehicle::setArmed(bool armed) { // We specifically use COMMAND_LONG:MAV_CMD_COMPONENT_ARM_DISARM since it is supported by more flight stacks. - - mavlink_message_t msg; - mavlink_command_long_t cmd; - - cmd.command = (uint16_t)MAV_CMD_COMPONENT_ARM_DISARM; - cmd.confirmation = 0; - cmd.param1 = armed ? 1.0f : 0.0f; - cmd.param2 = 0.0f; - cmd.param3 = 0.0f; - cmd.param4 = 0.0f; - cmd.param5 = 0.0f; - cmd.param6 = 0.0f; - cmd.param7 = 0.0f; - cmd.target_system = id(); - cmd.target_component = defaultComponentId(); - - mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(), - _mavlink->getComponentId(), - priorityLink()->mavlinkChannel(), - &msg, - &cmd); - - sendMessageOnLink(priorityLink(), msg); + sendMavCommand(defaultComponentId(), + MAV_CMD_COMPONENT_ARM_DISARM, + true, // show error if fails + armed ? 1.0f : 0.0f); } bool Vehicle::flightModeSetAvailable(void) @@ -1812,27 +1788,11 @@ void Vehicle::setGuidedMode(bool guidedMode) void Vehicle::emergencyStop(void) { - mavlink_message_t msg; - mavlink_command_long_t cmd; - - cmd.command = (uint16_t)MAV_CMD_COMPONENT_ARM_DISARM; - cmd.confirmation = 0; - cmd.param1 = 0.0f; - cmd.param2 = 21196.0f; // Magic number for emergency stop - cmd.param3 = 0.0f; - cmd.param4 = 0.0f; - cmd.param5 = 0.0f; - cmd.param6 = 0.0f; - cmd.param7 = 0.0f; - cmd.target_system = id(); - cmd.target_component = defaultComponentId(); - mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(), - _mavlink->getComponentId(), - priorityLink()->mavlinkChannel(), - &msg, - &cmd); - - sendMessageOnLink(priorityLink(), msg); + sendMavCommand(defaultComponentId(), + MAV_CMD_COMPONENT_ARM_DISARM, + true, // show error if fails + 0.0f, + 21196.0f); // Magic number for emergency stop } void Vehicle::setCurrentMissionSequence(int seq) @@ -1851,22 +1811,59 @@ void Vehicle::setCurrentMissionSequence(int seq) sendMessageOnLink(priorityLink(), msg); } -void Vehicle::doCommandLong(int component, MAV_CMD command, float param1, float param2, float param3, float param4, float param5, float param6, float param7) +void Vehicle::sendMavCommand(int component, MAV_CMD command, bool showError, float param1, float param2, float param3, float param4, float param5, float param6, float param7) { + MavCommandQueueEntry_t entry; + + entry.component = component; + entry.command = command; + entry.showError = showError; + entry.rgParam[0] = param1; + entry.rgParam[1] = param2; + entry.rgParam[2] = param3; + entry.rgParam[3] = param4; + entry.rgParam[4] = param5; + entry.rgParam[5] = param6; + entry.rgParam[6] = param7; + + _mavCommandQueue.append(entry); + + if (_mavCommandQueue.count() == 1) { + _mavCommandRetryCount = 0; + _sendMavCommandAgain(); + } +} + +void Vehicle::_sendMavCommandAgain(void) +{ + MavCommandQueueEntry_t& queuedCommand = _mavCommandQueue[0]; + + if (_mavCommandRetryCount++ > _mavCommandMaxRetryCount) { + emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */); + if (queuedCommand.showError) { + qgcApp()->showMessage(tr("Vehicle did not respond to command: %1").arg(qgcApp()->toolbox()->missionCommandTree()->friendlyName(queuedCommand.command))); + } + _mavCommandQueue.removeFirst(); + _sendNextQueuedMavCommand(); + return; + } + + _mavCommandAckTimer.start(); + mavlink_message_t msg; mavlink_command_long_t cmd; - cmd.command = command; + cmd.command = queuedCommand.command; cmd.confirmation = 0; - cmd.param1 = param1; - cmd.param2 = param2; - cmd.param3 = param3; - cmd.param4 = param4; - cmd.param5 = param5; - cmd.param6 = param6; - cmd.param7 = param7; - cmd.target_system = id(); - cmd.target_component = component; + cmd.param1 = queuedCommand.rgParam[0]; + cmd.param2 = queuedCommand.rgParam[1]; + cmd.param3 = queuedCommand.rgParam[2]; + cmd.param4 = queuedCommand.rgParam[3]; + cmd.param5 = queuedCommand.rgParam[4]; + cmd.param6 = queuedCommand.rgParam[5]; + cmd.param7 = queuedCommand.rgParam[6]; + cmd.target_system = _id; + cmd.target_component = queuedCommand.component; mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(), _mavlink->getComponentId(), priorityLink()->mavlinkChannel(), @@ -1876,6 +1873,15 @@ void Vehicle::doCommandLong(int component, MAV_CMD command, float param1, float sendMessageOnLink(priorityLink(), msg); } +void Vehicle::_sendNextQueuedMavCommand(void) +{ + if (_mavCommandQueue.count()) { + _mavCommandRetryCount = 0; + _sendMavCommandAgain(); + } +} + + void Vehicle::setPrearmError(const QString& prearmError) { _prearmError = prearmError; @@ -1921,7 +1927,7 @@ QString Vehicle::firmwareVersionTypeString(void) const void Vehicle::rebootVehicle() { - doCommandLong(defaultComponentId(), MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); + sendMavCommand(defaultComponentId(), MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, true, 1.0f); } int Vehicle::defaultComponentId(void) @@ -1941,7 +1947,7 @@ void Vehicle::setSoloFirmware(bool soloFirmware) // Temporarily removed, waiting for new command implementation void Vehicle::motorTest(int motor, int percent, int timeoutSecs) { - doCommandLong(defaultComponentId(), MAV_CMD_DO_MOTOR_TEST, motor, MOTOR_TEST_THROTTLE_PERCENT, percent, timeoutSecs); + doCommandLongUnverified(defaultComponentId(), MAV_CMD_DO_MOTOR_TEST, motor, MOTOR_TEST_THROTTLE_PERCENT, percent, timeoutSecs); } #endif @@ -2044,12 +2050,12 @@ VehicleGPSFactGroup::VehicleGPSFactGroup(QObject* parent) void Vehicle::startMavlinkLog() { - doCommandLong(defaultComponentId(), MAV_CMD_LOGGING_START); + sendMavCommand(defaultComponentId(), MAV_CMD_LOGGING_START, false /* showError */); } void Vehicle::stopMavlinkLog() { - doCommandLong(defaultComponentId(), MAV_CMD_LOGGING_STOP); + sendMavCommand(defaultComponentId(), MAV_CMD_LOGGING_STOP, false /* showError */); } void Vehicle::_ackMavlinkLogData(uint16_t sequence) diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 1ee3d62f6..2501a7acd 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -553,7 +553,13 @@ public: static const int cMaxRcChannels = 18; bool containsLink(LinkInterface* link) { return _links.contains(link); } - void doCommandLong(int component, MAV_CMD command, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f); + + /// Sends the specified MAV_CMD to the vehicle. If no Ack is received command will be retried. If a sendMavCommand is already in progress + /// the command will be queued and sent when the previous command completes. + /// @param component Component to send to + /// @param command MAV_CMD to send + /// @param showError true: Display error to user if command failed, false: no error shown + void sendMavCommand(int component, MAV_CMD command, bool showError, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f); int firmwareMajorVersion(void) const { return _firmwareMajorVersion; } int firmwareMinorVersion(void) const { return _firmwareMinorVersion; } @@ -609,7 +615,6 @@ 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 soloFirmwareChanged(bool soloFirmware); void unhealthySensorsChanged(void); @@ -653,6 +658,14 @@ signals: // Mavlink Log Download void mavlinkLogData (Vehicle* vehicle, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t first_message, QByteArray data, bool acked); + /// Signalled in response to usage of sendMavCommand + /// @param vehicleId Vehicle which command was sent to + /// @param component Component which command was sent to + /// @param command MAV_CMD Command which was sent + /// @param result MAV_RESULT returned in ack + /// @param noResponseFromVehicle true: vehicle did not respond to command, false: vehicle responsed, MAV_RESULT in result + void mavCommandResult(int vehicleId, int component, MAV_CMD command, MAV_RESULT result, bool noReponseFromVehicle); + private slots: void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message); void _linkInactiveOrDeleted(LinkInterface* link); @@ -682,6 +695,7 @@ private slots: void _prearmErrorTimeout(void); void _newMissionItemsAvailable(void); void _newGeoFenceAvailable(void); + void _sendMavCommandAgain(void); private: bool _containsLink(LinkInterface* link); @@ -713,6 +727,7 @@ private: void _handleMavlinkLoggingData(mavlink_message_t& message); void _handleMavlinkLoggingDataAcked(mavlink_message_t& message); void _ackMavlinkLogData(uint16_t sequence); + void _sendNextQueuedMavCommand(void); private: int _id; ///< Mavlink system id @@ -765,6 +780,19 @@ private: uint32_t _onboardControlSensorsHealth; uint32_t _onboardControlSensorsUnhealthy; + typedef struct { + int component; + MAV_CMD command; + float rgParam[7]; + bool showError; + } MavCommandQueueEntry_t; + + QList _mavCommandQueue; + QTimer _mavCommandAckTimer; + int _mavCommandRetryCount; + static const int _mavCommandMaxRetryCount = 3; + static const int _mavCommandAckTimeoutMSecs = 1000; + QString _prearmError; QTimer _prearmErrorTimer; static const int _prearmErrorTimeoutMSecs = 35 * 1000; ///< Take away prearm error after 35 seconds diff --git a/src/ViewWidgets/CustomCommandWidgetController.cc b/src/ViewWidgets/CustomCommandWidgetController.cc index cfefb1ae9..b8791a5dc 100644 --- a/src/ViewWidgets/CustomCommandWidgetController.cc +++ b/src/ViewWidgets/CustomCommandWidgetController.cc @@ -21,10 +21,10 @@ const char* CustomCommandWidgetController::_settingsKey = "CustomCommand.QmlFile"; CustomCommandWidgetController::CustomCommandWidgetController(void) : - _uas(NULL) + _vehicle(NULL) { if(qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()) { - _uas = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->uas(); + _vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle(); } QSettings settings; _customQmlFile = settings.value(_settingsKey).toString(); @@ -33,15 +33,21 @@ CustomCommandWidgetController::CustomCommandWidgetController(void) : void CustomCommandWidgetController::sendCommand(int commandId, QVariant componentId, QVariant confirm, QVariant param1, QVariant param2, QVariant param3, QVariant param4, QVariant param5, QVariant param6, QVariant param7) { - if(_uas) { - _uas->executeCommand((MAV_CMD)commandId, confirm.toInt(), param1.toFloat(), param2.toFloat(), param3.toFloat(), param4.toFloat(), param5.toFloat(), param6.toFloat(), param7.toFloat(), componentId.toInt()); + Q_UNUSED(confirm); + + if(_vehicle) { + _vehicle->sendMavCommand(componentId.toInt(), + (MAV_CMD)commandId, + true, // show error if fails + param1.toFloat(), param2.toFloat(), param3.toFloat(), param4.toFloat(), param5.toFloat(), param6.toFloat(), param7.toFloat()); } } void CustomCommandWidgetController::_activeVehicleChanged(Vehicle* activeVehicle) { - if(activeVehicle) - _uas = activeVehicle->uas(); + if (activeVehicle) { + _vehicle = activeVehicle; + } } void CustomCommandWidgetController::selectQmlFile(void) diff --git a/src/ViewWidgets/CustomCommandWidgetController.h b/src/ViewWidgets/CustomCommandWidgetController.h index 81f018e80..514e7a692 100644 --- a/src/ViewWidgets/CustomCommandWidgetController.h +++ b/src/ViewWidgets/CustomCommandWidgetController.h @@ -13,7 +13,6 @@ #include -#include "UASInterface.h" #include "AutoPilotPlugin.h" #include "FactPanelController.h" @@ -37,7 +36,7 @@ private slots: void _activeVehicleChanged (Vehicle* activeVehicle); private: - UASInterface* _uas; + Vehicle* _vehicle; QString _customQmlFile; static const char* _settingsKey; }; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index e313e87aa..6d1a40f1a 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1233,33 +1233,6 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, emit parameterUpdate(uasId, compId, paramName, rawValue.param_count, rawValue.param_index, rawValue.param_type, paramValue); } -void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component) -{ - if (!_vehicle) { - return; - } - - mavlink_message_t msg; - mavlink_command_long_t cmd; - cmd.command = (uint16_t)command; - cmd.confirmation = confirmation; - cmd.param1 = param1; - cmd.param2 = param2; - cmd.param3 = param3; - cmd.param4 = param4; - cmd.param5 = param5; - cmd.param6 = param6; - cmd.param7 = param7; - cmd.target_system = uasId; - cmd.target_component = component; - mavlink_msg_command_long_encode_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - _vehicle->priorityLink()->mavlinkChannel(), - &msg, - &cmd); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); -} - /** * Set the manual control commands. * This can only be done if the system has manual inputs enabled and is armed. diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 1e93a0670..ad9b94646 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -472,9 +472,6 @@ public: void requestImage(); public slots: - /** @brief Executes a command with 7 params */ - void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component); - /** @brief Order the robot to pair its receiver **/ void pairRX(int rxType, int rxSubType); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 10ad42fe8..8ae866f9b 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -141,10 +141,6 @@ public: virtual void stopBusConfig(void) = 0; public slots: - - /** @brief Executes a command **/ - virtual void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component) = 0; - /** @brief Order the robot to pair its receiver **/ virtual void pairRX(int rxType, int rxSubType) = 0; -- 2.22.0