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

Correct usage of Vehicle::sendMavCommand

QGC4.4
Don Gagne 8 years ago
parent
064be228a9
commit
fa0d4719ef
6 changed files with 42 additions and 144 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. 57
      src/AutoPilotPlugins/Common/ESP8266ComponentController.cc
  2. 2
      src/AutoPilotPlugins/Common/ESP8266ComponentController.h
  3. 35
      src/FactSystem/ParameterManager.cc
  4. 4
      src/Vehicle/Vehicle.cc
  5. 2
      src/Vehicle/Vehicle.h
  6. 86
      src/uas/UAS.cc

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

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

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

@ -82,7 +82,6 @@ signals: @@ -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: @@ -94,7 +93,6 @@ private:
void _restoreDefaults ();
private:
QTimer _timer;
QStringList _channels;
QStringList _baudRates;
QString _ipAddress;

35
src/FactSystem/ParameterManager.cc
Unescape View File

@ -816,15 +816,11 @@ void ParameterManager::_saveToEEPROM(void) @@ -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 @@ -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)

4
src/Vehicle/Vehicle.cc
Unescape View File

@ -1850,6 +1850,10 @@ void Vehicle::_sendMavCommandAgain(void) @@ -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;

2
src/Vehicle/Vehicle.h
Unescape View File

@ -791,7 +791,7 @@ private: @@ -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;

86
src/uas/UAS.cc
Unescape View File

@ -797,6 +797,8 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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);
}
/**

Write Preview
Loading…
Cancel
Save