diff --git a/src/AutoPilotPlugins/PX4/AM32Component.cc b/src/AutoPilotPlugins/PX4/AM32Component.cc new file mode 100644 index 00000000000..56884b45359 --- /dev/null +++ b/src/AutoPilotPlugins/PX4/AM32Component.cc @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * (c) 2009-2024 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "AM32Component.h" +#include "Vehicle.h" + +AM32Component::AM32Component(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) + : VehicleComponent(vehicle, autopilot, AutoPilotPlugin::UnknownVehicleComponent, parent) + , _name(tr("AM32 ESC")) +{ +} + +QString AM32Component::name(void) const +{ + return _name; +} + +QString AM32Component::description(void) const +{ + return tr("AM32 ESC Setup is used to configure AM32 ESC settings."); +} + +QString AM32Component::iconResource(void) const +{ + return "/qmlimages/EscIndicator.svg"; +} + +bool AM32Component::requiresSetup(void) const +{ + return false; +} + +bool AM32Component::setupComplete(void) const +{ + return true; +} + +QStringList AM32Component::setupCompleteChangedTriggerList(void) const +{ + return QStringList(); +} + +QUrl AM32Component::setupSource(void) const +{ + return QUrl::fromUserInput("qrc:/qml/QGroundControl/AutoPilotPlugins/PX4/AM32Component.qml"); +} + +QUrl AM32Component::summaryQmlSource(void) const +{ + return QUrl::fromUserInput("qrc:/qml/QGroundControl/AutoPilotPlugins/PX4/AM32ComponentSummary.qml"); +} diff --git a/src/AutoPilotPlugins/PX4/AM32Component.h b/src/AutoPilotPlugins/PX4/AM32Component.h new file mode 100644 index 00000000000..ff8ab3b471f --- /dev/null +++ b/src/AutoPilotPlugins/PX4/AM32Component.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * (c) 2009-2024 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "VehicleComponent.h" + +class AM32Component : public VehicleComponent +{ + Q_OBJECT + +public: + AM32Component(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = nullptr); + + // Overrides from VehicleComponent + QStringList setupCompleteChangedTriggerList(void) const override; + + // Overrides from VehicleComponent + QString name (void) const override; + QString description (void) const override; + QString iconResource (void) const override; + bool requiresSetup (void) const override; + bool setupComplete (void) const override; + QUrl setupSource (void) const override; + QUrl summaryQmlSource (void) const override; + bool allowSetupWhileArmed (void) const override { return true; } + +private: + const QString _name; +}; diff --git a/src/AutoPilotPlugins/PX4/AM32Component.qml b/src/AutoPilotPlugins/PX4/AM32Component.qml new file mode 100644 index 00000000000..01d5d254cde --- /dev/null +++ b/src/AutoPilotPlugins/PX4/AM32Component.qml @@ -0,0 +1,35 @@ +/**************************************************************************** + * + * (c) 2009-2024 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick +import QtQuick.Controls +import QtQuick.Layouts + +import QGroundControl +import QGroundControl.FactControls +import QGroundControl.Controls +import QGroundControl.AutoPilotPlugins.PX4 + +SetupPage { + id: am32Page + pageComponent: pageComponent + + Component { + id: pageComponent + + Item { + width: availableWidth + height: availableHeight + + AM32SettingsComponent { + anchors.fill: parent + } + } + } +} diff --git a/src/AutoPilotPlugins/PX4/AM32ComponentSummary.qml b/src/AutoPilotPlugins/PX4/AM32ComponentSummary.qml new file mode 100644 index 00000000000..761152b3421 --- /dev/null +++ b/src/AutoPilotPlugins/PX4/AM32ComponentSummary.qml @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * (c) 2009-2024 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick +import QtQuick.Controls + +import QGroundControl +import QGroundControl.FactControls +import QGroundControl.Controls + +Item { + anchors.fill: parent + + property var vehicle: globals.activeVehicle + + Column { + anchors.fill: parent + + VehicleSummaryRow { + labelText: qsTr("ESCs Detected") + valueText: vehicle && vehicle.am32eeproms ? vehicle.am32eeproms.count : qsTr("None") + } + + VehicleSummaryRow { + labelText: qsTr("AM32 Support") + valueText: { + if (vehicle && vehicle.am32eeproms && vehicle.am32eeproms.count > 0) { + return qsTr("Available") + } + return qsTr("Unavailable") + } + } + + VehicleSummaryRow { + labelText: qsTr("Status") + valueText: vehicle && vehicle.am32eeproms && vehicle.am32eeproms.count > 0 ? qsTr("Ready") : qsTr("Not Connected") + } + } +} diff --git a/src/AutoPilotPlugins/PX4/AM32EscMatchDots.qml b/src/AutoPilotPlugins/PX4/AM32EscMatchDots.qml new file mode 100644 index 00000000000..cc8be1ae5c2 --- /dev/null +++ b/src/AutoPilotPlugins/PX4/AM32EscMatchDots.qml @@ -0,0 +1,33 @@ +import QtQuick +import QGroundControl + +Row { + id: root + + required property string settingName + required property var eeproms + + anchors.right: parent.right + anchors.top: parent.top + anchors.margins: 4 + spacing: 2 + visible: eeproms && eeproms.count > 1 + + Repeater { + model: root.eeproms ? root.eeproms : null + + Rectangle { + property var esc: object + property var setting: esc ? esc.settings[root.settingName] : null + + width: 8 + height: 8 + radius: 4 + color: { + if (!esc || !esc.dataLoaded) return qgcPal.colorGrey + if (!setting) return qgcPal.colorGrey + return setting.matchesMajority ? qgcPal.colorGreen : qgcPal.colorRed + } + } + } +} diff --git a/src/AutoPilotPlugins/PX4/AM32SettingCheckbox.qml b/src/AutoPilotPlugins/PX4/AM32SettingCheckbox.qml new file mode 100644 index 00000000000..0bf4872fbd3 --- /dev/null +++ b/src/AutoPilotPlugins/PX4/AM32SettingCheckbox.qml @@ -0,0 +1,17 @@ +import QtQuick +import QGroundControl.Controls + +QGCCheckBox { + required property var firstEeprom + required property var updateSetting + + property string label: "" + property string settingName: "" + property var setting: firstEeprom ? firstEeprom.settings[settingName] : null + + visible: setting && firstEeprom && firstEeprom.isSettingAvailable(settingName) + text: label + (setting && setting.hasPendingChanges ? " *" : "") + checked: setting ? setting.fact.rawValue != 0 : false + textColor: setting && setting.hasPendingChanges ? qgcPal.colorOrange : (setting && !setting.allMatch ? qgcPal.colorRed : qgcPal.text) + onClicked: updateSetting(settingName, checked) +} diff --git a/src/AutoPilotPlugins/PX4/AM32SettingSlider.qml b/src/AutoPilotPlugins/PX4/AM32SettingSlider.qml new file mode 100644 index 00000000000..ea9d833080b --- /dev/null +++ b/src/AutoPilotPlugins/PX4/AM32SettingSlider.qml @@ -0,0 +1,92 @@ +import QtQuick +import QtQuick.Controls +import QGroundControl +import QGroundControl.Controls + +Column { + id: control + + property string label: "" + property var setting: null + + property var fact: setting ? setting.fact : null + property real value: fact ? fact.rawValue : 0 + + property real from: fact ? fact.min : 0 + property real to: fact ? fact.max : 100 + // fact.increment may be NaN if not set - default to 1 + property real stepSize: (fact && !isNaN(fact.increment)) ? fact.increment : 1 + property int decimalPlaces: fact ? fact.decimalPlaces : 0 + property bool enabled: true + + signal userValueChanged(real newValue) + + spacing: ScreenTools.defaultFontPixelHeight / 4 + + QGCLabel { + text: label + (setting && setting.hasPendingChanges ? " *" : "") + color: setting && setting.hasPendingChanges ? qgcPal.colorOrange : qgcPal.text + anchors.horizontalCenter: parent.horizontalCenter + } + + Slider { + id: slider + width: ScreenTools.defaultFontPixelWidth * 28 + from: parent.from + to: parent.to + value: parent.value + stepSize: parent.stepSize + snapMode: Slider.SnapAlways + enabled: parent.enabled + + onMoved: control.userValueChanged(value) + onPressedChanged: if (!pressed) value = Qt.binding(function() { return control.value }) + + // Visual handle + handle: Rectangle { + x: slider.leftPadding + slider.visualPosition * (slider.availableWidth - width) + y: slider.topPadding + slider.availableHeight / 2 - height / 2 + implicitWidth: ScreenTools.defaultFontPixelHeight + implicitHeight: implicitWidth + radius: width / 2 + color: slider.pressed ? qgcPal.buttonHighlight : qgcPal.button + border.color: qgcPal.text + } + + // Value display + ToolTip { + parent: slider.handle + visible: slider.pressed + text: slider.value.toFixed(decimalPlaces) + } + + // Slider fill direction (left to right) + background: Rectangle { + x: slider.leftPadding + y: slider.topPadding + slider.availableHeight / 2 - height / 2 + implicitWidth: 200 + implicitHeight: 4 + width: slider.availableWidth + height: implicitHeight + radius: 2 + color: qgcPal.window + border.color: qgcPal.text + border.width: 1 + + // Progress fill from left to right + Rectangle { + width: slider.visualPosition * parent.width + height: parent.height + color: qgcPal.text + radius: parent.radius + } + } + } + + // Value label + QGCLabel { + text: value.toFixed(decimalPlaces) + anchors.horizontalCenter: parent.horizontalCenter + font.pointSize: ScreenTools.smallFontPointSize + } +} diff --git a/src/AutoPilotPlugins/PX4/AM32SettingsComponent.qml b/src/AutoPilotPlugins/PX4/AM32SettingsComponent.qml new file mode 100644 index 00000000000..4fb723755b2 --- /dev/null +++ b/src/AutoPilotPlugins/PX4/AM32SettingsComponent.qml @@ -0,0 +1,494 @@ +import QtQuick +import QtQuick.Controls +import QtQuick.Layouts + +import QGroundControl +import QGroundControl.Controls +import QGroundControl.FactControls + +Item { + id: root + + property var vehicle: globals.activeVehicle + property var eeproms: vehicle ? vehicle.am32eeproms : null + property var selectedEeproms: [] + property var firstEeprom: selectedEeproms.length > 0 && eeproms ? eeproms.get(selectedEeproms[0]) : null + + readonly property real _margins: ScreenTools.defaultFontPixelHeight + readonly property real _groupMargins: ScreenTools.defaultFontPixelHeight / 2 + + // Slider configurations matching your exact layout + readonly property var motorSliderConfigs: [ + { label: qsTr("Timing advance"), name: "timingAdvance" }, + { label: qsTr("Startup power"), name: "startupPower" }, + { label: qsTr("Motor KV"), name: "motorKv" }, + { label: qsTr("Motor poles"), name: "motorPoles" }, + { label: qsTr("Beeper volume"), name: "beepVolume" }, + { label: qsTr("PWM Frequency"), name: "pwmFrequency" }, + ] + + readonly property var extendedSliderConfigs: [ + { label: qsTr("Ramp rate"), name: "maxRampSpeed" }, + { label: qsTr("Minimum duty cycle"), name: "minDutyCycle" }, + ] + + readonly property var limitsSliderConfigs: [ + { label: qsTr("Temperature limit"), name: "temperatureLimit" }, + { label: qsTr("Current limit"), name: "currentLimit" }, + { label: qsTr("Low voltage threshold"), name: "lowVoltageThreshold" }, + { label: qsTr("Absolute voltage cutoff"), name: "absoluteVoltageCutoff" }, + ] + + readonly property var currentControlSliderConfigs: [ + { label: qsTr("Current P"), name: "currentPidP" }, + { label: qsTr("Current I"), name: "currentPidI" }, + { label: qsTr("Current D"), name: "currentPidD" }, + ] + + Component.onCompleted: { + if (vehicle && eeproms) { + eeproms.requestReadAll(vehicle) + } + selectAllEeproms() + } + + Connections { + target: eeproms ?? null + function onCountChanged() { + selectAllEeproms() + } + } + + onEepromsChanged: selectAllEeproms() + + function selectAllEeproms() { + var selected = [] + if (eeproms) { + for (var i = 0; i < eeproms.count; i++) { + selected.push(i) + } + } + selectedEeproms = selected + } + + function getEscBorderColor(index) { + if (!eeproms || index >= eeproms.count) return "transparent" + + var esc = eeproms.get(index) + if (!esc) return qgcPal.colorGrey + + var isSelected = selectedEeproms.indexOf(index) >= 0 + if (!isSelected) return qgcPal.colorGrey + + // Yellow for pending changes + if (esc.hasUnsavedChanges) return qgcPal.colorYellow + + // Orange if data not loaded + if (!esc.dataLoaded) return qgcPal.colorOrange + + // Green for reference or matching reference + if (index === selectedEeproms[0]) return qgcPal.colorGreen + + if (firstEeprom && esc.settingsMatch(firstEeprom)) { + return qgcPal.colorGreen + } + + return qgcPal.colorRed + } + + function toggleEscSelection(index) { + var idx = selectedEeproms.indexOf(index) + var newSelection = selectedEeproms.slice() + + if (idx >= 0) { + newSelection.splice(idx, 1) + // Clear pending changes when deselecting + var esc = eeproms.get(index) + if (esc && esc.hasUnsavedChanges) { + esc.discardChanges() + } + } else { + newSelection.push(index) + newSelection.sort(function(a, b) { return a - b }) + } + + selectedEeproms = newSelection + } + + function hasAnyUnsavedChanges() { + if (!eeproms) return false + for (var i = 0; i < selectedEeproms.length; i++) { + var esc = eeproms.get(selectedEeproms[i]) + if (esc && esc.hasUnsavedChanges) { + return true + } + } + return false + } + + function writeSettings() { + eeproms.requestWriteAll(vehicle, selectedEeproms) + } + + function updateSetting(name, value) { + if (!eeproms) return + for (var i = 0; i < selectedEeproms.length; i++) { + var esc = eeproms.get(selectedEeproms[i]) + if (esc && esc.settings[name]) { + esc.settings[name].setPendingValue(value) + } + } + } + + ColumnLayout { + anchors.fill: parent + anchors.margins: _margins + spacing: _margins / 2 + + // No ESCs available message + Item { + Layout.fillWidth: true + Layout.fillHeight: true + visible: !eeproms || eeproms.count === 0 + + QGCLabel { + anchors.centerIn: parent + text: qsTr("No AM32 ESCs detected. Please ensure your vehicle is connected and powered.") + font.pointSize: ScreenTools.largeFontPointSize + } + } + + QGCLabel { + id: escSelectionLabel + Layout.alignment: Qt.AlignHCenter + text: qsTr("Select ESCs to configure") + font.pointSize: ScreenTools.mediumFontPointSize + font.italic: true + } + + RowLayout { + Layout.alignment: Qt.AlignHCenter + spacing: _margins + + // ESC Selection Row + Row { + id: escSelectionRow + spacing: _margins / 2 + visible: eeproms && eeproms.count > 0 + + Repeater { + model: eeproms ? eeproms : null + + Rectangle { + width: ScreenTools.defaultFontPixelWidth * 12 + height: ScreenTools.defaultFontPixelHeight * 4 + radius: ScreenTools.defaultFontPixelHeight / 4 + border.width: 3 + border.color: getEscBorderColor(index) + color: qgcPal.window + + MouseArea { + anchors.fill: parent + onClicked: toggleEscSelection(index) + cursorShape: Qt.PointingHandCursor + } + + Column { + anchors.centerIn: parent + spacing: 4 + + QGCLabel { + text: qsTr("ESC %1").arg(index + 1) + font.bold: true + anchors.horizontalCenter: parent.horizontalCenter + } + + Grid { + columns: 2 + columnSpacing: ScreenTools.defaultFontPixelWidth + rowSpacing: 1 + anchors.horizontalCenter: parent.horizontalCenter + + QGCLabel { text: "FW:"; font.pointSize: ScreenTools.smallFontPointSize } + QGCLabel { + text: (object.firmwareMajor && object.firmwareMinor) ? + object.firmwareMajor.rawValue + "." + object.firmwareMinor.rawValue : "---" + font.pointSize: ScreenTools.smallFontPointSize + } + + QGCLabel { text: "BL:"; font.pointSize: ScreenTools.smallFontPointSize } + QGCLabel { + text: object.bootloaderVersion ? object.bootloaderVersion.rawValue : "---" + font.pointSize: ScreenTools.smallFontPointSize + } + + QGCLabel { text: "EE:"; font.pointSize: ScreenTools.smallFontPointSize } + QGCLabel { + text: object.eepromVersion ? object.eepromVersion.rawValue : "---" + font.pointSize: ScreenTools.smallFontPointSize + } + } + } + } + } + } + + // Action Buttons + ColumnLayout { + Layout.fillWidth: true + spacing: _margins / 2 + + QGCButton { + text: qsTr("Read Settings") + onClicked: eeproms.requestReadAll(vehicle); + } + + QGCButton { + text: qsTr("Write Settings") + enabled: hasAnyUnsavedChanges() && selectedEeproms.length > 0 + highlighted: hasAnyUnsavedChanges() + onClicked: writeSettings() + } + } + } // RowLayout + + // Settings Panel + Flickable { + Layout.fillWidth: true + Layout.fillHeight: true + contentHeight: settingsColumn.height + contentWidth: width + clip: true + visible: firstEeprom && firstEeprom.dataLoaded + + ScrollBar.vertical: ScrollBar { policy: ScrollBar.AsNeeded } + + ColumnLayout { + id: settingsColumn + width: parent.width + spacing: _margins + + // Motor Group + SettingsGroupLayout { + heading: qsTr("Motor") + Layout.fillWidth: true + + RowLayout { + spacing: _margins * 2 + + // Left column with checkboxes + ColumnLayout { + Layout.preferredWidth: ScreenTools.defaultFontPixelWidth * 25 + spacing: _groupMargins / 2 + + AM32SettingCheckbox { firstEeprom: root.firstEeprom; updateSetting: root.updateSetting; label: qsTr("Stuck rotor protection"); settingName: "stuckRotorProtection" } + AM32SettingCheckbox { firstEeprom: root.firstEeprom; updateSetting: root.updateSetting; label: qsTr("Stall protection"); settingName: "stallProtection" } + AM32SettingCheckbox { firstEeprom: root.firstEeprom; updateSetting: root.updateSetting; label: qsTr("Use hall sensors"); settingName: "hallSensors" } + AM32SettingCheckbox { firstEeprom: root.firstEeprom; updateSetting: root.updateSetting; label: qsTr("30ms interval telemetry"); settingName: "telemetry30ms" } + AM32SettingCheckbox { firstEeprom: root.firstEeprom; updateSetting: root.updateSetting; label: qsTr("Variable PWM"); settingName: "variablePwmFreq" } + AM32SettingCheckbox { firstEeprom: root.firstEeprom; updateSetting: root.updateSetting; label: qsTr("Complementary PWM"); settingName: "complementaryPwm" } + AM32SettingCheckbox { firstEeprom: root.firstEeprom; updateSetting: root.updateSetting; label: qsTr("Auto timing advance"); settingName: "autoTiming" } + + Item { Layout.fillHeight: true } // Spacer + } + + // Right side with 3x2 grid of sliders + GridLayout { + Layout.fillWidth: true + columns: 3 + rowSpacing: _margins + columnSpacing: _margins * 1.5 + + Repeater { + model: motorSliderConfigs + + Rectangle { + id: motorSliderRect + property string settingName: modelData.name + + visible: firstEeprom && firstEeprom.isSettingAvailable(settingName) + color: "transparent" + border.color: qgcPal.groupBorder + border.width: 1 + radius: 4 + implicitWidth: sliderColumn.width + _groupMargins * 2 + implicitHeight: sliderColumn.height + _groupMargins * 2 + + AM32EscMatchDots { + settingName: motorSliderRect.settingName + eeproms: root.eeproms + } + + AM32SettingSlider { + id: sliderColumn + anchors.centerIn: parent + label: modelData.label + setting: firstEeprom ? firstEeprom.settings[modelData.name] : null + onUserValueChanged: function(newValue) { updateSetting(modelData.name, newValue) } + } + } + } + } + } + } // Motor Group + + // Extended Settings Group + SettingsGroupLayout { + heading: qsTr("Extended Settings") + Layout.fillWidth: true + + RowLayout { + spacing: _margins * 2 + + // Left column with checkbox + ColumnLayout { + Layout.preferredWidth: ScreenTools.defaultFontPixelWidth * 25 + spacing: _groupMargins + AM32SettingCheckbox { firstEeprom: root.firstEeprom; updateSetting: root.updateSetting; label: qsTr("Disable stick calibration"); settingName: "disableStickCalibration" } + + Item { Layout.fillHeight: true } // Spacer + } + + // Right side with sliders + GridLayout { + Layout.fillWidth: true + columns: 3 + rowSpacing: _margins + columnSpacing: _margins * 1.5 + + Repeater { + model: extendedSliderConfigs + + Rectangle { + id: extendedSliderRect + property string settingName: modelData.name + + visible: firstEeprom && firstEeprom.isSettingAvailable(settingName) + color: "transparent" + border.color: qgcPal.groupBorder + border.width: 1 + radius: 4 + implicitWidth: sliderColumn.width + _groupMargins * 2 + implicitHeight: sliderColumn.height + _groupMargins * 2 + + AM32EscMatchDots { + settingName: extendedSliderRect.settingName + eeproms: root.eeproms + } + + AM32SettingSlider { + id: sliderColumn + anchors.centerIn: parent + label: modelData.label + setting: firstEeprom ? firstEeprom.settings[modelData.name] : null + onUserValueChanged: function(newValue) { updateSetting(modelData.name, newValue) } + } + } + } + } + } + } // Extended Settings Group + + // Limits Group + SettingsGroupLayout { + heading: qsTr("Limits") + Layout.fillWidth: true + + RowLayout { + spacing: _margins * 2 + + // Left column with checkbox + ColumnLayout { + Layout.preferredWidth: ScreenTools.defaultFontPixelWidth * 25 + spacing: _groupMargins + + AM32SettingCheckbox { firstEeprom: root.firstEeprom; updateSetting: root.updateSetting; label: qsTr("Low voltage cut off"); settingName: "lowVoltageCutoff" } + Item { Layout.fillHeight: true } // Spacer + } + + // Right side with 2x2 grid of sliders + GridLayout { + Layout.fillWidth: true + columns: 2 + rowSpacing: _margins + columnSpacing: _margins * 1.5 + + Repeater { + model: limitsSliderConfigs + + Rectangle { + id: limitsSliderRect + property string settingName: modelData.name + + visible: firstEeprom && firstEeprom.isSettingAvailable(settingName) + color: "transparent" + border.color: qgcPal.groupBorder + border.width: 1 + radius: 4 + implicitWidth: sliderColumn.width + _groupMargins * 2 + implicitHeight: sliderColumn.height + _groupMargins * 2 + + AM32EscMatchDots { + settingName: limitsSliderRect.settingName + eeproms: root.eeproms + } + + AM32SettingSlider { + id: sliderColumn + anchors.centerIn: parent + label: modelData.label + setting: firstEeprom ? firstEeprom.settings[modelData.name] : null + onUserValueChanged: function(newValue) { updateSetting(modelData.name, newValue) } + } + } + } + } + } + } // Limits Group + + // Current Control Group + SettingsGroupLayout { + heading: qsTr("Current Control") + Layout.fillWidth: true + + GridLayout { + width: parent.width + columns: 3 + rowSpacing: _margins + columnSpacing: _margins * 1.5 + + Repeater { + model: currentControlSliderConfigs + + Rectangle { + id: currentControlSliderRect + property string settingName: modelData.name + + visible: firstEeprom && firstEeprom.isSettingAvailable(settingName) + color: "transparent" + border.color: qgcPal.groupBorder + border.width: 1 + radius: 4 + implicitWidth: sliderColumn.width + _groupMargins * 2 + implicitHeight: sliderColumn.height + _groupMargins * 2 + + AM32EscMatchDots { + settingName: currentControlSliderRect.settingName + eeproms: root.eeproms + } + + AM32SettingSlider { + id: sliderColumn + anchors.centerIn: parent + label: modelData.label + setting: firstEeprom ? firstEeprom.settings[modelData.name] : null + onUserValueChanged: function(newValue) { updateSetting(modelData.name, newValue) } + } + } + } // Repeater + } // GridLayout + } // Current Control Group + } // ColumnLayout + } // Flickable + } // ColumnLayout +} // Item diff --git a/src/AutoPilotPlugins/PX4/CMakeLists.txt b/src/AutoPilotPlugins/PX4/CMakeLists.txt index 3b3093257cb..d676c910ac5 100644 --- a/src/AutoPilotPlugins/PX4/CMakeLists.txt +++ b/src/AutoPilotPlugins/PX4/CMakeLists.txt @@ -13,6 +13,8 @@ target_sources(${CMAKE_PROJECT_NAME} AirframeComponentAirframes.h AirframeComponentController.cc AirframeComponentController.h + AM32Component.cc + AM32Component.h FlightModesComponent.cc FlightModesComponent.h PowerComponent.cc @@ -64,7 +66,13 @@ qt_add_qml_module(AutoPilotPluginsPX4Module ActuatorSlider.qml AirframeComponent.qml AirframeComponentSummary.qml + AM32EscMatchDots.qml + AM32SettingsComponent.qml BatteryParams.qml + AM32Component.qml + AM32ComponentSummary.qml + AM32SettingCheckbox.qml + AM32SettingSlider.qml FlightModesComponentSummary.qml PowerComponent.qml PowerComponentSummary.qml diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc index e003ab8fb57..e68d19a6caa 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc @@ -5,6 +5,7 @@ #include "PX4RadioComponent.h" #include "PX4TuningComponent.h" #include "PowerComponent.h" +#include "AM32Component.h" #include "SafetyComponent.h" #include "SensorsComponent.h" #include "ParameterManager.h" @@ -18,6 +19,7 @@ PX4AutoPilotPlugin::PX4AutoPilotPlugin(Vehicle* vehicle, QObject* parent) , _airframeComponent(nullptr) , _radioComponent(nullptr) , _esp8266Component(nullptr) + , _am32Component(nullptr) , _flightModesComponent(nullptr) , _sensorsComponent(nullptr) , _safetyComponent(nullptr) @@ -72,6 +74,17 @@ const QVariantList& PX4AutoPilotPlugin::vehicleComponents(void) _powerComponent->setupTriggerSignals(); _components.append(QVariant::fromValue(static_cast(_powerComponent))); + // Add AM32 component if parameter DSHOT_ESC_TYPE exists and is set to AM32 + bool dshotEscTypeExists = _vehicle->parameterManager()->parameterExists(ParameterManager::defaultComponentId, "DSHOT_ESC_TYPE"); + if (dshotEscTypeExists) { + uint32_t escType = _vehicle->parameterManager()->getParameter(ParameterManager::defaultComponentId, "DSHOT_ESC_TYPE")->rawValue().toUInt(); + if (escType == 1) { + _am32Component = new AM32Component(_vehicle, this, this); + _am32Component->setupTriggerSignals(); + _components.append(QVariant::fromValue(static_cast(_am32Component))); + } + } + if (_vehicle->actuators()) { _vehicle->actuators()->init(); // At this point params are loaded, so we can init the actuators } diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h index 584e7edd19c..a2fb94ee2b4 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h +++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h @@ -6,6 +6,7 @@ #include "AirframeComponent.h" #include "PX4RadioComponent.h" #include "ESP8266Component.h" +#include "AM32Component.h" #include "FlightModesComponent.h" #include "SensorsComponent.h" #include "SafetyComponent.h" @@ -37,6 +38,7 @@ class PX4AutoPilotPlugin : public AutoPilotPlugin AirframeComponent* _airframeComponent; PX4RadioComponent* _radioComponent; ESP8266Component* _esp8266Component; + AM32Component* _am32Component; FlightModesComponent* _flightModesComponent; SensorsComponent* _sensorsComponent; SafetyComponent* _safetyComponent; diff --git a/src/UI/toolbar/EscIndicator.qml b/src/UI/toolbar/EscIndicator.qml index ff980ef5b14..16330731c16 100644 --- a/src/UI/toolbar/EscIndicator.qml +++ b/src/UI/toolbar/EscIndicator.qml @@ -17,46 +17,83 @@ Item { // ESC status properties derived from vehicle data property int _motorCount: _escs && _escs.count > 0 ? _escs.get(0).count.rawValue : 0 - property int _onlineBitmask: _escs && _escs.count > 0? _escs.get(0).info.rawValue : 0 - property int _onlineMotorCount: _getOnlineMotorCount() - property bool _escHealthy: _getEscHealthStatus() + property int _onlineMotorCount: 0 + property bool _escHealthy: false - function _getOnlineMotorCount() { - if (_motorCount === 0) return 0; - - let count = 0; - let mask = _onlineBitmask; - - // Count all set bits in the bitmask - while (mask) { - count += mask & 1; - mask >>= 1; - } - - return count; + function _isMotorOnline(motorIndex) { + // Each ESC's info fact now contains its individual online status (1=online, 0=offline) + return _escs && motorIndex < _escs.count && _escs.get(motorIndex).info.rawValue !== 0 } - function _getEscHealthStatus() { - // Health is good if all expected motors are online and have no failure flags - if (_onlineMotorCount !== _motorCount) return false - - // Check failure flags for each motor (4 per group) - for (let index = 0; index < 4; index++) { - if ((_onlineBitmask & (1 << index)) !== 0) { // Motor is online - if (_escs.get(index).failureFlags > 0) { // Any failure flag set means unhealthy - return false + function _recalcEscStatus() { + // Recalculate online motor count + let onlineCount = 0; + if (_escs && _escs.count > 0) { + for (let i = 0; i < _escs.count; i++) { + if (_isMotorOnline(i)) { + onlineCount++; } } } - - return true + _onlineMotorCount = onlineCount; + + // Recalculate health status + let healthy = true; + if (_onlineMotorCount !== _motorCount) { + healthy = false; + } else if (_escs) { + for (let index = 0; index < _escs.count; index++) { + if (_isMotorOnline(index)) { + if (_escs.get(index).failureFlags.rawValue > 0) { + healthy = false; + break; + } + } + } + } + _escHealthy = healthy; } function getEscStatusColor() { return _escHealthy ? qgcPal.colorGreen : qgcPal.colorRed } + Component.onCompleted: _recalcEscStatus() + + Timer { + id: escDebounceTimer + interval: 50 + running: false + repeat: false + onTriggered: control._recalcEscStatus() + } + + Connections { + target: _escs + function onCountChanged() { escDebounceTimer.restart() } + } + + // Connect to each ESC's info and failureFlags changes via an Instantiator + Instantiator { + model: _escs + active: _escs && _escs.count > 0 + + delegate: QtObject { + property var esc: model ? _escs.get(index) : null + + property var _infoConn: Connections { + target: esc ? esc.info : null + function onRawValueChanged() { escDebounceTimer.restart() } + } + + property var _failureFlagsConn: Connections { + target: esc ? esc.failureFlags : null + function onRawValueChanged() { escDebounceTimer.restart() } + } + } + } + QGCPalette { id: qgcPal } Row { diff --git a/src/UI/toolbar/EscIndicatorPage.qml b/src/UI/toolbar/EscIndicatorPage.qml index 161e0670d65..fca27c18357 100644 --- a/src/UI/toolbar/EscIndicatorPage.qml +++ b/src/UI/toolbar/EscIndicatorPage.qml @@ -14,10 +14,10 @@ ToolIndicatorPage { property string valueNA: qsTr("–", "No data to display") property var _escs: activeVehicle ? activeVehicle.escs : null - property int _onlineBitmask: _escs ? _escs.get(0).info.rawValue : 0 function _isMotorOnline(motorIndex) { - return (_onlineBitmask & (1 << motorIndex)) !== 0 + // Each ESC's info fact now contains its individual online status (1=online, 0=offline) + return _escs && motorIndex < _escs.count && _escs.get(motorIndex).info.rawValue !== 0 } function _isMotorHealthy(motorIndex) { @@ -41,6 +41,7 @@ ToolIndicatorPage { Layout.fillWidth: true label: qsTr("Healthy Motors") labelText: { + if (!_escs) return valueNA let healthyCount = 0 for (let i = 0; i < _escs.count; i++) { if (_isMotorHealthy(i)) healthyCount++ @@ -53,6 +54,7 @@ ToolIndicatorPage { Layout.fillWidth: true label: qsTr("Total Errors") labelText: { + if (!_escs) return valueNA let totalErrors = 0 for (let i = 0; i < _escs.count; i++) { totalErrors += _escs.get(i).errorCount.rawValue @@ -63,44 +65,53 @@ ToolIndicatorPage { } } - Repeater { - model: _escs + GridLayout { + rows: _escs ? Math.ceil(_escs.count / 2) : 1 + columns: _escs && _escs.count > 4 ? 2 : 1 + flow: GridLayout.TopToBottom + columnSpacing: ScreenTools.defaultFontPixelWidth + rowSpacing: ScreenTools.defaultFontPixelHeight / 4 - SettingsGroupLayout { - heading: qsTr("Motor %1 %2").arg(object.id.rawValue + 1).arg( _isThisMotorHealthy ? "" : qsTr("- OFFLINE")) - headingPointSize: ScreenTools.defaultFontPointSize * ScreenTools.smallFontPointRatio - outerBorderColor: _isThisMotorHealthy ? QGroundControl.globalPalette.colorGreen : QGroundControl.globalPalette.colorRed + Repeater { + model: _escs - property bool _isThisMotorHealthy: _isMotorHealthy(index) + SettingsGroupLayout { + Layout.fillWidth: true + heading: qsTr("Motor %1 %2").arg(object.id.rawValue + 1).arg( _isThisMotorHealthy ? "" : qsTr("- OFFLINE")) + headingPointSize: ScreenTools.defaultFontPointSize * ScreenTools.smallFontPointRatio + outerBorderColor: _isThisMotorHealthy ? QGroundControl.globalPalette.colorGreen : QGroundControl.globalPalette.colorRed - GridLayout { - columns: 2 - columnSpacing: ScreenTools.defaultFontPixelWidth * 2.5 - flow: GridLayout.LeftToRight + property bool _isThisMotorHealthy: _isMotorHealthy(index) - LabelledFactLabel { - label: qsTr("RPM") - fact: object.rpm - } + GridLayout { + columns: 2 + columnSpacing: ScreenTools.defaultFontPixelWidth * 2.5 + flow: GridLayout.LeftToRight - LabelledLabel { - label: qsTr("Temp") - labelText: (object.temperature !== 32767 ? object.temperature.valueString : na) + " " + object.temperature.units - } + LabelledFactLabel { + label: qsTr("RPM") + fact: object.rpm + } - LabelledFactLabel { - label: qsTr("Voltage") - fact: object.voltage - } + LabelledLabel { + label: qsTr("Temp") + labelText: (object.temperature.rawValue !== 32767 ? object.temperature.valueString : na) + " " + object.temperature.units + } - LabelledFactLabel { - label: qsTr("Current") - fact: object.current - } + LabelledFactLabel { + label: qsTr("Voltage") + fact: object.voltage + } - LabelledFactLabel { - label: qsTr("Errors") - fact: object.errorCount + LabelledFactLabel { + label: qsTr("Current") + fact: object.current + } + + LabelledFactLabel { + label: qsTr("Errors") + fact: object.errorCount + } } } } diff --git a/src/Vehicle/FactGroups/AM32EepromFact.json b/src/Vehicle/FactGroups/AM32EepromFact.json new file mode 100644 index 00000000000..1827f7149f5 --- /dev/null +++ b/src/Vehicle/FactGroups/AM32EepromFact.json @@ -0,0 +1,41 @@ +{ + "version": 1, + "fileType": "FactMetaData", + "QGC.MetaData.Facts": +[ +{ + "name": "escIndex", + "shortDesc": "ESC Index", + "type": "uint32", + "default": 0 +}, +{ + "name": "eepromVersion", + "shortDesc": "EEPROM Version", + "type": "uint8", + "default": 0, + "readOnly": true +}, +{ + "name": "bootloaderVersion", + "shortDesc": "Bootloader Version", + "type": "uint8", + "default": 0, + "readOnly": true +}, +{ + "name": "firmwareMajor", + "shortDesc": "Firmware Major", + "type": "uint8", + "default": 0, + "readOnly": true +}, +{ + "name": "firmwareMinor", + "shortDesc": "Firmware Minor", + "type": "uint8", + "default": 0, + "readOnly": true +} +] +} diff --git a/src/Vehicle/FactGroups/AM32EepromFactGroupListModel.cc b/src/Vehicle/FactGroups/AM32EepromFactGroupListModel.cc new file mode 100644 index 00000000000..443ed1bc081 --- /dev/null +++ b/src/Vehicle/FactGroups/AM32EepromFactGroupListModel.cc @@ -0,0 +1,624 @@ +/**************************************************************************** + * + * (c) 2025 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "AM32EepromFactGroupListModel.h" +#include "AM32EepromSchema.h" +#include "MAVLinkProtocol.h" +#include "QGCLoggingCategory.h" +#include "Vehicle.h" + +QGC_LOGGING_CATEGORY(AM32EepromLog, "Vehicle.AM32Eeprom") + + +//----------------------------------------------------------------------------- +// AM32Setting Implementation +//----------------------------------------------------------------------------- + +AM32Setting::AM32Setting(const AM32FieldDef& fieldDef, QObject* parent) + : QObject(parent) + , _eepromByteIndex(fieldDef.offset) + , _displayName(fieldDef.displayName) + , _description(fieldDef.description) + , _unit(fieldDef.unit) + , _isEnum(fieldDef.isEnum) + , _isBool(fieldDef.isBool) + , _fromRaw(fieldDef.fromRaw) + , _toRaw(fieldDef.toRaw) +{ + _fact = new Fact(0, fieldDef.name, fieldDef.valueType, this); + + // Apply min/max/units to the fact's metadata + FactMetaData* metaData = _fact->metaData(); + + // Use display min/max if available, otherwise raw + if (fieldDef.displayMin.isValid()) { + metaData->setRawMin(fieldDef.displayMin); + } else if (fieldDef.rawMin.isValid()) { + metaData->setRawMin(fieldDef.rawMin); + } + + if (fieldDef.displayMax.isValid()) { + metaData->setRawMax(fieldDef.displayMax); + } else if (fieldDef.rawMax.isValid()) { + metaData->setRawMax(fieldDef.rawMax); + } + + if (!fieldDef.unit.isEmpty()) { + metaData->setRawUnits(fieldDef.unit); + } + + // Initialize with a default value (0 in raw units, converted to display units) + _fact->setRawValue(_fromRaw(0)); +} + +void AM32Setting::updateFromEeprom(uint8_t value) +{ + _rawOriginalValue = value; + _fact->setRawValue(_fromRaw(value)); + emit pendingChangesChanged(); +} + +void AM32Setting::updateConversions(const AM32FieldDef& fieldDef) +{ + _fromRaw = fieldDef.fromRaw; + _toRaw = fieldDef.toRaw; + + // Update fact metadata with version-specific min/max + FactMetaData* metaData = _fact->metaData(); + if (fieldDef.displayMin.isValid()) { + metaData->setRawMin(fieldDef.displayMin); + } + if (fieldDef.displayMax.isValid()) { + metaData->setRawMax(fieldDef.displayMax); + } +} + +uint8_t AM32Setting::getRawValue() const +{ + return _toRaw(_fact->rawValue()); +} + +void AM32Setting::setPendingValue(const QVariant& value) +{ + _fact->setRawValue(value); + emit pendingChangesChanged(); +} + +bool AM32Setting::hasPendingChanges() const +{ + return getRawValue() != _rawOriginalValue; +} + +void AM32Setting::discardChanges() +{ + _fact->setRawValue(_fromRaw(_rawOriginalValue)); + emit pendingChangesChanged(); +} + +void AM32Setting::setMatchesMajority(bool matches) +{ + if (_matchesMajority != matches) { + _matchesMajority = matches; + emit matchesMajorityChanged(); + } +} + +void AM32Setting::setAllMatch(bool allMatch) +{ + if (_allMatch != allMatch) { + _allMatch = allMatch; + emit allMatchChanged(); + } +} + +//----------------------------------------------------------------------------- +// AM32EepromFactGroupListModel Implementation +//----------------------------------------------------------------------------- + +AM32EepromFactGroupListModel::AM32EepromFactGroupListModel(QObject* parent) + : FactGroupListModel("am32Eeprom", parent) +{ + // Start fetching schema if not already loaded + AM32EepromSchema* schema = AM32EepromSchema::instance(); + if (!schema->isLoaded() && !schema->isFetching()) { + schema->fetchSchema(); + } +} + +bool AM32EepromFactGroupListModel::_shouldHandleMessage(const mavlink_message_t &message, QList &ids) const +{ + if (message.msgid == MAVLINK_MSG_ID_ESC_EEPROM) { + mavlink_esc_eeprom_t eeprom{}; + mavlink_msg_esc_eeprom_decode(&message, &eeprom); + + // Only handle AM32 firmware messages + if (eeprom.firmware != ESC_FIRMWARE_AM32) { + return false; + } + + ids.append(eeprom.esc_index); + return true; + } + + return false; +} + +void AM32EepromFactGroupListModel::requestReadAll(Vehicle* vehicle) +{ + if (!vehicle) { + return; + } + + vehicle->sendMavCommand( + vehicle->defaultComponentId(), + MAV_CMD_REQUEST_MESSAGE, + false, // showError + MAVLINK_MSG_ID_ESC_EEPROM, + 255, // param2: ESC index (255 = all) + 0, 0, 0, 0, 0 // unused params + ); +} + +bool AM32EepromFactGroupListModel::_allEscsHaveMatchingChanges(const QList& escIndices) +{ + // Broadcast only when every ESC is selected and all share the same pending + // changes. This is intentionally strict — partial selection always uses + // individual writes to avoid accidentally overwriting ESCs the user didn't select. + if (escIndices.isEmpty() || escIndices.count() != count()) { + return false; + } + + auto* firstEsc = value(escIndices[0]); + if (!firstEsc || !firstEsc->hasUnsavedChanges()) { + return false; + } + + for (int i = 1; i < escIndices.count(); i++) { + auto* esc = value(escIndices[i]); + if (!esc || !esc->hasUnsavedChanges()) { + return false; + } + + if (!esc->settingsMatch(firstEsc)) { + return false; + } + } + + return true; +} + +void AM32EepromFactGroupListModel::_sendEepromWrite(Vehicle* vehicle, uint8_t escIndex, const QByteArray& data, const uint32_t writeMask[6]) +{ + qCDebug(AM32EepromLog) << "Writing AM32 EEPROM to ESC" << (escIndex == 255 ? "broadcast" : QString::number(escIndex + 1)) + << "target system:" << vehicle->id() << "component:" << vehicle->defaultComponentId(); + qCDebug(AM32EepromLog) << "Write mask:" << Qt::hex + << writeMask[0] << writeMask[1] << writeMask[2] + << writeMask[3] << writeMask[4] << writeMask[5]; + + mavlink_message_t msg; + mavlink_esc_eeprom_t eeprom{}; + + eeprom.target_system = vehicle->id(); + eeprom.target_component = vehicle->defaultComponentId(); + eeprom.firmware = ESC_FIRMWARE_AM32; + eeprom.msg_index = 0; + eeprom.msg_count = 1; + eeprom.esc_index = escIndex; + memcpy(eeprom.write_mask, writeMask, sizeof(eeprom.write_mask)); + eeprom.length = qMin(data.size(), sizeof(eeprom.data)); + memcpy(eeprom.data, data.data(), eeprom.length); + + SharedLinkInterfacePtr sharedLink = vehicle->vehicleLinkManager()->primaryLink().lock(); + + if (sharedLink) { + mavlink_msg_esc_eeprom_encode_chan( + static_cast(MAVLinkProtocol::instance()->getSystemId()), + static_cast(MAVLinkProtocol::getComponentId()), + sharedLink->mavlinkChannel(), + &msg, + &eeprom + ); + + vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg); + } +} + +void AM32EepromFactGroupListModel::requestWriteAll(Vehicle* vehicle, const QList& escIndices) +{ + if (!vehicle || escIndices.isEmpty()) { + return; + } + + if (_allEscsHaveMatchingChanges(escIndices)) { + // Broadcast write - all ESCs get the same data + qCDebug(AM32EepromLog) << "All ESCs have matching changes, using broadcast write"; + + auto* firstEsc = value(escIndices[0]); + if (!firstEsc) { + return; + } + + QByteArray packedData = firstEsc->getEepromData(); + + uint32_t writeMask[6] = {0}; + for (int escIndex : escIndices) { + auto* esc = value(escIndex); + if (esc && esc->hasUnsavedChanges()) { + uint32_t escWriteMask[6]; + esc->calculateWriteMask(escWriteMask); + for (int i = 0; i < 6; i++) { + writeMask[i] |= escWriteMask[i]; + } + } + } + + _sendEepromWrite(vehicle, 255, packedData, writeMask); + } else { + // Individual writes - ESCs have different changes + qCDebug(AM32EepromLog) << "ESCs have different changes, writing individually"; + + for (int escIndex : escIndices) { + auto* esc = value(escIndex); + if (esc && esc->hasUnsavedChanges()) { + QByteArray packedData = esc->getEepromData(); + uint32_t writeMask[6]; + esc->calculateWriteMask(writeMask); + _sendEepromWrite(vehicle, esc->escIndex(), packedData, writeMask); + } + } + } +} + +//----------------------------------------------------------------------------- +// AM32EepromFactGroup Implementation +//----------------------------------------------------------------------------- + +void AM32EepromFactGroup::handleMessage(Vehicle *vehicle, const mavlink_message_t &message) +{ + switch (message.msgid) { + case MAVLINK_MSG_ID_ESC_EEPROM: + _handleEscEeprom(vehicle, message); + break; + default: + break; + } +} + +void AM32EepromFactGroup::_handleEscEeprom(Vehicle *vehicle, const mavlink_message_t &message) +{ + Q_UNUSED(vehicle); + + mavlink_esc_eeprom_t eeprom{}; + mavlink_msg_esc_eeprom_decode(&message, &eeprom); + + // Only handle AM32 firmware messages + if (eeprom.firmware != ESC_FIRMWARE_AM32) { + return; + } + + if (eeprom.esc_index != _idFact.rawValue().toUInt()) { + // Only handle messages for our ESC index + return; + } + + if (eeprom.length != AM32EepromFactGroupListModel::kAM32EepromSize) { + qCWarning(AM32EepromLog) << "AM32 EEPROM data length mismatch:" << eeprom.length; + return; + } + + // Store original data + _originalEepromData = QByteArray(reinterpret_cast(eeprom.data), eeprom.length); + + // Parse read-only info + _eepromVersionFact.setRawValue(eeprom.data[1]); + _bootloaderVersionFact.setRawValue(eeprom.data[2]); + _firmwareMajorFact.setRawValue(eeprom.data[3]); + _firmwareMinorFact.setRawValue(eeprom.data[4]); + + int eepromVer = eeprom.data[1]; + + // Apply version-specific overrides to settings + AM32EepromSchema* schema = AM32EepromSchema::instance(); + QString fwVersion = firmwareVersionString(); + for (AM32Setting* setting : _settings) { + const AM32FieldDef* baseDef = schema->field(setting->name()); + if (baseDef && (!baseDef->versionOverrides.isEmpty() || !baseDef->firmwareVersionOverrides.isEmpty())) { + AM32FieldDef updatedDef = *baseDef; + updatedDef.applyVersionOverrides(eepromVer); + updatedDef.applyFirmwareVersionOverrides(fwVersion); + AM32EepromSchema::setupConversionFunctions(updatedDef); + setting->updateConversions(updatedDef); + } + } + + // Update all settings + for (AM32Setting* setting : _settings) { + uint8_t index = setting->byteIndex(); + if (index < eeprom.length) { + setting->updateFromEeprom(eeprom.data[index]); + } + } + + _dataLoaded = true; + emit dataLoadedChanged(); + + // Clear any unsaved changes flag since we just loaded fresh data + _updateHasUnsavedChanges(); + + qCDebug(AM32EepromLog) << "ESC" << (_escIndex + 1) << "received eeprom data, version:" << eepromVersionValue(); +} + +FactGroupWithId *AM32EepromFactGroupListModel::_createFactGroupWithId(uint32_t id) +{ + auto* esc = new AM32EepromFactGroup(id, this); + _connectEscSignals(esc); + return esc; +} + +void AM32EepromFactGroupListModel::_connectEscSignals(AM32EepromFactGroup* esc) +{ + // When data is loaded, recalculate majority matches for all ESCs + connect(esc, &AM32EepromFactGroup::dataLoadedChanged, this, &AM32EepromFactGroupListModel::_updateMajorityMatches); + + // When any setting changes, recalculate majority matches + connect(esc, &AM32EepromFactGroup::hasUnsavedChangesChanged, this, &AM32EepromFactGroupListModel::_updateMajorityMatches); +} + +void AM32EepromFactGroupListModel::_updateMajorityMatches() +{ + // Collect all ESCs + QList escs; + for (int i = 0; i < count(); i++) { + auto* esc = value(i); + if (esc && esc->dataLoaded()) { + escs.append(esc); + } + } + + if (escs.isEmpty()) { + return; + } + + // Get list of setting names from first ESC + auto* firstEsc = escs.first(); + QStringList settingNames = firstEsc->settings()->keys(); + + // For each setting, find majority value and update all ESCs + for (const QString& settingName : settingNames) { + // Count occurrences of each value + QMap valueCounts; + for (auto* esc : escs) { + auto* setting = esc->getSetting(settingName); + if (setting) { + uint8_t rawValue = setting->getRawValue(); + valueCounts[rawValue] = valueCounts.value(rawValue, 0) + 1; + } + } + + // Find the most common value. On ties, the lowest raw value wins + // (QMap iterates in key order), so behavior is deterministic. + uint8_t majorityValue = 0; + int maxCount = 0; + for (auto it = valueCounts.begin(); it != valueCounts.end(); ++it) { + if (it.value() > maxCount) { + maxCount = it.value(); + majorityValue = it.key(); + } + } + + // All ESCs match if there's only one unique value + bool allMatch = (valueCounts.size() == 1); + + // Update each ESC's setting with whether it matches majority and whether all match + for (auto* esc : escs) { + auto* setting = esc->getSetting(settingName); + if (setting) { + setting->setMatchesMajority(setting->getRawValue() == majorityValue); + setting->setAllMatch(allMatch); + } + } + } +} + +AM32EepromFactGroup::AM32EepromFactGroup(uint8_t escIndex, QObject* parent) + : FactGroupWithId(1000, QStringLiteral(":/json/Vehicle/AM32EepromFact.json"), parent) + , _settingsMap(new QQmlPropertyMap(this)) + , _escIndex(escIndex) +{ + _idFact.setRawValue(escIndex); + + // Add read-only facts to the group + _addFact(&_eepromVersionFact); + _addFact(&_bootloaderVersionFact); + _addFact(&_firmwareMajorFact); + _addFact(&_firmwareMinorFact); + + AM32EepromSchema* schema = AM32EepromSchema::instance(); + // Connect first, then check — avoids race where schema loads between check and connect. + // _initializeSettingsFromSchema() guards against double-init via !_settings.isEmpty(). + connect(schema, &AM32EepromSchema::schemaLoaded, this, &AM32EepromFactGroup::_initializeSettingsFromSchema); + if (schema->isLoaded()) { + _initializeSettingsFromSchema(); + } +} + +void AM32EepromFactGroup::_initializeSettingsFromSchema() +{ + // Avoid re-initialization + if (!_settings.isEmpty()) { + return; + } + + AM32EepromSchema* schema = AM32EepromSchema::instance(); + + // Disconnect from signal now that we're initializing + disconnect(schema, &AM32EepromSchema::schemaLoaded, this, &AM32EepromFactGroup::_initializeSettingsFromSchema); + + if (!schema->isLoaded()) { + qCWarning(AM32EepromLog) << "AM32 schema not loaded, ESC settings unavailable"; + return; + } + + // Get all editable fields from schema + // Note: We create settings for ALL fields regardless of EEPROM version. + // The UI will filter based on version. This allows settings to be updated + // when EEPROM data arrives and we know the actual version. + QList fields = schema->editableFieldsForEepromVersion(999); + + for (const AM32FieldDef* fieldDef : fields) { + // Make a mutable copy so we can apply version overrides later if needed + AM32FieldDef field = *fieldDef; + + auto* setting = new AM32Setting(field, this); + + _addFact(setting->fact()); + _settings.append(setting); + _settingsMap->insert(field.name, QVariant::fromValue(setting)); + + connect(setting, &AM32Setting::pendingChangesChanged, this, &AM32EepromFactGroup::_updateHasUnsavedChanges); + } + + qCDebug(AM32EepromLog) << "Initialized" << _settings.count() << "settings from schema for ESC" << (_escIndex + 1); +} + +QString AM32EepromFactGroup::firmwareVersionString() const +{ + return QStringLiteral("%1.%2") + .arg(_firmwareMajorFact.rawValue().toInt()) + .arg(_firmwareMinorFact.rawValue().toInt(), 2, 10, QChar('0')); +} + +AM32Setting* AM32EepromFactGroup::getSetting(const QString& name) +{ + for (auto* setting : _settings) { + if (setting->name() == name) { + return setting; + } + } + return nullptr; +} + +const AM32Setting* AM32EepromFactGroup::getSetting(const QString& name) const +{ + for (const auto* setting : _settings) { + if (setting->name() == name) { + return setting; + } + } + return nullptr; +} + +bool AM32EepromFactGroup::isSettingAvailable(const QString& name) const +{ + AM32EepromSchema* schema = AM32EepromSchema::instance(); + const AM32FieldDef* field = schema->field(name); + + if (!field) { + return false; + } + + // Check EEPROM version + if (!field->isAvailableForEepromVersion(eepromVersionValue())) { + return false; + } + + // Check firmware version + if (!field->isAvailableForFirmwareVersion(firmwareVersionString())) { + return false; + } + + return true; +} + +bool AM32EepromFactGroup::hasUnsavedChanges() const +{ + for (const auto* setting : _settings) { + if (setting->hasPendingChanges()) { + return true; + } + } + return false; +} + +void AM32EepromFactGroup::_updateHasUnsavedChanges() +{ + bool hasChanges = hasUnsavedChanges(); + if (_hasUnsavedChanges != hasChanges) { + _hasUnsavedChanges = hasChanges; + emit hasUnsavedChangesChanged(); + } +} + +bool AM32EepromFactGroup::settingsMatch(const AM32EepromFactGroup* other) const +{ + if (!other || !_dataLoaded || !other->_dataLoaded) { + return false; + } + + // Compare all settings + for (const auto* mySetting : _settings) { + auto* otherSetting = other->getSetting(mySetting->name()); + + if (!otherSetting || + mySetting->getRawValue() != otherSetting->getRawValue()) { + return false; + } + } + + return true; +} + +QByteArray AM32EepromFactGroup::getEepromData() const +{ + QByteArray data = _originalEepromData; + + if (data.isEmpty()) { + data = QByteArray(AM32EepromFactGroupListModel::kAM32EepromSize, 0); + data[0] = 1; // Start byte + } + + // Write all current setting values - the write mask controls what actually gets written + for (const auto* setting : _settings) { + uint8_t index = setting->byteIndex(); + if (index < data.size()) { + data[index] = setting->getRawValue(); + } + } + + return data; +} + +void AM32EepromFactGroup::calculateWriteMask(uint32_t writeMask[6]) const +{ + // Initialize mask to all zeros + memset(writeMask, 0, 6 * sizeof(uint32_t)); + + // Set bits only for modified bytes + for (const auto* setting : _settings) { + if (setting->hasPendingChanges()) { + uint8_t byteIndex = setting->byteIndex(); + if (byteIndex < 192) { + int maskIndex = byteIndex / 32; + int bitIndex = byteIndex % 32; + writeMask[maskIndex] |= (1U << bitIndex); + } + } + } + + // Never write to read-only bytes 0-4 + writeMask[0] &= 0xFFFFFFE0; // Clear bits 0-4 +} + +void AM32EepromFactGroup::discardChanges() +{ + for (auto* setting : _settings) { + setting->discardChanges(); + } + _updateHasUnsavedChanges(); +} diff --git a/src/Vehicle/FactGroups/AM32EepromFactGroupListModel.h b/src/Vehicle/FactGroups/AM32EepromFactGroupListModel.h new file mode 100644 index 00000000000..b5341065d5f --- /dev/null +++ b/src/Vehicle/FactGroups/AM32EepromFactGroupListModel.h @@ -0,0 +1,201 @@ +/**************************************************************************** + * + * (c) 2009-2024 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "QGCMAVLink.h" +#include "FactGroupListModel.h" +#include +#include + +class Vehicle; +class AM32EepromFactGroup; +struct AM32FieldDef; + +/// AM32Setting class with conversion support - created dynamically from schema +class AM32Setting : public QObject +{ + Q_OBJECT + QML_ELEMENT + QML_UNCREATABLE("") + + Q_PROPERTY(QString name READ name CONSTANT) + Q_PROPERTY(QString displayName READ displayName CONSTANT) + Q_PROPERTY(QString description READ description CONSTANT) + Q_PROPERTY(QString unit READ unit CONSTANT) + Q_PROPERTY(Fact* fact READ fact CONSTANT) + Q_PROPERTY(bool hasPendingChanges READ hasPendingChanges NOTIFY pendingChangesChanged) + Q_PROPERTY(bool matchesMajority READ matchesMajority NOTIFY matchesMajorityChanged) + Q_PROPERTY(bool allMatch READ allMatch NOTIFY allMatchChanged) + Q_PROPERTY(bool isEnum READ isEnum CONSTANT) + Q_PROPERTY(bool isBool READ isBool CONSTANT) + +public: + /// Create from schema field definition + explicit AM32Setting(const AM32FieldDef& fieldDef, QObject* parent = nullptr); + + QString name() const { return _fact->name(); } + QString displayName() const { return _displayName; } + QString description() const { return _description; } + QString unit() const { return _unit; } + Fact* fact() { return _fact; } + uint8_t byteIndex() const { return _eepromByteIndex; } + bool isEnum() const { return _isEnum; } + bool isBool() const { return _isBool; } + + bool hasPendingChanges() const; + bool matchesMajority() const { return _matchesMajority; } + void setMatchesMajority(bool matches); + bool allMatch() const { return _allMatch; } + void setAllMatch(bool allMatch); + + Q_INVOKABLE void setPendingValue(const QVariant& value); + void updateFromEeprom(uint8_t value); + void updateConversions(const AM32FieldDef& fieldDef); + uint8_t getRawValue() const; + void discardChanges(); + +signals: + void pendingChangesChanged(); + void matchesMajorityChanged(); + void allMatchChanged(); + +private: + uint8_t _eepromByteIndex; + uint8_t _rawOriginalValue = 0; + Fact* _fact; + QString _displayName; + QString _description; + QString _unit; + bool _matchesMajority = true; + bool _allMatch = true; + bool _isEnum = false; + bool _isBool = false; + + std::function _fromRaw; + std::function _toRaw; +}; + +class AM32EepromFactGroupListModel : public FactGroupListModel +{ + Q_OBJECT + QML_ELEMENT + QML_UNCREATABLE("") + +public: + static constexpr uint8_t kAM32EepromSize = 48; + + explicit AM32EepromFactGroupListModel(QObject* parent = nullptr); + + /// Request EEPROM read from ESC + Q_INVOKABLE void requestReadAll(Vehicle* vehicle); + + /// Write settings to all ESCs at once (broadcast) + Q_INVOKABLE void requestWriteAll(Vehicle* vehicle, const QList& escIndices); + +protected: + // Overrides from FactGroupListModel + bool _shouldHandleMessage(const mavlink_message_t &message, QList &ids) const final; + FactGroupWithId *_createFactGroupWithId(uint32_t id) final; + +private: + bool _allEscsHaveMatchingChanges(const QList& escIndices); + void _sendEepromWrite(Vehicle* vehicle, uint8_t escIndex, const QByteArray& data, const uint32_t writeMask[6]); + void _updateMajorityMatches(); + void _connectEscSignals(AM32EepromFactGroup* esc); +}; + + +/// AM32 ESC EEPROM settings fact group +class AM32EepromFactGroup : public FactGroupWithId +{ + Q_OBJECT + + // Read-only info properties + Q_PROPERTY(Fact* firmwareMajor READ firmwareMajor CONSTANT) + Q_PROPERTY(Fact* firmwareMinor READ firmwareMinor CONSTANT) + Q_PROPERTY(Fact* bootloaderVersion READ bootloaderVersion CONSTANT) + Q_PROPERTY(Fact* eepromVersion READ eepromVersion CONSTANT) + + // Convenience version accessors for QML + Q_PROPERTY(int eepromVersionValue READ eepromVersionValue NOTIFY dataLoadedChanged) + Q_PROPERTY(QString firmwareVersionString READ firmwareVersionString NOTIFY dataLoadedChanged) + + // Dynamic qml binding map for AM32Settings + Q_PROPERTY(QQmlPropertyMap* settings READ settings CONSTANT) + + Q_PROPERTY(bool dataLoaded READ dataLoaded NOTIFY dataLoadedChanged) + Q_PROPERTY(bool hasUnsavedChanges READ hasUnsavedChanges NOTIFY hasUnsavedChangesChanged) + Q_PROPERTY(uint8_t escIndex READ escIndex CONSTANT) + +public: + AM32EepromFactGroup(uint8_t escIndex, QObject* parent = nullptr); + + void handleMessage(Vehicle *vehicle, const mavlink_message_t &message) final; + + QQmlPropertyMap* settings() { return _settingsMap; } + + // Read-only info facts + Fact* firmwareMajor() { return &_firmwareMajorFact; } + Fact* firmwareMinor() { return &_firmwareMinorFact; } + Fact* bootloaderVersion() { return &_bootloaderVersionFact; } + Fact* eepromVersion() { return &_eepromVersionFact; } + + // Convenience accessors + int eepromVersionValue() const { return _eepromVersionFact.rawValue().toInt(); } + QString firmwareVersionString() const; + + // Status + bool dataLoaded() const { return _dataLoaded; } + bool hasUnsavedChanges() const; + uint8_t escIndex() const { return _escIndex; } + + /// Get a setting by name + Q_INVOKABLE AM32Setting* getSetting(const QString& name); + const AM32Setting* getSetting(const QString& name) const; + + /// Check if a setting is available for this ESC's EEPROM/firmware version + Q_INVOKABLE bool isSettingAvailable(const QString& name) const; + + /// Check if settings match another ESC + Q_INVOKABLE bool settingsMatch(const AM32EepromFactGroup* other) const; + + /// Discard all pending changes + Q_INVOKABLE void discardChanges(); + + /// Get current EEPROM data for transmission + QByteArray getEepromData() const; + + void calculateWriteMask(uint32_t writeMask[6]) const; + +signals: + void dataLoadedChanged(); + void hasUnsavedChangesChanged(); + +private: + void _handleEscEeprom(Vehicle *vehicle, const mavlink_message_t &message); + void _initializeSettingsFromSchema(); + void _updateHasUnsavedChanges(); + + // Info facts (read-only) + Fact _eepromVersionFact = Fact(0, QStringLiteral("eepromVersion"), FactMetaData::valueTypeUint8); + Fact _bootloaderVersionFact = Fact(0, QStringLiteral("bootloaderVersion"), FactMetaData::valueTypeUint8); + Fact _firmwareMajorFact = Fact(0, QStringLiteral("firmwareMajor"), FactMetaData::valueTypeUint8); + Fact _firmwareMinorFact = Fact(0, QStringLiteral("firmwareMinor"), FactMetaData::valueTypeUint8); + + // Settings list + QQmlPropertyMap* _settingsMap; + QList _settings; + + // State + bool _dataLoaded = false; + bool _hasUnsavedChanges = false; + uint8_t _escIndex = 0; + QByteArray _originalEepromData; +}; diff --git a/src/Vehicle/FactGroups/AM32EepromSchema.cc b/src/Vehicle/FactGroups/AM32EepromSchema.cc new file mode 100644 index 00000000000..6d0bfb72b7b --- /dev/null +++ b/src/Vehicle/FactGroups/AM32EepromSchema.cc @@ -0,0 +1,527 @@ +/**************************************************************************** + * + * (c) 2025 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "AM32EepromSchema.h" +#include "QGCCachedFileDownload.h" + +#include +#include +#include +#include +#include +#include +#include + +AM32EepromSchema* AM32EepromSchema::_instance = nullptr; + +AM32EepromSchema* AM32EepromSchema::instance() +{ + if (!_instance) { + _instance = new AM32EepromSchema(qApp); + } + return _instance; +} + +AM32EepromSchema::AM32EepromSchema(QObject* parent) + : QObject(parent) + , _cachedFileDownload(new QGCCachedFileDownload( + QStandardPaths::writableLocation(QStandardPaths::CacheLocation) + QLatin1String("/AM32SchemaCache"), this)) +{ + connect(_cachedFileDownload, &QGCCachedFileDownload::finished, + this, &AM32EepromSchema::_onDownloadComplete); +} + +void AM32EepromSchema::fetchSchema() +{ + if (_loaded) { + qCDebug(AM32EepromLog) << "Schema already loaded"; + emit schemaLoaded(); + return; + } + + if (_fetching) { + qCDebug(AM32EepromLog) << "Schema fetch already in progress"; + return; + } + + _fetching = true; + qCDebug(AM32EepromLog) << "Fetching AM32 schema from" << schemaUrl; + + if (!_cachedFileDownload->download(schemaUrl, cacheMaxAgeSec)) { + _fetching = false; + emit schemaLoadError(QStringLiteral("Failed to start schema download")); + } +} + +void AM32EepromSchema::_onDownloadComplete(bool success, const QString& localFile, const QString& errorMsg, bool fromCache) +{ + Q_UNUSED(fromCache); + _fetching = false; + + if (!success) { + qCWarning(AM32EepromLog) << "Schema download failed:" << errorMsg << "- trying built-in schema"; + if (!loadFromFile(QLatin1String(localSchemaPath))) { + emit schemaLoadError(QStringLiteral("Failed to download schema and built-in fallback failed: %1").arg(errorMsg)); + } + return; + } + + qCDebug(AM32EepromLog) << "Schema downloaded to" << localFile; + + if (!loadFromFile(localFile)) { + qCWarning(AM32EepromLog) << "Downloaded schema invalid, trying built-in"; + if (!loadFromFile(QLatin1String(localSchemaPath))) { + // Error already emitted by loadFromFile + } + } +} + +bool AM32EepromSchema::loadFromFile(const QString& path) +{ + QFile file(path); + if (!file.open(QIODevice::ReadOnly)) { + emit schemaLoadError(QStringLiteral("Failed to open schema file: %1").arg(path)); + return false; + } + + QByteArray data = file.readAll(); + file.close(); + + return loadFromJson(data); +} + +bool AM32EepromSchema::loadFromJson(const QByteArray& jsonData) +{ + QJsonParseError error; + QJsonDocument doc = QJsonDocument::fromJson(jsonData, &error); + + if (error.error != QJsonParseError::NoError) { + emit schemaLoadError(QStringLiteral("JSON parse error: %1").arg(error.errorString())); + return false; + } + + if (!doc.isObject()) { + emit schemaLoadError(QStringLiteral("Schema root must be an object")); + return false; + } + + return parseSchema(doc.object()); +} + +bool AM32EepromSchema::parseSchema(const QJsonObject& root) +{ + _fields.clear(); + _groups.clear(); + + _schemaVersion = root.value("version").toString("unknown"); + + // Parse fields + QJsonObject fieldsObj = root.value("fields").toObject(); + for (auto it = fieldsObj.begin(); it != fieldsObj.end(); ++it) { + QString fieldName = it.key(); + QJsonObject fieldObj = it.value().toObject(); + + AM32FieldDef field = parseField(fieldName, fieldObj); + _fields.insert(fieldName, field); + } + + // Parse groups + QJsonObject groupsObj = root.value("groups").toObject(); + for (auto it = groupsObj.begin(); it != groupsObj.end(); ++it) { + AM32FieldGroup group; + group.id = it.key(); + + QJsonObject groupObj = it.value().toObject(); + group.name = groupObj.value("name").toString(); + group.description = groupObj.value("description").toString(); + group.minEepromVersion = groupObj.value("minEepromVersion").toInt(0); + + QJsonArray fieldsArray = groupObj.value("fields").toArray(); + for (const QJsonValue& v : fieldsArray) { + group.fieldNames.append(v.toString()); + } + + _groups.append(group); + } + + _loaded = true; + emit schemaLoaded(); + + qCDebug(AM32EepromLog) << "AM32 Schema loaded:" << _fields.count() << "fields," << _groups.count() << "groups"; + return true; +} + +AM32FieldDef AM32EepromSchema::parseField(const QString& name, const QJsonObject& fieldObj) +{ + AM32FieldDef field; + field.name = name; + field.displayName = fieldObj.value("name").toString(name); + field.description = fieldObj.value("description").toString(); + field.unit = fieldObj.value("unit").toString(); + field.offset = fieldObj.value("offset").toInt(); + field.size = fieldObj.value("size").toInt(1); + field.isReadOnly = fieldObj.value("readOnly").toBool(false); + + // Version gating + field.minEepromVersion = fieldObj.value("minEepromVersion").toInt(0); + field.maxEepromVersion = fieldObj.value("maxEepromVersion").toInt(999); + field.minFirmwareVersion = fieldObj.value("minFirmwareVersion").toString(); + field.maxFirmwareVersion = fieldObj.value("maxFirmwareVersion").toString(); + + // Determine type + QString typeStr = fieldObj.value("type").toString("uint8"); + field.isBool = (typeStr == "bool"); + field.isEnum = (typeStr == "enum"); + + // Parse raw constraints + QJsonObject rawObj = fieldObj.value("raw").toObject(); + if (!rawObj.isEmpty()) { + if (rawObj.contains("min")) { + field.rawMin = rawObj.value("min").toVariant(); + } + if (rawObj.contains("max")) { + field.rawMax = rawObj.value("max").toVariant(); + } + } + + // Parse display conversion + QJsonObject displayObj = fieldObj.value("display").toObject(); + if (!displayObj.isEmpty()) { + field.displayFactor = displayObj.value("factor").toDouble(1.0); + field.displayOffset = displayObj.value("offset").toDouble(0.0); + field.displayDecimals = displayObj.value("decimals").toInt(0); + + if (displayObj.contains("min")) { + field.displayMin = displayObj.value("min").toVariant(); + } + if (displayObj.contains("max")) { + field.displayMax = displayObj.value("max").toVariant(); + } + } + + // Parse enum values + if (field.isEnum) { + QJsonArray valuesArray = fieldObj.value("values").toArray(); + for (const QJsonValue& v : valuesArray) { + QJsonObject enumObj = v.toObject(); + int raw = enumObj.value("raw").toInt(); + QString enumName = enumObj.value("name").toString(); + field.enumValues.append(qMakePair(raw, enumName)); + } + } + + // Parse disabled value + QJsonObject disabledObj = fieldObj.value("disabledValue").toObject(); + if (!disabledObj.isEmpty()) { + field.disabledRawValue = disabledObj.value("raw").toVariant(); + field.disabledDisplayText = disabledObj.value("display").toString("Disabled"); + } + + // Parse version-specific overrides + QJsonObject versionsObj = fieldObj.value("versions").toObject(); + for (auto it = versionsObj.begin(); it != versionsObj.end(); ++it) { + QString versionKey = it.key(); + + // Parse version keys like "eeprom:3+" or "firmware:2.18+" + if (versionKey.startsWith("eeprom:")) { + QString verStr = versionKey.mid(7); + verStr.remove('+'); + bool ok = false; + int ver = verStr.toInt(&ok); + if (!ok) { + qCWarning(AM32EepromLog) << "Invalid eeprom version override key:" << versionKey; + continue; + } + field.versionOverrides.insert(ver, it.value().toObject()); + } + else if (versionKey.startsWith("firmware:")) { + QString verStr = versionKey.mid(9); + verStr.remove('+'); + field.firmwareVersionOverrides.insert(verStr, it.value().toObject()); + } + // "default" is stored as version 0 + else if (versionKey == "default") { + field.versionOverrides.insert(0, it.value().toObject()); + } + } + + // Determine the FactMetaData value type + field.valueType = determineValueType(field); + + // Setup conversion functions + setupConversionFunctions(field); + + return field; +} + +FactMetaData::ValueType_t AM32EepromSchema::determineValueType(const AM32FieldDef& field) +{ + if (field.isBool) { + return FactMetaData::valueTypeBool; + } + + if (field.isEnum) { + return FactMetaData::valueTypeUint8; + } + + // If there's a display conversion + if (field.displayFactor != 1.0 || field.displayOffset != 0.0) { + // Use double if we have decimals + if (field.displayDecimals > 0) { + return FactMetaData::valueTypeDouble; + } + // Use int32 if offset might produce negative values + if (field.displayOffset < 0 || + (field.displayMin.isValid() && field.displayMin.toDouble() < 0)) { + return FactMetaData::valueTypeInt32; + } + return FactMetaData::valueTypeUint32; + } + + return FactMetaData::valueTypeUint8; +} + +void AM32EepromSchema::setupConversionFunctions(AM32FieldDef& field) +{ + double factor = field.displayFactor; + double offset = field.displayOffset; + + // For EEPROM fields, default raw range is 0-255 (uint8) + double rawMinVal = field.rawMin.isValid() ? field.rawMin.toDouble() : 0.0; + double rawMaxVal = field.rawMax.isValid() ? field.rawMax.toDouble() : 255.0; + + // Compute displayMin/displayMax from rawMin/rawMax if not explicitly set + // Formula: display = raw * factor + offset + if (!field.displayMin.isValid()) { + // Handle potential flip if factor is negative + double dispMin = rawMinVal * factor + offset; + double dispMax = rawMaxVal * factor + offset; + field.displayMin = QVariant(qMin(dispMin, dispMax)); + } + if (!field.displayMax.isValid()) { + double dispMin = rawMinVal * factor + offset; + double dispMax = rawMaxVal * factor + offset; + field.displayMax = QVariant(qMax(dispMin, dispMax)); + } + + // Identity conversion + if (factor == 1.0 && offset == 0.0) { + field.fromRaw = [](uint8_t v) { return QVariant(v); }; + field.toRaw = [](QVariant v) { return static_cast(v.toUInt()); }; + return; + } + + // Setup conversion based on value type + switch (field.valueType) { + case FactMetaData::valueTypeDouble: + field.fromRaw = [factor, offset](uint8_t v) { + return QVariant(v * factor + offset); + }; + field.toRaw = [factor, offset](QVariant v) { + return static_cast(qBound(0, static_cast((v.toDouble() - offset) / factor), 255)); + }; + break; + + case FactMetaData::valueTypeInt32: + field.fromRaw = [factor, offset](uint8_t v) { + return QVariant(static_cast(v * factor + offset)); + }; + field.toRaw = [factor, offset](QVariant v) { + return static_cast(qBound(0, static_cast((v.toInt() - offset) / factor), 255)); + }; + break; + + case FactMetaData::valueTypeUint32: + field.fromRaw = [factor, offset](uint8_t v) { + return QVariant(static_cast(v * factor + offset)); + }; + field.toRaw = [factor, offset](QVariant v) { + return static_cast(qBound(0, static_cast((v.toUInt() - offset) / factor), 255)); + }; + break; + + default: + field.fromRaw = [](uint8_t v) { return QVariant(v); }; + field.toRaw = [](QVariant v) { return static_cast(v.toUInt()); }; + break; + } +} + +static QPair parseFirmwareVersion(const QString& v) +{ + QString cleaned = v; + cleaned.remove('v').remove('V'); + QStringList parts = cleaned.split('.'); + return qMakePair(parts.value(0).toInt(), parts.value(1).toInt()); +} + +static int compareFirmwareVersions(QPair a, QPair b) +{ + if (a.first != b.first) return a.first - b.first; + return a.second - b.second; +} + +void AM32FieldDef::_applyOverrideObject(const QJsonObject& override) +{ + if (override.contains("raw")) { + QJsonObject rawObj = override.value("raw").toObject(); + if (rawObj.contains("min")) rawMin = rawObj.value("min").toVariant(); + if (rawObj.contains("max")) rawMax = rawObj.value("max").toVariant(); + } + + if (override.contains("display")) { + QJsonObject displayObj = override.value("display").toObject(); + if (displayObj.contains("factor")) displayFactor = displayObj.value("factor").toDouble(); + if (displayObj.contains("offset")) displayOffset = displayObj.value("offset").toDouble(); + if (displayObj.contains("decimals")) displayDecimals = displayObj.value("decimals").toInt(); + if (displayObj.contains("min")) displayMin = displayObj.value("min").toVariant(); + if (displayObj.contains("max")) displayMax = displayObj.value("max").toVariant(); + } + + if (override.contains("values")) { + enumValues.clear(); + QJsonArray valuesArray = override.value("values").toArray(); + for (const QJsonValue& v : valuesArray) { + QJsonObject enumObj = v.toObject(); + int raw = enumObj.value("raw").toInt(); + QString enumName = enumObj.value("name").toString(); + enumValues.append(qMakePair(raw, enumName)); + } + } +} + +void AM32FieldDef::applyVersionOverrides(int eepromVersion) +{ + // Find the highest version override that applies + // Start at -1 so the "default" override (stored at key 0) can be selected + int bestVersion = -1; + QJsonObject* bestOverride = nullptr; + + for (auto it = versionOverrides.begin(); it != versionOverrides.end(); ++it) { + int overrideVersion = it.key(); + if (overrideVersion <= eepromVersion && overrideVersion > bestVersion) { + bestVersion = overrideVersion; + bestOverride = &it.value(); + } + } + + if (bestOverride) { + _applyOverrideObject(*bestOverride); + } +} + +void AM32FieldDef::applyFirmwareVersionOverrides(const QString& firmwareVersion) +{ + if (firmwareVersionOverrides.isEmpty()) { + return; + } + + QPair current = parseFirmwareVersion(firmwareVersion); + QPair bestVer = qMakePair(-1, -1); + QJsonObject* bestOverride = nullptr; + + for (auto it = firmwareVersionOverrides.begin(); it != firmwareVersionOverrides.end(); ++it) { + QPair overrideVer = parseFirmwareVersion(it.key()); + if (compareFirmwareVersions(overrideVer, current) <= 0 && + compareFirmwareVersions(overrideVer, bestVer) > 0) { + bestVer = overrideVer; + bestOverride = &it.value(); + } + } + + if (bestOverride) { + _applyOverrideObject(*bestOverride); + } +} + +bool AM32FieldDef::isAvailableForFirmwareVersion(const QString& version) const +{ + if (minFirmwareVersion.isEmpty() && maxFirmwareVersion.isEmpty()) { + return true; + } + + QPair current = parseFirmwareVersion(version); + + if (!minFirmwareVersion.isEmpty()) { + QPair minVer = parseFirmwareVersion(minFirmwareVersion); + if (compareFirmwareVersions(current, minVer) < 0) { + return false; + } + } + + if (!maxFirmwareVersion.isEmpty()) { + QPair maxVer = parseFirmwareVersion(maxFirmwareVersion); + if (compareFirmwareVersions(current, maxVer) > 0) { + return false; + } + } + + return true; +} + +const AM32FieldDef* AM32EepromSchema::field(const QString& name) const +{ + auto it = _fields.find(name); + if (it != _fields.end()) { + return &it.value(); + } + return nullptr; +} + +QList AM32EepromSchema::fieldsForEepromVersion(int version) const +{ + QList result; + + for (auto it = _fields.begin(); it != _fields.end(); ++it) { + if (it.value().isAvailableForEepromVersion(version)) { + result.append(&it.value()); + } + } + + // Sort by offset + std::sort(result.begin(), result.end(), [](const AM32FieldDef* a, const AM32FieldDef* b) { + return a->offset < b->offset; + }); + + return result; +} + +QList AM32EepromSchema::editableFieldsForEepromVersion(int version) const +{ + QList result; + + for (auto it = _fields.begin(); it != _fields.end(); ++it) { + const AM32FieldDef& field = it.value(); + if (field.isAvailableForEepromVersion(version) && + !field.isReadOnly && + field.size == 1) { // Skip array fields like melody + result.append(&field); + } + } + + // Sort by offset + std::sort(result.begin(), result.end(), [](const AM32FieldDef* a, const AM32FieldDef* b) { + return a->offset < b->offset; + }); + + return result; +} + +QList AM32EepromSchema::groupsForEepromVersion(int version) const +{ + QList result; + + for (const AM32FieldGroup& group : _groups) { + if (version >= group.minEepromVersion) { + result.append(group); + } + } + + return result; +} diff --git a/src/Vehicle/FactGroups/AM32EepromSchema.h b/src/Vehicle/FactGroups/AM32EepromSchema.h new file mode 100644 index 00000000000..1e30b2c046a --- /dev/null +++ b/src/Vehicle/FactGroups/AM32EepromSchema.h @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * (c) 2025 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "FactMetaData.h" + +#include +#include +#include +#include +#include + +#include + +class QGCCachedFileDownload; + +Q_DECLARE_LOGGING_CATEGORY(AM32EepromLog) + +/// Schema-based field definition loaded from JSON +struct AM32FieldDef { + QString name; + QString displayName; + QString description; + QString unit; + int offset = 0; + int size = 1; + + // Type info + FactMetaData::ValueType_t valueType = FactMetaData::valueTypeUint8; + bool isReadOnly = false; + bool isBool = false; + bool isEnum = false; + + // Version gating + int minEepromVersion = 0; + int maxEepromVersion = 999; + QString minFirmwareVersion; + QString maxFirmwareVersion; + + // Raw value constraints + QVariant rawMin; + QVariant rawMax; + + // Display conversion: display = raw * factor + offset + double displayFactor = 1.0; + double displayOffset = 0.0; + int displayDecimals = 0; + QVariant displayMin; + QVariant displayMax; + + // Enum values (for enum types) + QList> enumValues; + + // Disabled value indicator + QVariant disabledRawValue; + QString disabledDisplayText; + + // Version-specific overrides (eepromVersion -> override data) + QMap versionOverrides; + + // Firmware version-specific overrides (firmwareVersion string -> override data) + QMap firmwareVersionOverrides; + + // Conversion functions (generated from factor/offset) + std::function fromRaw; + std::function toRaw; + + /// Check if this field is available for the given EEPROM version + bool isAvailableForEepromVersion(int version) const { + return version >= minEepromVersion && version <= maxEepromVersion; + } + + /// Check if this field is available for the given firmware version + bool isAvailableForFirmwareVersion(const QString& version) const; + + /// Get the effective display values for a specific EEPROM version + /// (applies version-specific overrides if present) + void applyVersionOverrides(int eepromVersion); + + /// Apply firmware version-specific overrides (e.g. enum values for firmware:2.18+) + void applyFirmwareVersionOverrides(const QString& firmwareVersion); + +private: + void _applyOverrideObject(const QJsonObject& override); +}; + +/// Field group definition for UI organization +struct AM32FieldGroup { + QString id; + QString name; + QString description; + QStringList fieldNames; + int minEepromVersion = 0; +}; + +/// Singleton class that loads and manages the AM32 EEPROM schema +class AM32EepromSchema : public QObject +{ + Q_OBJECT + +public: + static AM32EepromSchema* instance(); + + /// Start downloading schema from am32.ca (cached) + /// Emits schemaLoaded() on success or schemaLoadError() on failure + void fetchSchema(); + + /// Load schema from a JSON file (filesystem) + bool loadFromFile(const QString& path); + + /// Load schema from JSON data + bool loadFromJson(const QByteArray& jsonData); + + /// Get all field definitions + const QMap& fields() const { return _fields; } + + /// Get a specific field definition by name + const AM32FieldDef* field(const QString& name) const; + + /// Get fields available for a specific EEPROM version + QList fieldsForEepromVersion(int version) const; + + /// Get editable (non-readonly) fields for a specific EEPROM version + QList editableFieldsForEepromVersion(int version) const; + + /// Get field groups + const QList& groups() const { return _groups; } + + /// Get groups available for a specific EEPROM version + QList groupsForEepromVersion(int version) const; + + /// Get schema version + QString schemaVersion() const { return _schemaVersion; } + + /// Check if schema is loaded + bool isLoaded() const { return _loaded; } + + /// Check if schema is currently being fetched + bool isFetching() const { return _fetching; } + + /// Setup conversion functions for a field definition + /// (must be called after modifying factor/offset via applyVersionOverrides) + static void setupConversionFunctions(AM32FieldDef& field); + + /// URL for the AM32 EEPROM schema + static constexpr const char* schemaUrl = "https://am32.ca/eeprom.json"; + + /// Built-in schema resource path (fallback for offline use) + static constexpr const char* localSchemaPath = ":/AM32/eeprom.json"; + + /// Cache expiration time in seconds (7 days) + static constexpr int cacheMaxAgeSec = 7 * 24 * 3600; + +signals: + void schemaLoaded(); + void schemaLoadError(const QString& error); + +private slots: + void _onDownloadComplete(bool success, const QString& localFile, const QString& errorMsg, bool fromCache); + +private: + AM32EepromSchema(QObject* parent = nullptr); + + bool parseSchema(const QJsonObject& root); + AM32FieldDef parseField(const QString& name, const QJsonObject& fieldObj); + FactMetaData::ValueType_t determineValueType(const AM32FieldDef& field); + + QMap _fields; + QList _groups; + QString _schemaVersion; + bool _loaded = false; + bool _fetching = false; + + QGCCachedFileDownload* _cachedFileDownload = nullptr; + + static AM32EepromSchema* _instance; +}; diff --git a/src/Vehicle/FactGroups/CMakeLists.txt b/src/Vehicle/FactGroups/CMakeLists.txt index 9952307fc8d..af2510c3381 100644 --- a/src/Vehicle/FactGroups/CMakeLists.txt +++ b/src/Vehicle/FactGroups/CMakeLists.txt @@ -5,6 +5,9 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE + AM32EepromFactGroupListModel.cc + AM32EepromSchema.cc + AM32EepromSchema.h BatteryFactGroupListModel.cc BatteryFactGroupListModel.h EscStatusFactGroupListModel.cc @@ -51,6 +54,7 @@ target_sources(${CMAKE_PROJECT_NAME} # Fact Group Definition Resources # ---------------------------------------------------------------------------- set(JSON_FILES + AM32EepromFact.json RPMFact.json TemperatureFact.json GPSFact.json @@ -82,4 +86,12 @@ qt_add_resources(${CMAKE_PROJECT_NAME} json_vehicle_fact_group_alias FILES LocalPositionFact.json ) +# ---------------------------------------------------------------------------- +# AM32 EEPROM Schema (built-in fallback for offline use) +# ---------------------------------------------------------------------------- +qt_add_resources(${CMAKE_PROJECT_NAME} am32_eeprom_schema + PREFIX "/AM32" + FILES eeprom.json +) + target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/Vehicle/FactGroups/EscStatusFactGroupListModel.cc b/src/Vehicle/FactGroups/EscStatusFactGroupListModel.cc index 4b50f4c8dc4..bd9b6fcebc1 100644 --- a/src/Vehicle/FactGroups/EscStatusFactGroupListModel.cc +++ b/src/Vehicle/FactGroups/EscStatusFactGroupListModel.cc @@ -17,26 +17,31 @@ bool EscStatusFactGroupListModel::_shouldHandleMessage(const mavlink_message_t & switch (message.msgid) { case MAVLINK_MSG_ID_ESC_INFO: { - mavlink_esc_status_t escStatus{}; - mavlink_msg_esc_status_decode(&message, &escStatus); - firstIndex = escStatus.index; - shouldHandle = true; + mavlink_esc_info_t escInfo{}; + mavlink_msg_esc_info_decode(&message, &escInfo); + firstIndex = escInfo.index; + // ESC_INFO should only be handled if index is multiple of 4 + shouldHandle = (firstIndex % 4 == 0); } break; + case MAVLINK_MSG_ID_ESC_STATUS: { mavlink_esc_status_t escStatus{}; mavlink_msg_esc_status_decode(&message, &escStatus); firstIndex = escStatus.index; - shouldHandle = true; + // ESC_STATUS should only be handled if index is multiple of 4 + shouldHandle = (firstIndex % 4 == 0); } break; + default: shouldHandle = false; // Not a message we care about break; } if (shouldHandle) { + // ESC_STATUS and ESC_INFO cover 4 consecutive ESCs for (uint32_t index = firstIndex; index <= firstIndex + 3; index++) { ids.append(index); } @@ -73,6 +78,8 @@ EscStatusFactGroup::EscStatusFactGroup(uint32_t escIndex, QObject *parent) _failureFlagsFact.setRawValue(0); _errorCountFact.setRawValue(0); _temperatureFact.setRawValue(0); + + setLiveUpdates(true); } void EscStatusFactGroup::handleMessage(Vehicle *vehicle, const mavlink_message_t &message) @@ -102,9 +109,12 @@ void EscStatusFactGroup::_handleEscInfo(Vehicle * /*vehicle*/, const mavlink_mes } index %= 4; // Convert to 0-based index for the arrays in escInfo + _countFact.setRawValue(escInfo.count); _connectionTypeFact.setRawValue(escInfo.connection_type); - _infoFact.setRawValue(escInfo.info); + // Extract the individual online status bit for this ESC from the bitmask + bool isOnline = (escInfo.info >> index) & 1; + _infoFact.setRawValue(isOnline ? 1 : 0); _failureFlagsFact.setRawValue(escInfo.failure_flags[index]); _errorCountFact.setRawValue(escInfo.error_count[index]); _temperatureFact.setRawValue(escInfo.temperature[index]); diff --git a/src/Vehicle/FactGroups/eeprom.json b/src/Vehicle/FactGroups/eeprom.json new file mode 100644 index 00000000000..9df16f44dc8 --- /dev/null +++ b/src/Vehicle/FactGroups/eeprom.json @@ -0,0 +1,923 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "$id": "https://github.com/am32-firmware/AM32/am32-eeprom-schema.json", + "title": "AM32 EEPROM Settings Schema", + "description": "Unified schema for AM32 ESC EEPROM layout, settings metadata, and version-specific behavior", + "version": "1.0.1", + + "eepromVersions": { + "2": { + "description": "Legacy EEPROM format (pre-extended settings)", + "minSize": 48, + "bufferSize": 192, + "notes": "Original format without extended settings like current PID. Offsets 5-16 contained ESC name string." + }, + "3": { + "description": "Extended settings format", + "minSize": 48, + "bufferSize": 192, + "notes": "Added current PID, ramp rate, min duty cycle, absolute voltage cutoff at offsets 5-12" + }, + "4": { + "description": "CAN settings format", + "minSize": 184, + "bufferSize": 192, + "notes": "Added CAN bus configuration at offset 176-191" + } + }, + + "fields": { + "bootByte": { + "offset": 0, + "size": 1, + "type": "uint8", + "name": "Boot Byte", + "description": "Boot indicator byte (0x01 = valid)", + "readOnly": true, + "cName": "reserved_0" + }, + + "eepromVersion": { + "offset": 1, + "size": 1, + "type": "uint8", + "name": "EEPROM Version", + "description": "Layout revision number", + "readOnly": true, + "cName": "eeprom_version" + }, + + "bootloaderVersion": { + "offset": 2, + "size": 1, + "type": "uint8", + "name": "Bootloader Version", + "readOnly": true, + "cName": "reserved_1" + }, + + "firmwareMajor": { + "offset": 3, + "size": 1, + "type": "uint8", + "name": "Firmware Major Version", + "readOnly": true, + "cName": "version.major" + }, + + "firmwareMinor": { + "offset": 4, + "size": 1, + "type": "uint8", + "name": "Firmware Minor Version", + "readOnly": true, + "cName": "version.minor" + }, + + "maxRampSpeed": { + "offset": 5, + "size": 1, + "type": "number", + "name": "Max Ramp Speed", + "description": "Maximum throttle ramp rate", + "unit": "% per ms", + "minEepromVersion": 3, + "cName": "max_ramp", + "raw": { + "min": 1, + "max": 200 + }, + "display": { + "min": 0.1, + "max": 20.0, + "factor": 0.1, + "offset": 0, + "decimals": 1 + } + }, + + "minDutyCycle": { + "offset": 6, + "size": 1, + "type": "number", + "name": "Minimum Duty Cycle", + "description": "Minimum motor duty cycle", + "unit": "%", + "minEepromVersion": 3, + "cName": "minimum_duty_cycle", + "raw": { + "min": 0, + "max": 50 + }, + "display": { + "min": 0.0, + "max": 25.0, + "factor": 0.5, + "offset": 0, + "decimals": 1 + } + }, + + "disableStickCalibration": { + "offset": 7, + "size": 1, + "type": "bool", + "name": "Disable Stick Calibration", + "description": "Disable automatic throttle range calibration", + "minEepromVersion": 3, + "cName": "disable_stick_calibration" + }, + + "absoluteVoltageCutoff": { + "offset": 8, + "size": 1, + "type": "number", + "name": "Absolute Voltage Cutoff", + "description": "Fixed voltage cutoff threshold (when low voltage mode = Absolute)", + "unit": "V", + "minEepromVersion": 3, + "cName": "absolute_voltage_cutoff", + "raw": { + "min": 1, + "max": 100 + }, + "display": { + "min": 0.5, + "max": 50.0, + "factor": 0.5, + "offset": 0, + "decimals": 1 + } + }, + + "currentPidP": { + "offset": 9, + "size": 1, + "type": "number", + "name": "Current PID P", + "description": "Proportional gain for current control loop", + "minEepromVersion": 3, + "cName": "current_P", + "raw": { + "min": 0, + "max": 255 + }, + "display": { + "min": 0, + "max": 510, + "factor": 2, + "offset": 0, + "decimals": 0 + } + }, + + "currentPidI": { + "offset": 10, + "size": 1, + "type": "uint8", + "name": "Current PID I", + "description": "Integral gain for current control loop", + "minEepromVersion": 3, + "cName": "current_I", + "raw": { + "min": 0, + "max": 255 + } + }, + + "currentPidD": { + "offset": 11, + "size": 1, + "type": "number", + "name": "Current PID D", + "description": "Derivative gain for current control loop", + "minEepromVersion": 3, + "cName": "current_D", + "raw": { + "min": 0, + "max": 255 + }, + "display": { + "min": 0, + "max": 510, + "factor": 2, + "offset": 0, + "decimals": 0 + } + }, + + "activeBrakePower": { + "offset": 12, + "size": 1, + "type": "uint8", + "name": "Active Brake Power", + "description": "Active braking duty cycle", + "unit": "%", + "minEepromVersion": 3, + "cName": "active_brake_power", + "raw": { + "min": 0, + "max": 10 + } + }, + + "reserved0": { + "offset": 13, + "size": 4, + "type": "reserved", + "name": "Reserved", + "description": "Reserved bytes for future use", + "cName": "reserved_eeprom_3", + "hidden": true + }, + + "directionReversed": { + "offset": 17, + "size": 1, + "type": "bool", + "name": "Direction Reversed", + "description": "Reverse motor rotation direction", + "cName": "dir_reversed" + }, + + "bidirectionalMode": { + "offset": 18, + "size": 1, + "type": "bool", + "name": "Bidirectional Mode", + "description": "Enable 3D mode (forward and reverse)", + "cName": "bi_direction" + }, + + "sineStartup": { + "offset": 19, + "size": 1, + "type": "bool", + "name": "Sinusoidal Startup", + "description": "Use sinusoidal commutation during startup", + "cName": "use_sine_start" + }, + + "complementaryPwm": { + "offset": 20, + "size": 1, + "type": "bool", + "name": "Complementary PWM", + "description": "Enable complementary PWM mode", + "cName": "comp_pwm" + }, + + "variablePwmFreq": { + "offset": 21, + "size": 1, + "type": "enum", + "name": "Variable PWM Frequency", + "description": "PWM frequency mode", + "cName": "variable_pwm", + "versions": { + "default": { + "values": [ + { "raw": 0, "name": "Off", "description": "Fixed PWM frequency" }, + { "raw": 1, "name": "On", "description": "Variable PWM frequency" } + ] + }, + "firmware:2.18+": { + "values": [ + { "raw": 0, "name": "Fixed", "description": "Fixed PWM frequency" }, + { "raw": 1, "name": "Variable", "description": "Variable PWM frequency" }, + { "raw": 2, "name": "By RPM", "description": "PWM frequency varies with RPM" } + ] + } + } + }, + + "stuckRotorProtection": { + "offset": 22, + "size": 1, + "type": "bool", + "name": "Stuck Rotor Protection", + "description": "Stop motor if rotor is stuck", + "cName": "stuck_rotor_protection" + }, + + "timingAdvance": { + "offset": 23, + "size": 1, + "type": "number", + "name": "Timing Advance", + "description": "Motor commutation timing advance angle (0.9375° per internal unit)", + "unit": "°", + "cName": "advance_level", + "versions": { + "default": { + "raw": { + "min": 0, + "max": 3 + }, + "display": { + "min": 0, + "max": 22.5, + "factor": 7.5, + "offset": 0, + "decimals": 1 + }, + "notes": "Old format: raw 0-3 maps to 0°, 7.5°, 15°, 22.5°" + }, + "eeprom:3+": { + "raw": { + "min": 10, + "max": 42 + }, + "display": { + "min": 0, + "max": 30, + "factor": 0.9375, + "offset": -9.375, + "decimals": 1 + }, + "notes": "New format: raw 10-42 stored in EEPROM, internally converted to 0-32, then multiplied by 0.9375 for degrees" + } + } + }, + + "pwmFrequency": { + "offset": 24, + "size": 1, + "type": "uint8", + "name": "PWM Frequency", + "description": "Base PWM switching frequency", + "unit": "kHz", + "cName": "pwm_frequency", + "versions": { + "default": { + "raw": { + "min": 8, + "max": 48 + } + }, + "eeprom:3+": { + "raw": { + "min": 8, + "max": 144 + } + } + } + }, + + "startupPower": { + "offset": 25, + "size": 1, + "type": "uint8", + "name": "Startup Power", + "description": "Power level during motor startup", + "unit": "%", + "cName": "startup_power", + "raw": { + "min": 50, + "max": 150 + } + }, + + "motorKv": { + "offset": 26, + "size": 1, + "type": "number", + "name": "Motor KV", + "description": "Motor KV rating (RPM per volt)", + "cName": "motor_kv", + "raw": { + "min": 0, + "max": 255 + }, + "display": { + "min": 20, + "max": 10220, + "factor": 40, + "offset": 20, + "decimals": 0 + } + }, + + "motorPoles": { + "offset": 27, + "size": 1, + "type": "uint8", + "name": "Motor Poles", + "description": "Number of motor magnetic poles", + "cName": "motor_poles", + "raw": { + "min": 2, + "max": 36 + } + }, + + "brakeOnStop": { + "offset": 28, + "size": 1, + "type": "enum", + "name": "Brake on Stop", + "description": "Braking behavior when throttle is at minimum", + "cName": "brake_on_stop", + "values": [ + { "raw": 0, "name": "Off", "description": "No braking when stopped" }, + { "raw": 1, "name": "Brake", "description": "Apply brake when throttle is at minimum" }, + { "raw": 2, "name": "Active Brake", "description": "Active braking mode (requires arming)" } + ] + }, + + "stallProtection": { + "offset": 29, + "size": 1, + "type": "bool", + "name": "Stall Protection", + "description": "Anti-stall protection during low-speed operation", + "cName": "stall_protection" + }, + + "beepVolume": { + "offset": 30, + "size": 1, + "type": "uint8", + "name": "Beep Volume", + "description": "Startup beep volume level", + "cName": "beep_volume", + "raw": { + "min": 0, + "max": 11 + } + }, + + "telemetry30ms": { + "offset": 31, + "size": 1, + "type": "bool", + "name": "30ms Interval Telemetry", + "description": "Use 30ms telemetry interval instead of default", + "cName": "telemetry_on_interval" + }, + + "servoLowThreshold": { + "offset": 32, + "size": 1, + "type": "number", + "name": "Servo Low Threshold", + "description": "Minimum servo pulse width for zero throttle", + "unit": "us", + "cName": "servo.low_threshold", + "raw": { + "min": 0, + "max": 250 + }, + "display": { + "min": 750, + "max": 1250, + "factor": 2, + "offset": 750, + "decimals": 0 + } + }, + + "servoHighThreshold": { + "offset": 33, + "size": 1, + "type": "number", + "name": "Servo High Threshold", + "description": "Maximum servo pulse width for full throttle", + "unit": "us", + "cName": "servo.high_threshold", + "raw": { + "min": 0, + "max": 250 + }, + "display": { + "min": 1750, + "max": 2250, + "factor": 2, + "offset": 1750, + "decimals": 0 + } + }, + + "servoNeutral": { + "offset": 34, + "size": 1, + "type": "number", + "name": "Servo Neutral", + "description": "Servo pulse width for neutral position (3D mode)", + "unit": "us", + "cName": "servo.neutral", + "raw": { + "min": 0, + "max": 255 + }, + "display": { + "min": 1374, + "max": 1629, + "factor": 1, + "offset": 1374, + "decimals": 0 + } + }, + + "servoDeadband": { + "offset": 35, + "size": 1, + "type": "uint8", + "name": "Servo Deadband", + "description": "Deadband around neutral position", + "cName": "servo.dead_band", + "raw": { + "min": 0, + "max": 100 + } + }, + + "lowVoltageCutoff": { + "offset": 36, + "size": 1, + "type": "enum", + "name": "Low Voltage Cutoff", + "description": "Low voltage protection mode", + "cName": "low_voltage_cut_off", + "versions": { + "default": { + "values": [ + { "raw": 0, "name": "Off", "description": "No low voltage protection" }, + { "raw": 1, "name": "On", "description": "Cell-based voltage cutoff" } + ] + }, + "firmware:2.19+": { + "values": [ + { "raw": 0, "name": "Off", "description": "No low voltage protection" }, + { "raw": 1, "name": "Cell Based", "description": "Per-cell voltage threshold" }, + { "raw": 2, "name": "Absolute", "description": "Fixed voltage threshold" } + ] + } + } + }, + + "lowVoltageThreshold": { + "offset": 37, + "size": 1, + "type": "number", + "name": "Low Voltage Threshold", + "description": "Per-cell voltage cutoff threshold", + "unit": "V", + "cName": "low_cell_volt_cutoff", + "raw": { + "min": 0, + "max": 100 + }, + "display": { + "min": 2.50, + "max": 3.50, + "factor": 0.01, + "offset": 2.50, + "decimals": 2 + } + }, + + "rcCarReversing": { + "offset": 38, + "size": 1, + "type": "bool", + "name": "RC Car Reversing", + "description": "Enable RC car style brake-then-reverse behavior", + "cName": "rc_car_reverse" + }, + + "hallSensors": { + "offset": 39, + "size": 1, + "type": "bool", + "name": "Hall Sensors", + "description": "Use hall sensor feedback for commutation", + "cName": "use_hall_sensors" + }, + + "sineModeRange": { + "offset": 40, + "size": 1, + "type": "uint8", + "name": "Sine Mode Range", + "description": "Throttle range for sine mode operation", + "unit": "%", + "cName": "sine_mode_changeover_thottle_level", + "raw": { + "min": 5, + "max": 25 + } + }, + + "dragBrakeStrength": { + "offset": 41, + "size": 1, + "type": "uint8", + "name": "Drag Brake Strength", + "description": "Brake strength when throttle released", + "cName": "drag_brake_strength", + "raw": { + "min": 1, + "max": 10 + } + }, + + "runningBrakeLevel": { + "offset": 42, + "size": 1, + "type": "uint8", + "name": "Running Brake Level", + "description": "Brake level during deceleration", + "cName": "driving_brake_strength", + "raw": { + "min": 1, + "max": 10 + } + }, + + "temperatureLimit": { + "offset": 43, + "size": 1, + "type": "uint8", + "name": "Temperature Limit", + "description": "Maximum ESC temperature before throttling", + "unit": "°C", + "cName": "limits.temperature", + "raw": { + "min": 70, + "max": 141 + }, + "disabledValue": { + "raw": 141, + "display": "Disabled" + } + }, + + "currentLimit": { + "offset": 44, + "size": 1, + "type": "number", + "name": "Current Limit", + "description": "Maximum motor current", + "unit": "A", + "cName": "limits.current", + "raw": { + "min": 0, + "max": 101 + }, + "display": { + "min": 0, + "max": 202, + "factor": 2, + "offset": 0, + "decimals": 0 + }, + "disabledValue": { + "raw": 101, + "displayValue": 202, + "display": "Disabled" + } + }, + + "sineModePower": { + "offset": 45, + "size": 1, + "type": "uint8", + "name": "Sine Mode Power", + "description": "Power level during sine mode startup", + "cName": "sine_mode_power", + "raw": { + "min": 1, + "max": 10 + } + }, + + "inputType": { + "offset": 46, + "size": 1, + "type": "enum", + "name": "Input Type", + "description": "ESC input protocol", + "cName": "input_type", + "values": [ + { "raw": 0, "name": "Auto", "description": "Automatically detect input protocol" }, + { "raw": 1, "name": "DShot", "description": "DShot digital protocol" }, + { "raw": 2, "name": "Servo", "description": "Standard PWM servo signal" }, + { "raw": 3, "name": "Serial", "description": "Serial communication" }, + { "raw": 4, "name": "EDT ARM", "description": "EDT arming protocol" } + ] + }, + + "autoTiming": { + "offset": 47, + "size": 1, + "type": "bool", + "name": "Auto Timing", + "description": "Automatically adjust commutation timing", + "minFirmwareVersion": "2.16", + "cName": "auto_advance" + }, + + "startupMelody": { + "offset": 48, + "size": 128, + "type": "rtttl", + "name": "Startup Melody", + "description": "RTTTL format startup tune", + "cName": "tune" + }, + + "canNode": { + "offset": 176, + "size": 1, + "type": "uint8", + "name": "CAN Node ID", + "description": "CAN bus node identifier", + "minEepromVersion": 4, + "cName": "can.can_node", + "raw": { + "min": 0, + "max": 127 + } + }, + + "canEscIndex": { + "offset": 177, + "size": 1, + "type": "uint8", + "name": "CAN ESC Index", + "description": "ESC index for CAN communication", + "minEepromVersion": 4, + "cName": "can.esc_index", + "raw": { + "min": 0, + "max": 32 + } + }, + + "canRequireArming": { + "offset": 178, + "size": 1, + "type": "bool", + "name": "CAN Require Arming", + "description": "Require arming command before motor start", + "minEepromVersion": 4, + "cName": "can.require_arming" + }, + + "canTelemRate": { + "offset": 179, + "size": 1, + "type": "uint8", + "name": "CAN Telemetry Rate", + "description": "CAN telemetry transmission rate", + "unit": "Hz", + "minEepromVersion": 4, + "cName": "can.telem_rate", + "raw": { + "min": 0, + "max": 200 + } + }, + + "canRequireZeroThrottle": { + "offset": 180, + "size": 1, + "type": "bool", + "name": "CAN Require Zero Throttle", + "description": "Require zero throttle before arming", + "minEepromVersion": 4, + "cName": "can.require_zero_throttle" + }, + + "canFilterHz": { + "offset": 181, + "size": 1, + "type": "uint8", + "name": "CAN Filter Hz", + "description": "CAN input filter frequency", + "unit": "Hz", + "minEepromVersion": 4, + "cName": "can.filter_hz", + "raw": { + "min": 0, + "max": 100 + } + }, + + "canDebugRate": { + "offset": 182, + "size": 1, + "type": "uint8", + "name": "CAN Debug Rate", + "description": "CAN debug message rate", + "unit": "Hz", + "minEepromVersion": 4, + "cName": "can.debug_rate", + "raw": { + "min": 0, + "max": 200 + } + }, + + "canTermEnable": { + "offset": 183, + "size": 1, + "type": "bool", + "name": "CAN Termination Enable", + "description": "Enable CAN bus termination resistor", + "minEepromVersion": 4, + "cName": "can.term_enable" + }, + + "canReserved": { + "offset": 184, + "size": 8, + "type": "uint8", + "name": "CAN Reserved", + "description": "Reserved bytes for future CAN settings", + "minEepromVersion": 4, + "cName": "can.reserved" + } + }, + + "groups": { + "info": { + "name": "ESC Information", + "fields": ["bootByte", "eepromVersion", "bootloaderVersion", "firmwareMajor", "firmwareMinor"], + "description": "Read-only ESC identification and version information" + }, + "motor": { + "name": "Motor Settings", + "fields": ["directionReversed", "bidirectionalMode", "motorKv", "motorPoles", "timingAdvance", "autoTiming", "startupPower", "pwmFrequency", "variablePwmFreq", "complementaryPwm", "stuckRotorProtection", "stallProtection", "hallSensors"], + "description": "Motor configuration and commutation settings" + }, + "extended": { + "name": "Extended Settings", + "fields": ["maxRampSpeed", "minDutyCycle", "disableStickCalibration"], + "description": "Extended motor control settings (EEPROM v3+)", + "minEepromVersion": 3 + }, + "limits": { + "name": "Limits", + "fields": ["temperatureLimit", "currentLimit", "lowVoltageCutoff", "lowVoltageThreshold", "absoluteVoltageCutoff"], + "description": "Protection limits and cutoffs" + }, + "currentControl": { + "name": "Current Control", + "fields": ["currentPidP", "currentPidI", "currentPidD"], + "description": "Current control PID tuning (EEPROM v3+)", + "minEepromVersion": 3 + }, + "sineStartup": { + "name": "Sinusoidal Startup", + "fields": ["sineStartup", "sineModeRange", "sineModePower"], + "description": "Sinusoidal startup mode settings" + }, + "brake": { + "name": "Brake", + "fields": ["brakeOnStop", "rcCarReversing", "dragBrakeStrength", "runningBrakeLevel", "activeBrakePower"], + "description": "Braking behavior settings" + }, + "servo": { + "name": "Servo Settings", + "fields": ["servoLowThreshold", "servoHighThreshold", "servoNeutral", "servoDeadband"], + "description": "PWM servo input calibration" + }, + "misc": { + "name": "Miscellaneous", + "fields": ["beepVolume", "telemetry30ms", "inputType", "startupMelody"], + "description": "Other ESC settings" + }, + "can": { + "name": "CAN Bus", + "fields": ["canNode", "canEscIndex", "canRequireArming", "canTelemRate", "canRequireZeroThrottle", "canFilterHz", "canDebugRate", "canTermEnable"], + "description": "CAN bus communication settings (EEPROM v4+)", + "minEepromVersion": 4 + } + }, + + "meta": { + "maintainer": "AM32 Project", + "repository": "https://github.com/am32-firmware/AM32", + "license": "GPL-3.0", + "lastUpdated": "2026-01-05", + "consumers": [ + { + "name": "AM32 Firmware", + "language": "C", + "usage": "Source of truth for EEPROM struct layout" + }, + { + "name": "AM32 Configurator", + "language": "TypeScript/Vue", + "usage": "Web-based ESC configuration tool" + }, + { + "name": "QGroundControl", + "language": "C++/QML", + "usage": "Ground control station ESC settings" + } + ] + } +} diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 42f56f33bd3..0c242423012 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2,6 +2,7 @@ #include "Actuators.h" #include "BatteryFactGroupListModel.h" #include "EscStatusFactGroupListModel.h" +#include "AM32EepromFactGroupListModel.h" #include "TerrainFactGroup.h" #include "VehicleClockFactGroup.h" #include "VehicleDistanceSensorFactGroup.h" @@ -311,6 +312,7 @@ void Vehicle::_commonInit(LinkInterface* link) _terrainFactGroup = new TerrainFactGroup(this); _batteryFactGroupListModel = new BatteryFactGroupListModel(this); _escStatusFactGroupListModel = new EscStatusFactGroupListModel(this); + _am32EepromFactGroupListModel = new AM32EepromFactGroupListModel(this); if (!_offlineEditingVehicle) { _terrainProtocolHandler = new TerrainProtocolHandler(this, _terrainFactGroup, this); @@ -407,6 +409,8 @@ FactGroup* Vehicle::rpmFactGroup() { return _rpmFactGroup; } QmlObjectListModel* Vehicle::batteries() { return _batteryFactGroupListModel; } QmlObjectListModel* Vehicle::escs() { return _escStatusFactGroupListModel; } +QmlObjectListModel* Vehicle::am32eeproms() { return _am32EepromFactGroupListModel; } + void Vehicle::_deleteCameraManager() { @@ -570,6 +574,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes // Handle creation of dynamic fact group lists _batteryFactGroupListModel->handleMessageForFactGroupCreation(this, message); _escStatusFactGroupListModel->handleMessageForFactGroupCreation(this, message); + _am32EepromFactGroupListModel->handleMessageForFactGroupCreation(this, message); // Let the fact groups take a whack at the mavlink traffic for (FactGroup* factGroup : factGroups()) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index e01404ec477..5fa25d09980 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -24,6 +24,7 @@ class Actuators; class AutoPilotPlugin; class BatteryFactGroupListModel; class EscStatusFactGroupListModel; +class AM32EepromFactGroupListModel; class GimbalController; class TerrainFactGroup; class VehicleClockFactGroup; @@ -257,6 +258,7 @@ class Vehicle : public VehicleFactGroup // Dynamic FactGroupListModel properties Q_PROPERTY(QmlObjectListModel* batteries READ batteries CONSTANT) Q_PROPERTY(QmlObjectListModel* escs READ escs CONSTANT) + Q_PROPERTY(QmlObjectListModel* am32eeproms READ am32eeproms CONSTANT) Q_PROPERTY(int firmwareMajorVersion READ firmwareMajorVersion NOTIFY firmwareVersionChanged) Q_PROPERTY(int firmwareMinorVersion READ firmwareMinorVersion NOTIFY firmwareVersionChanged) @@ -576,6 +578,7 @@ class Vehicle : public VehicleFactGroup QmlObjectListModel* batteries (); QmlObjectListModel* escs (); + QmlObjectListModel* am32eeproms (); MissionManager* missionManager () { return _missionManager; } GeoFenceManager* geoFenceManager () { return _geoFenceManager; } @@ -1253,6 +1256,7 @@ void _activeVehicleChanged (Vehicle* newActiveVehicle); // Dynamic FactGroups BatteryFactGroupListModel* _batteryFactGroupListModel = nullptr; EscStatusFactGroupListModel* _escStatusFactGroupListModel = nullptr; + AM32EepromFactGroupListModel* _am32EepromFactGroupListModel = nullptr; TerrainProtocolHandler* _terrainProtocolHandler = nullptr;