diff --git a/src/AutoPilotPlugins/Common/ESP8266ComponentController.cc b/src/AutoPilotPlugins/Common/ESP8266ComponentController.cc index 4ba9430c75b96a0d11360922896531c3b7e71b5b..1e7c937d148372ccd8b2a8b1d0b7ab15c338d8b9 100644 --- a/src/AutoPilotPlugins/Common/ESP8266ComponentController.cc +++ b/src/AutoPilotPlugins/Common/ESP8266ComponentController.cc @@ -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; diff --git a/src/AutoPilotPlugins/Common/ESP8266ComponentController.h b/src/AutoPilotPlugins/Common/ESP8266ComponentController.h index afba9939c4d1c387049dab219fb4dd05e8746704..1093eb2676f4600163b0a7237376e0c6666e2517 100644 --- a/src/AutoPilotPlugins/Common/ESP8266ComponentController.h +++ b/src/AutoPilotPlugins/Common/ESP8266ComponentController.h @@ -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; diff --git a/src/FactSystem/ParameterManager.cc b/src/FactSystem/ParameterManager.cc index 2cec90baa00b0139cf22822934c075960c0b793c..925621b65597e5686fa3deb71af0bd10a9bc8da1 100644 --- a/src/FactSystem/ParameterManager.cc +++ b/src/FactSystem/ParameterManager.cc @@ -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) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 82afb8489983492d4154a4e21b45d94b08961ca0..2c7cb24d6008524bee8776d0647435368d43d62b 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -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; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index fd19666a2cdd893d736c73ef38e4a7daf896f43e..1f8eb3cb39fc25e690526bffe07d6d8c55e98779 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -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; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 6d1a40f1a0ec8706eb5c45dc638aff7e07964d46..b7ada3e587ca496a245bb1ae46d7a74dce634c67 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -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); } /**