zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

New Vehicle::sendMavCommand - Queued MAV_CMD with retry

QGC4.4
Don Gagne 8 years ago
parent
7266bb3c54
commit
d84dd7e42d
15 changed files with 244 additions and 360 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      src/AutoPilotPlugins/APM/APMCompassCal.cc
  2. 18
      src/AutoPilotPlugins/Common/ESP8266ComponentController.cc
  3. 2
      src/AutoPilotPlugins/Common/ESP8266ComponentController.h
  4. 2
      src/AutoPilotPlugins/PX4/AirframeComponentController.cc
  5. 28
      src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
  6. 177
      src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
  7. 66
      src/Vehicle/MAVLinkLogManager.cc
  8. 5
      src/Vehicle/MAVLinkLogManager.h
  9. 214
      src/Vehicle/Vehicle.cc
  10. 32
      src/Vehicle/Vehicle.h
  11. 18
      src/ViewWidgets/CustomCommandWidgetController.cc
  12. 3
      src/ViewWidgets/CustomCommandWidgetController.h
  13. 27
      src/uas/UAS.cc
  14. 3
      src/uas/UAS.h
  15. 4
      src/uas/UASInterface.h

5
src/AutoPilotPlugins/APM/APMCompassCal.cc
Unescape View File

@ -145,7 +145,10 @@ CalWorkerThread::calibrate_return CalWorkerThread::calibrate(void) @@ -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]);
}
}
}

18
src/AutoPilotPlugins/Common/ESP8266ComponentController.cc
Unescape View File

@ -38,7 +38,7 @@ ESP8266ComponentController::ESP8266ComponentController() @@ -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() @@ -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);
}
}

2
src/AutoPilotPlugins/Common/ESP8266ComponentController.h
Unescape View File

@ -83,7 +83,7 @@ signals: @@ -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);

2
src/AutoPilotPlugins/PX4/AirframeComponentController.cc
Unescape View File

@ -117,7 +117,7 @@ void AirframeComponentController::_waitParamWriteSignal(QVariant value) @@ -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);

28
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
Unescape View File

@ -125,29 +125,11 @@ void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitu @@ -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)

177
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
Unescape View File

@ -283,30 +283,16 @@ QObject* PX4FirmwarePlugin::loadParameterMetaData(const QString& metaDataFile) @@ -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) @@ -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)

66
src/Vehicle/MAVLinkLogManager.cc
Unescape View File

@ -19,8 +19,6 @@ @@ -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) @@ -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) @@ -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() @@ -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() @@ -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) @@ -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) @@ -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 @@ -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.";

5
src/Vehicle/MAVLinkLogManager.h
Unescape View File

@ -172,8 +172,7 @@ private slots: @@ -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: @@ -200,8 +199,6 @@ private:
bool _loggingDisabled;
MAVLinkLogProcessor* _logProcessor;
bool _deleteAfterUpload;
int _loggingCmdTryCount;
QTimer _ackTimer;
};
#endif

214
src/Vehicle/Vehicle.cc
Unescape View File

@ -174,12 +174,18 @@ Vehicle::Vehicle(LinkInterface* link, @@ -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, @@ -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) @@ -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) @@ -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) @@ -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) @@ -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 @@ -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 @@ -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) @@ -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) @@ -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)

32
src/Vehicle/Vehicle.h
Unescape View File

@ -553,7 +553,13 @@ public: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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

18
src/ViewWidgets/CustomCommandWidgetController.cc
Unescape View File

@ -21,10 +21,10 @@ @@ -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) : @@ -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)

3
src/ViewWidgets/CustomCommandWidgetController.h
Unescape View File

@ -13,7 +13,6 @@ @@ -13,7 +13,6 @@
#include <QObject>
#include "UASInterface.h"
#include "AutoPilotPlugin.h"
#include "FactPanelController.h"
@ -37,7 +36,7 @@ private slots: @@ -37,7 +36,7 @@ private slots:
void _activeVehicleChanged (Vehicle* activeVehicle);
private:
UASInterface* _uas;
Vehicle* _vehicle;
QString _customQmlFile;
static const char* _settingsKey;
};

27
src/uas/UAS.cc
Unescape View File

@ -1233,33 +1233,6 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, @@ -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.

3
src/uas/UAS.h
Unescape View File

@ -472,9 +472,6 @@ public: @@ -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);

4
src/uas/UASInterface.h
Unescape View File

@ -141,10 +141,6 @@ public: @@ -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;

Write Preview
Loading…
Cancel
Save