Commit 35534c33 authored by Don Gagne's avatar Don Gagne

Keep ParameterLoader on first seen dedicated link

parent 68908488
...@@ -50,6 +50,7 @@ ParameterLoader::ParameterLoader(AutoPilotPlugin* autopilot, Vehicle* vehicle, Q ...@@ -50,6 +50,7 @@ ParameterLoader::ParameterLoader(AutoPilotPlugin* autopilot, Vehicle* vehicle, Q
, _autopilot(autopilot) , _autopilot(autopilot)
, _vehicle(vehicle) , _vehicle(vehicle)
, _mavlink(qgcApp()->toolbox()->mavlinkProtocol()) , _mavlink(qgcApp()->toolbox()->mavlinkProtocol())
, _dedicatedLink(_vehicle->priorityLink())
, _parametersReady(false) , _parametersReady(false)
, _initialLoadComplete(false) , _initialLoadComplete(false)
, _defaultComponentId(FactSystem::defaultComponentId) , _defaultComponentId(FactSystem::defaultComponentId)
...@@ -327,7 +328,7 @@ void ParameterLoader::refreshAllParameters(uint8_t componentID) ...@@ -327,7 +328,7 @@ void ParameterLoader::refreshAllParameters(uint8_t componentID)
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, _vehicle->id(), componentID); mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, _vehicle->id(), componentID);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); _vehicle->sendMessageOnLink(_dedicatedLink, msg);
QString what = (componentID == MAV_COMP_ID_ALL) ? "MAV_COMP_ID_ALL" : QString::number(componentID); QString what = (componentID == MAV_COMP_ID_ALL) ? "MAV_COMP_ID_ALL" : QString::number(componentID);
qCDebug(ParameterLoaderLog) << "Request to refresh all parameters for component ID:" << what; qCDebug(ParameterLoaderLog) << "Request to refresh all parameters for component ID:" << what;
...@@ -529,7 +530,7 @@ void ParameterLoader::_tryCacheLookup() ...@@ -529,7 +530,7 @@ void ParameterLoader::_tryCacheLookup()
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_param_request_read_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, _vehicle->id(), MAV_COMP_ID_ALL, "_HASH_CHECK", -1); mavlink_msg_param_request_read_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, _vehicle->id(), MAV_COMP_ID_ALL, "_HASH_CHECK", -1);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); _vehicle->sendMessageOnLink(_dedicatedLink, msg);
} }
void ParameterLoader::_readParameterRaw(int componentId, const QString& paramName, int paramIndex) void ParameterLoader::_readParameterRaw(int componentId, const QString& paramName, int paramIndex)
...@@ -545,7 +546,7 @@ void ParameterLoader::_readParameterRaw(int componentId, const QString& paramNam ...@@ -545,7 +546,7 @@ void ParameterLoader::_readParameterRaw(int componentId, const QString& paramNam
componentId, // Target component id componentId, // Target component id
fixedParamName, // Named parameter being requested fixedParamName, // Named parameter being requested
paramIndex); // Parameter index being requested, -1 for named paramIndex); // Parameter index being requested, -1 for named
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); _vehicle->sendMessageOnLink(_dedicatedLink, msg);
} }
void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramName, const QVariant& value) void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramName, const QVariant& value)
...@@ -598,7 +599,7 @@ void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramNa ...@@ -598,7 +599,7 @@ void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramNa
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_param_set_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &p); mavlink_msg_param_set_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &p);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); _vehicle->sendMessageOnLink(_dedicatedLink, msg);
} }
void ParameterLoader::_writeLocalParamCache() void ParameterLoader::_writeLocalParamCache()
...@@ -675,7 +676,7 @@ void ParameterLoader::_saveToEEPROM(void) ...@@ -675,7 +676,7 @@ void ParameterLoader::_saveToEEPROM(void)
if (_vehicle->firmwarePlugin()->isCapable(FirmwarePlugin::MavCmdPreflightStorageCapability)) { if (_vehicle->firmwarePlugin()->isCapable(FirmwarePlugin::MavCmdPreflightStorageCapability)) {
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_command_long_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, _vehicle->id(), 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0); mavlink_msg_command_long_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, _vehicle->id(), 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); _vehicle->sendMessageOnLink(_dedicatedLink, msg);
qCDebug(ParameterLoaderLog) << "_saveToEEPROM"; qCDebug(ParameterLoaderLog) << "_saveToEEPROM";
} else { } else {
qCDebug(ParameterLoaderLog) << "_saveToEEPROM skipped due to FirmwarePlugin::isCapable"; qCDebug(ParameterLoaderLog) << "_saveToEEPROM skipped due to FirmwarePlugin::isCapable";
......
...@@ -127,6 +127,8 @@ private: ...@@ -127,6 +127,8 @@ private:
FactMetaData::ValueType_t _mavTypeToFactType(MAV_PARAM_TYPE mavType); FactMetaData::ValueType_t _mavTypeToFactType(MAV_PARAM_TYPE mavType);
void _saveToEEPROM(void); void _saveToEEPROM(void);
void _checkInitialLoadComplete(void); void _checkInitialLoadComplete(void);
LinkInterface* _dedicatedLink; ///< Parameter protocol stays on this link
/// First mapping is by component id /// First mapping is by component id
/// Second mapping is parameter name, to Fact* in QVariant /// Second mapping is parameter name, to Fact* in QVariant
......
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