Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 6 additions & 3 deletions src/AutoPilotPlugins/APM/APMSensorsComponentController.cc
Original file line number Diff line number Diff line change
Expand Up @@ -381,13 +381,16 @@ void APMSensorsComponentController::_refreshParams()
QStringLiteral("INS_ACCOFFS_X"), QStringLiteral("INS_ACCOFFS_Y"), QStringLiteral("INS_ACCOFFS_Z")
};

// After a calibration finishes (success or cancel) the FC can be slow to answer the
// bulk refresh, so suppress the per-parameter "read failed" popups — otherwise dozens
// of modals stack up and freeze the UI (e.g. cancelling compass cal on ArduPilot).
for (const QString &paramName : fastRefreshList) {
_vehicle->parameterManager()->refreshParameter(ParameterManager::defaultComponentId, paramName);
_vehicle->parameterManager()->refreshParameter(ParameterManager::defaultComponentId, paramName, false /* notifyFailure */);
}

// Now ask for all to refresh
_vehicle->parameterManager()->refreshParametersPrefix(ParameterManager::defaultComponentId, QStringLiteral("COMPASS_"));
_vehicle->parameterManager()->refreshParametersPrefix(ParameterManager::defaultComponentId, QStringLiteral("INS_"));
_vehicle->parameterManager()->refreshParametersPrefix(ParameterManager::defaultComponentId, QStringLiteral("COMPASS_"), false /* notifyFailure */);
_vehicle->parameterManager()->refreshParametersPrefix(ParameterManager::defaultComponentId, QStringLiteral("INS_"), false /* notifyFailure */);
}

void APMSensorsComponentController::_updateAndEmitShowOrientationCalArea(bool show)
Expand Down
10 changes: 6 additions & 4 deletions src/AutoPilotPlugins/PX4/SensorsComponentController.cc
Original file line number Diff line number Diff line change
Expand Up @@ -434,15 +434,17 @@ void SensorsComponentController::_refreshParams(void)
{
QStringList fastRefreshList;

// We ask for a refresh on these first so that the rotation combo show up as fast as possible
// We ask for a refresh on these first so that the rotation combo show up as fast as possible.
// Suppress per-parameter failure popups: a slow/unresponsive FC otherwise stacks dozens of
// modals during the bulk refresh and freezes the UI.
fastRefreshList << "CAL_MAG0_ID" << "CAL_MAG1_ID" << "CAL_MAG2_ID" << "CAL_MAG0_ROT" << "CAL_MAG1_ROT" << "CAL_MAG2_ROT";
for (const QString &paramName : std::as_const(fastRefreshList)) {
_vehicle->parameterManager()->refreshParameter(ParameterManager::defaultComponentId, paramName);
_vehicle->parameterManager()->refreshParameter(ParameterManager::defaultComponentId, paramName, false /* notifyFailure */);
}

// Now ask for all to refresh
_vehicle->parameterManager()->refreshParametersPrefix(ParameterManager::defaultComponentId, "CAL_");
_vehicle->parameterManager()->refreshParametersPrefix(ParameterManager::defaultComponentId, "SENS_");
_vehicle->parameterManager()->refreshParametersPrefix(ParameterManager::defaultComponentId, "CAL_", false /* notifyFailure */);
_vehicle->parameterManager()->refreshParametersPrefix(ParameterManager::defaultComponentId, "SENS_", false /* notifyFailure */);
}

void SensorsComponentController::_updateAndEmitShowOrientationCalArea(bool show)
Expand Down
8 changes: 4 additions & 4 deletions src/FactSystem/ParameterManager.cc
Original file line number Diff line number Diff line change
Expand Up @@ -704,23 +704,23 @@ int ParameterManager::_actualComponentId(int componentId) const
return componentId;
}

void ParameterManager::refreshParameter(int componentId, const QString &paramName)
void ParameterManager::refreshParameter(int componentId, const QString &paramName, bool notifyFailure)
{
componentId = _actualComponentId(componentId);

qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "refreshParameter - name:" << paramName << ")";

_mavlinkParamRequestRead(componentId, paramName, -1, true /* notifyFailure */);
_mavlinkParamRequestRead(componentId, paramName, -1, notifyFailure);
}

