Skip to content

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

New Vehicle::sendMavCommand - Queued MAV_CMD with retry

parent 7266bb3c
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 244 additions and 360 deletions
src/AutoPilotPlugins/APM/APMCompassCal.cc
View file @ d84dd7e4
......@@ -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 @ d84dd7e4
......@@ -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);
}
}
......
src/AutoPilotPlugins/Common/ESP8266ComponentController.h
View file @ d84dd7e4
......@@ -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);
......
src/AutoPilotPlugins/PX4/AirframeComponentController.cc
View file @ d84dd7e4
......@@ -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/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
View file @ d84dd7e4
......@@ -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 @ d84dd7e4
......@@ -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 @ d84dd7e4
......@@ -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, 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.";
......
src/Vehicle/MAVLinkLogManager.h
View file @ d84dd7e4
......@@ -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
src/Vehicle/Vehicle.cc
View file @ d84dd7e4
This diff is collapsed.
src/Vehicle/Vehicle.h
View file @ d84dd7e4
......@@ -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<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 @ d84dd7e4
......@@ -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 @ d84dd7e4
......@@ -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/uas/UAS.cc
View file @ d84dd7e4
......@@ -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 @ d84dd7e4
......@@ -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);
......
src/uas/UASInterface.h
View file @ d84dd7e4
......@@ -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;
......
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