Skip to content
GitLab
Commit a0f3e658 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files

Merge pull request #4295 from DonLakeFlyer/MavCommandRetryWork 

Mav command retry work
parents 2732e9a5 eebbd613
Hide whitespace changes
Inline Side-by-side
qgroundcontrol.pro
View file @ a0f3e658
......@@ -381,6 +381,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
src/qgcunittest/TCPLinkTest.h \
src/qgcunittest/TCPLoopBackServer.h \
src/qgcunittest/UnitTest.h \
src/Vehicle/SendMavCommandTest.h \
SOURCES += \
src/AnalyzeView/LogDownloadTest.cc \
......@@ -409,6 +410,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
src/qgcunittest/TCPLoopBackServer.cc \
src/qgcunittest/UnitTest.cc \
src/qgcunittest/UnitTestList.cc \
src/Vehicle/SendMavCommandTest.cc \
} } } } } }
# Main QGC Headers and Source files
......
src/AutoPilotPlugins/APM/APMCompassCal.cc
View file @ a0f3e658
......@@ -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]);
}
}
}
......
src/AutoPilotPlugins/Common/ESP8266ComponentController.cc
View file @ a0f3e658
......@@ -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, int command, int 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);
}
}
......
src/AutoPilotPlugins/Common/ESP8266ComponentController.h
View file @ a0f3e658
......@@ -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, int command, int result, bool noReponseFromVehicle);
void _ssidChanged (QVariant value);
void _passwordChanged (QVariant value);
void _baudChanged (QVariant value);
......
src/AutoPilotPlugins/PX4/AirframeComponentController.cc
View file @ a0f3e658
......@@ -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);
......
src/FactSystem/ParameterManager.cc
View file @ a0f3e658
......@@ -1066,7 +1066,7 @@ void ParameterManager::_initialRequestTimeout(void)
} else {
if (!_vehicle->genericFirmware()) {
// Generic vehicles (like BeBop) may not have any parameters, so don't annoy the user
QString errorMsg = tr("Vehicle %1 did not respond to request for parameters"
QString errorMsg = tr("Vehicle %1 did not respond to request for parameters. "
"This will cause QGroundControl to be unable to display its full user interface.").arg(_vehicle->id());
qCDebug(ParameterManagerLog) << errorMsg;
qgcApp()->showMessage(errorMsg);
......
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
View file @ a0f3e658
......@@ -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)
......
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
View file @ a0f3e658
......@@ -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)
......
src/Vehicle/MAVLinkLogManager.cc
View file @ a0f3e658
......@@ -19,8 +19,6 @@
#include <QFile>
#include <QFileInfo>
#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<MAVLinkLogManager>("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, int command, int 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.";
......
src/Vehicle/MAVLinkLogManager.h
View file @ a0f3e658
......@@ -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, int command, int 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
src/Vehicle/SendMavCommandTest.cc 0 → 100644
View file @ a0f3e658
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "SendMavCommandTest.h"
#include "MultiVehicleManager.h"
#include "QGCApplication.h"
void SendMavCommandTest::_noFailure(void)
{
_connectMockLink();
MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
Vehicle* vehicle = vehicleMgr->activeVehicle();
QVERIFY(vehicle);
vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_1, true /* showError */);
QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool)));
QCOMPARE(spyResult.wait(10000), true);
QList<QVariant> arguments = spyResult.takeFirst();
QCOMPARE(arguments.count(), 5);
QCOMPARE(arguments.at(0).toInt(), vehicle->id());
QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_1);
QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_ACCEPTED);
QCOMPARE(arguments.at(4).toBool(), false);
}
void SendMavCommandTest::_failureShowError(void)
{
// Will pop error about request failure
setExpectedMessageBox(QMessageBox::Ok);
_connectMockLink();
MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
Vehicle* vehicle = vehicleMgr->activeVehicle();
QVERIFY(vehicle);
vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_2, true /* showError */);
QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool)));
QCOMPARE(spyResult.wait(10000), true);
QList<QVariant> arguments = spyResult.takeFirst();
QCOMPARE(arguments.count(), 5);
QCOMPARE(arguments.at(0).toInt(), vehicle->id());
QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_2);
QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED);
QCOMPARE(arguments.at(4).toBool(), false);
// User should have been notified
checkExpectedMessageBox();
}
void SendMavCommandTest::_failureNoShowError(void)
{
_connectMockLink();
MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
Vehicle* vehicle = vehicleMgr->activeVehicle();
QVERIFY(vehicle);
vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_2, false /* showError */);
QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool)));
QCOMPARE(spyResult.wait(10000), true);
QList<QVariant> arguments = spyResult.takeFirst();
QCOMPARE(arguments.count(), 5);
QCOMPARE(arguments.at(0).toInt(), vehicle->id());
QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_2);
QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED);
QCOMPARE(arguments.at(4).toBool(), false);
}
void SendMavCommandTest::_noFailureAfterRetry(void)
{
_connectMockLink();
MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
Vehicle* vehicle = vehicleMgr->activeVehicle();
QVERIFY(vehicle);
vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_3, true /* showError */);
QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool)));
QCOMPARE(spyResult.wait(10000), true);
QList<QVariant> arguments = spyResult.takeFirst();
QCOMPARE(arguments.count(), 5);
QCOMPARE(arguments.at(0).toInt(), vehicle->id());
QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_3);
QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_ACCEPTED);
QCOMPARE(arguments.at(4).toBool(), false);
}
void SendMavCommandTest::_failureAfterRetry(void)
{
// Will pop error about request failure
setExpectedMessageBox(QMessageBox::Ok);
_connectMockLink();
MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
Vehicle* vehicle = vehicleMgr->activeVehicle();
QVERIFY(vehicle);
vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_4, true /* showError */);
QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool)));
QCOMPARE(spyResult.wait(10000), true);
QList<QVariant> arguments = spyResult.takeFirst();
QCOMPARE(arguments.count(), 5);
QCOMPARE(arguments.at(0).toInt(), vehicle->id());
QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_4);
QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED);
QCOMPARE(arguments.at(4).toBool(), false);
// User should have been notified
checkExpectedMessageBox();
}
void SendMavCommandTest::_failureAfterNoReponse(void)
{
// Will pop error about request failure
setExpectedMessageBox(QMessageBox::Ok);
_connectMockLink();
MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
Vehicle* vehicle = vehicleMgr->activeVehicle();
QVERIFY(vehicle);
vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_5, true /* showError */);
QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool)));
QCOMPARE(spyResult.wait(10000), true);
QList<QVariant> arguments = spyResult.takeFirst();
QCOMPARE(arguments.count(), 5);
QCOMPARE(arguments.at(0).toInt(), vehicle->id());
QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_5);
QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED);
QCOMPARE(arguments.at(4).toBool(), true);
// User should have been notified
checkExpectedMessageBox();
}
src/Vehicle/SendMavCommandTest.h 0 → 100644
View file @ a0f3e658
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#ifndef SendMavCommandTest_H
#define SendMavCommandTest_H
#include "UnitTest.h"
class SendMavCommandTest : public UnitTest
{
Q_OBJECT
private slots:
void _noFailure(void);
void _failureShowError(void);
void _failureNoShowError(void);
void _noFailureAfterRetry(void);
void _failureAfterRetry(void);
void _failureAfterNoReponse(void);
private:
};
#endif
src/Vehicle/Vehicle.cc
View file @ a0f3e658
......@@ -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,42 @@ 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) {
_mavCommandAckTimer.stop();
showError = _mavCommandQueue[0].showError;
_mavCommandQueue.removeFirst();
}
emit mavCommandResult(_id, message.compid, ack.command, 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 +1299,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 +1789,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 +1812,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 +1874,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 +1928,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 +1948,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 +2051,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)
......
src/Vehicle/Vehicle.h
View file @ a0f3e658
......@@ -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, int command, int 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<MavCommandQueueEntry_t> _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
......
src/ViewWidgets/CustomCommandWidgetController.cc
View file @ a0f3e658
......@@ -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)
......
src/ViewWidgets/CustomCommandWidgetController.h
View file @ a0f3e658
......@@ -13,7 +13,6 @@
#include <QObject>
#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;
};
......
src/comm/MockLink.cc
View file @ a0f3e658
......@@ -807,6 +807,40 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
commandResult = MAV_RESULT_ACCEPTED;
_respondWithAutopilotVersion();
break;
case MAV_CMD_USER_1:
// Test command which always returns MAV_RESULT_ACCEPTED
commandResult = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_USER_2:
// Test command which always returns MAV_RESULT_FAILED
commandResult = MAV_RESULT_FAILED;
break;
case MAV_CMD_USER_3:
// Test command which returns MAV_RESULT_ACCEPTED on second attempt
static bool firstCmdUser3 = true;
if (firstCmdUser3) {
firstCmdUser3 = false;
return;
} else {
firstCmdUser3 = true;
commandResult = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_USER_4:
// Test command which returns MAV_RESULT_FAILED on second attempt
static bool firstCmdUser4 = true;
if (firstCmdUser4) {
firstCmdUser4 = false;
return;
} else {
firstCmdUser4 = true;
commandResult = MAV_RESULT_FAILED;
}
break;
case MAV_CMD_USER_5:
// No response
return;
break;
}
mavlink_message_t commandAck;
......
src/qgcunittest/UnitTestList.cc
View file @ a0f3e658
......@@ -31,6 +31,7 @@
#include "ParameterManagerTest.h"
#include "MissionCommandTreeTest.h"
#include "LogDownloadTest.h"
#include "SendMavCommandTest.h"
UT_REGISTER_TEST(FactSystemTestGeneric)
UT_REGISTER_TEST(FactSystemTestPX4)
......@@ -50,6 +51,7 @@ UT_REGISTER_TEST(TCPLinkTest)
UT_REGISTER_TEST(ParameterManagerTest)
UT_REGISTER_TEST(MissionCommandTreeTest)
UT_REGISTER_TEST(LogDownloadTest)
UT_REGISTER_TEST(SendMavCommandTest)
// List of unit test which are currently disabled.
// If disabling a new test, include reason in comment.
......
src/uas/UAS.cc
View file @ a0f3e658
......@@ -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.
......
src/uas/UAS.h
View file @ a0f3e658
......@@ -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);
......
Supports Markdown
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