void ParameterManager::refreshParametersPrefix(int componentId, const QString &namePrefix)
void ParameterManager::refreshParametersPrefix(int componentId, const QString &namePrefix, bool notifyFailure)
{
componentId = _actualComponentId(componentId);
qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "refreshParametersPrefix - name:" << namePrefix << ")";

for (const QString &paramName: _mapCompId2FactMap[componentId].keys()) {
if (paramName.startsWith(namePrefix)) {
refreshParameter(componentId, paramName);
refreshParameter(componentId, paramName, notifyFailure);
}
}
}
Expand Down
9 changes: 6 additions & 3 deletions src/FactSystem/ParameterManager.h
Original file line number Diff line number Diff line change
Expand Up @@ -55,11 +55,14 @@ class ParameterManager : public QObject
/// vehicle is not PX4, cacheCheckOnlyFailed() is emitted.
void tryHashCheckCacheLoad();

/// Request a refresh on the specific parameter
void refreshParameter(int componentId, const QString &paramName);
/// Request a refresh on the specific parameter.
/// @param notifyFailure: when true (default) a user-visible popup is shown if the
/// vehicle never responds. Pass false for batch/background refreshes that would
/// otherwise spawn one modal per parameter and freeze the UI.
void refreshParameter(int componentId, const QString &paramName, bool notifyFailure = true);

/// Request a refresh on all parameters that begin with the specified prefix
void refreshParametersPrefix(int componentId, const QString &namePrefix);
void refreshParametersPrefix(int componentId, const QString &namePrefix, bool notifyFailure = true);

void resetAllParametersToDefaults();
void resetAllToVehicleConfiguration();
Expand Down
157 changes: 100 additions & 57 deletions src/QmlControls/ParameterEditorController.cc
Original file line number Diff line number Diff line change
Expand Up @@ -397,72 +397,115 @@ bool ParameterEditorController::buildDiffFromFile(const QString& filename)

QTextStream stream(&file);

// Accept tab/whitespace/comma separators so we handle QGC's tab-separated
// export, Mission Planner whitespace-aligned dumps, and simple `name,value`
// CSVs from ArduPilot tooling.
static const QRegularExpression rxSeparator(QStringLiteral("[\\s,]+"));

