Skip to content

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

Correct usage of Vehicle::sendMavCommand

parent 064be228
Hide whitespace changes
Inline Side-by-side
Showing with 42 additions and 144 deletions
src/AutoPilotPlugins/Common/ESP8266ComponentController.cc
View file @ fa0d4719
......@@ -37,7 +37,6 @@ ESP8266ComponentController::ESP8266ComponentController()
_baudRates.append("230400");
_baudRates.append("460800");
_baudRates.append("921600");
connect(&_timer, &QTimer::timeout, this, &ESP8266ComponentController::_processTimeout);
connect(_vehicle, &Vehicle::mavCommandResult, this, &ESP8266ComponentController::_mavCommandResult);
Fact* ssid = getParameterFact(MAV_COMP_ID_UDP_BRIDGE, "WIFI_SSID4");
connect(ssid, &Fact::valueChanged, this, &ESP8266ComponentController::_ssidChanged);
......@@ -316,67 +315,16 @@ ESP8266ComponentController::restoreDefaults()
void
ESP8266ComponentController::_reboot()
{
mavlink_message_t msg;
mavlink_msg_command_long_pack_chan(
qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
_vehicle->id(),
MAV_COMP_ID_UDP_BRIDGE,
MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN,
1.0f, // Confirmation
0.0f, // Param1
1.0f, // Param2
0.0f,0.0f,0.0f,0.0f,0.0f);
_vehicle->sendMavCommand(MAV_COMP_ID_UDP_BRIDGE, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, true /* showError */, 0.0f, 1.0f);
qCDebug(ESP8266ComponentControllerLog) << "_reboot()";
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
_timer.start(1000);
}
//-----------------------------------------------------------------------------
void
ESP8266ComponentController::_restoreDefaults()
{
mavlink_message_t msg;
mavlink_msg_command_long_pack_chan(
qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
_vehicle->id(),
MAV_COMP_ID_UDP_BRIDGE,
MAV_CMD_PREFLIGHT_STORAGE,
1.0f, // Confirmation
2.0f, // Param1
0.0f,0.0f,0.0f,0.0f,0.0f,0.0f);
_vehicle->sendMavCommand(MAV_COMP_ID_UDP_BRIDGE, MAV_CMD_PREFLIGHT_STORAGE, true /* showError */, 2.0f);
qCDebug(ESP8266ComponentControllerLog) << "_restoreDefaults()";
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
_timer.start(1000);
}
//-----------------------------------------------------------------------------
void
ESP8266ComponentController::_processTimeout()
{
if(!--_retries) {
qCDebug(ESP8266ComponentControllerLog) << "_processTimeout Giving Up";
_timer.stop();
_waitType = WAIT_FOR_NOTHING;
emit busyChanged();
} else {
switch(_waitType) {
case WAIT_FOR_REBOOT:
qCDebug(ESP8266ComponentControllerLog) << "_processTimeout for Reboot";
_reboot();
break;
case WAIT_FOR_RESTORE:
qCDebug(ESP8266ComponentControllerLog) << "_processTimeout for Restore Defaults";
_restoreDefaults();
break;
}
}
}
//-----------------------------------------------------------------------------
......@@ -393,7 +341,6 @@ ESP8266ComponentController::_mavCommandResult(int vehicleId, int component, int
}
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;
......
src/AutoPilotPlugins/Common/ESP8266ComponentController.h
View file @ fa0d4719
......@@ -82,7 +82,6 @@ signals:
void busyChanged ();
private slots:
void _processTimeout ();
void _mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle);
void _ssidChanged (QVariant value);
void _passwordChanged (QVariant value);
......@@ -94,7 +93,6 @@ private:
void _restoreDefaults ();
private:
QTimer _timer;
QStringList _channels;
QStringList _baudRates;
QString _ipAddress;
......
src/FactSystem/ParameterManager.cc
View file @ fa0d4719
......@@ -816,15 +816,11 @@ void ParameterManager::_saveToEEPROM(void)
if (_saveRequired) {
_saveRequired = false;
if (_vehicle->firmwarePlugin()->isCapable(_vehicle, FirmwarePlugin::MavCmdPreflightStorageCapability)) {
mavlink_message_t msg;
mavlink_msg_command_long_pack_chan(_mavlink->getSystemId(),
_mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
_vehicle->id(),
0,
MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
_vehicle->sendMavCommand(MAV_COMP_ID_ALL,
MAV_CMD_PREFLIGHT_STORAGE,
true, // showError
1, // Write parameters to EEPROM
-1); // Don't do anything with mission storage
qCDebug(ParameterManagerLog) << _logVehiclePrefix() << "_saveToEEPROM";
} else {
qCDebug(ParameterManagerLog) << _logVehiclePrefix() << "_saveToEEPROM skipped due to FirmwarePlugin::isCapable";
......@@ -1442,22 +1438,11 @@ bool ParameterManager::loadFromJson(const QJsonObject& json, bool required, QStr
void ParameterManager::resetAllParametersToDefaults(void)
{
mavlink_message_t msg;
MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
mavlink_msg_command_long_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
_vehicle->id(), // Target systeem
_vehicle->defaultComponentId(), // Target component
MAV_CMD_PREFLIGHT_STORAGE,
0, // Confirmation
2, // 2 = Reset params to default
-1, // -1 = No change to mission storage
0, // 0 = Ignore
0, 0, 0, 0); // Unused
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
_vehicle->sendMavCommand(MAV_COMP_ID_ALL,
MAV_CMD_PREFLIGHT_STORAGE,
true, // showError
2, // Reset params to default
-1); // Don't do anything with mission storage
}
QString ParameterManager::_logVehiclePrefix(int componentId)
......
src/Vehicle/Vehicle.cc
View file @ fa0d4719
......@@ -1850,6 +1850,10 @@ void Vehicle::_sendMavCommandAgain(void)
return;
}
if (_mavCommandRetryCount > 1) {
qDebug() << "Vehicle::_sendMavCommandAgain retrying command:_mavCommandRetryCount" << queuedCommand.command << _mavCommandRetryCount;
}
_mavCommandAckTimer.start();
mavlink_message_t msg;
......
src/Vehicle/Vehicle.h
View file @ fa0d4719
......@@ -791,7 +791,7 @@ private:
QTimer _mavCommandAckTimer;
int _mavCommandRetryCount;
static const int _mavCommandMaxRetryCount = 3;
static const int _mavCommandAckTimeoutMSecs = 1000;
static const int _mavCommandAckTimeoutMSecs = 3000;
QString _prearmError;
QTimer _prearmErrorTimer;
......
src/uas/UAS.cc
View file @ fa0d4719
......@@ -797,6 +797,8 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
break;
}
// We can't use sendMavCommand here since we have no idea how long it will be before the command returns a result. This in turn
// causes the retry logic to break down.
mavlink_message_t msg;
mavlink_msg_command_long_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
......@@ -822,23 +824,16 @@ void UAS::stopCalibration(void)
return;
}
mavlink_message_t msg;
mavlink_msg_command_long_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
uasId,
_vehicle->defaultComponentId(), // target component
MAV_CMD_PREFLIGHT_CALIBRATION, // command id
0, // 0=first transmission of command
0, // gyro cal
0, // mag cal
0, // ground pressure
0, // radio cal
0, // accel cal
0, // airspeed cal
0); // unused
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
_vehicle->sendMavCommand(_vehicle->defaultComponentId(), // target component
MAV_CMD_PREFLIGHT_CALIBRATION, // command id
true, // showError
0, // gyro cal
0, // mag cal
0, // ground pressure
0, // radio cal
0, // accel cal
0, // airspeed cal
0); // unused
}
void UAS::startBusConfig(UASInterface::StartBusConfigType calType)
......@@ -858,23 +853,10 @@ void UAS::startBusConfig(UASInterface::StartBusConfigType calType)
break;
}
mavlink_message_t msg;
mavlink_msg_command_long_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
uasId,
_vehicle->defaultComponentId(), // target component
MAV_CMD_PREFLIGHT_UAVCAN, // command id
0, // 0=first transmission of command
actuatorCal, // actuators
0,
0,
0,
0,
0,
0);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
_vehicle->sendMavCommand(_vehicle->defaultComponentId(), // target component
MAV_CMD_PREFLIGHT_UAVCAN, // command id
true, // showError
actuatorCal); // actuators
}
void UAS::stopBusConfig(void)
......@@ -883,23 +865,10 @@ void UAS::stopBusConfig(void)
return;
}
mavlink_message_t msg;
mavlink_msg_command_long_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
uasId,
_vehicle->defaultComponentId(), // target component
MAV_CMD_PREFLIGHT_UAVCAN, // command id
0, // 0=first transmission of command
0,
0,
0,
0,
0,
0,
0);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
_vehicle->sendMavCommand(_vehicle->defaultComponentId(), // target component
MAV_CMD_PREFLIGHT_UAVCAN, // command id
true, // showError
0); // cancel
}
/**
......@@ -1478,16 +1447,11 @@ void UAS::pairRX(int rxType, int rxSubType)
return;
}
mavlink_message_t msg;
mavlink_msg_command_long_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
uasId,
_vehicle->defaultComponentId(),
MAV_CMD_START_RX_PAIR, 0, rxType, rxSubType, 0, 0, 0, 0, 0);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
_vehicle->sendMavCommand(_vehicle->defaultComponentId(), // target component
MAV_CMD_START_RX_PAIR, // command id
true, // showError
rxType,
rxSubType);
}
/**
......
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