int firstComponentId = -1;
while (!stream.atEnd()) {
QString line = stream.readLine();
if (!line.startsWith("#")) {
QStringList wpParams = line.split("\t");
if (wpParams.size() == 5) {
int vehicleId = wpParams.at(0).toInt();
int componentId = wpParams.at(1).toInt();
QString paramName = wpParams.at(2);
QString fileValueStr = wpParams.at(3);
int mavParamType = wpParams.at(4).toInt();
QString vehicleValueStr;
QString units;
QVariant fileValueVar = fileValueStr;
bool noVehicleValue = false;
bool readOnly = false;

if (_vehicle->id() != vehicleId) {
_diffOtherVehicle = true;
}
if (firstComponentId == -1) {
firstComponentId = componentId;
} else if (firstComponentId != componentId) {
_diffMultipleComponents = true;
}
const QString line = stream.readLine().trimmed();
if (line.isEmpty() || line.startsWith("#")) {
continue;
}

if (_parameterMgr->parameterExists(componentId, paramName)) {
Fact* vehicleFact = _parameterMgr->getParameter(componentId, paramName);
FactMetaData* vehicleFactMetaData = vehicleFact->metaData();
Fact* fileFact = new Fact(vehicleFact->componentId(), vehicleFact->name(), vehicleFact->type(), this);

// Turn off reboot messaging before setting value in fileFact
bool vehicleRebootRequired = vehicleFactMetaData->vehicleRebootRequired();
vehicleFactMetaData->setVehicleRebootRequired(false);
fileFact->setMetaData(vehicleFact->metaData());
fileFact->setRawValue(fileValueStr);
vehicleFactMetaData->setVehicleRebootRequired(vehicleRebootRequired);
readOnly = vehicleFact->readOnly();

if (vehicleFact->rawValue() == fileFact->rawValue()) {
continue;
}
fileValueStr = fileFact->enumOrValueString();
fileValueVar = fileFact->rawValue();
vehicleValueStr = vehicleFact->enumOrValueString();
units = vehicleFact->cookedUnits();
} else {
noVehicleValue = true;
}
const QStringList tokens = line.split(rxSeparator, Qt::SkipEmptyParts);

int vehicleId = _vehicle->id();
int componentId = MAV_COMP_ID_AUTOPILOT1;
QString paramName;
QString fileValueStr;
int mavParamType = -1;

// Supported per-line formats:
// 5 tokens: vehicleId componentId name value mavType
// 2 tokens: name value (component/type resolved against the vehicle)
if (tokens.size() == 5) {
vehicleId = tokens.at(0).toInt();
componentId = tokens.at(1).toInt();
paramName = tokens.at(2);
fileValueStr = tokens.at(3);
mavParamType = tokens.at(4).toInt();
} else if (tokens.size() == 2) {
paramName = tokens.at(0);
fileValueStr = tokens.at(1);
} else {
continue;
}

if (!readOnly) {
ParameterEditorDiff* paramDiff = new ParameterEditorDiff(this);
QString vehicleValueStr;
QString units;
QVariant fileValueVar = fileValueStr;
bool noVehicleValue = false;
bool readOnly = false;

paramDiff->componentId = componentId;
paramDiff->name = paramName;
paramDiff->valueType = ParameterManager::mavTypeToFactType(static_cast<MAV_PARAM_TYPE>(mavParamType));
paramDiff->fileValue = fileValueStr;
paramDiff->fileValueVar = fileValueVar;
paramDiff->vehicleValue = vehicleValueStr;
paramDiff->noVehicleValue = noVehicleValue;
paramDiff->units = units;
if (_vehicle->id() != vehicleId) {
_diffOtherVehicle = true;
}

_diffList.append(paramDiff);
// 2-column files don't carry a component id; locate the parameter on
// whichever component owns it before recording multi-component state.
if (tokens.size() == 2 && !_parameterMgr->parameterExists(componentId, paramName)) {
for (int compId : _parameterMgr->componentIds()) {
if (_parameterMgr->parameterExists(compId, paramName)) {
componentId = compId;
break;
}
}
}

if (firstComponentId == -1) {
firstComponentId = componentId;
} else if (firstComponentId != componentId) {
_diffMultipleComponents = true;
}

if (_parameterMgr->parameterExists(componentId, paramName)) {
Fact* vehicleFact = _parameterMgr->getParameter(componentId, paramName);
FactMetaData* vehicleFactMetaData = vehicleFact->metaData();
Fact* fileFact = new Fact(vehicleFact->componentId(), vehicleFact->name(), vehicleFact->type(), this);

if (mavParamType < 0) {
mavParamType = ParameterManager::factTypeToMavType(vehicleFact->type());
}

// Turn off reboot messaging before setting value in fileFact
bool vehicleRebootRequired = vehicleFactMetaData->vehicleRebootRequired();
vehicleFactMetaData->setVehicleRebootRequired(false);
fileFact->setMetaData(vehicleFact->metaData());
fileFact->setRawValue(fileValueStr);
vehicleFactMetaData->setVehicleRebootRequired(vehicleRebootRequired);
readOnly = vehicleFact->readOnly();

if (vehicleFact->rawValue() == fileFact->rawValue()) {
continue;
}
fileValueStr = fileFact->enumOrValueString();
fileValueVar = fileFact->rawValue();
vehicleValueStr = vehicleFact->enumOrValueString();
units = vehicleFact->cookedUnits();
} else {
noVehicleValue = true;
if (mavParamType < 0) {
// No vehicle parameter and no type column: can't infer what to send.
continue;
}
}

if (!readOnly) {
ParameterEditorDiff* paramDiff = new ParameterEditorDiff(this);

paramDiff->componentId = componentId;
paramDiff->name = paramName;
paramDiff->valueType = ParameterManager::mavTypeToFactType(static_cast<MAV_PARAM_TYPE>(mavParamType));
paramDiff->fileValue = fileValueStr;
paramDiff->fileValueVar = fileValueVar;
paramDiff->vehicleValue = vehicleValueStr;
paramDiff->noVehicleValue = noVehicleValue;
paramDiff->units = units;

_diffList.append(paramDiff);
}
}

file.close();
Expand Down
Loading