From 500892893a9c2b5e3d38319ceef4a1f31e0cae18 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 22 Jun 2016 20:41:05 -0700 Subject: [PATCH] Rover fixes --- .../APM/APMFlightModesComponent.qml | 9 ++++--- .../APM/APMFlightModesComponentController.cc | 12 ++++++---- .../APM/APMFlightModesComponentController.h | 4 ++++ .../APM/APMFlightModesComponentSummary.qml | 14 ++++++----- .../APM/APMSensorsComponent.cc | 24 +++++++++++++------ .../APM/APMParameterMetaData.cc | 2 +- src/Vehicle/Vehicle.cc | 5 ++++ src/Vehicle/Vehicle.h | 2 ++ 8 files changed, 51 insertions(+), 21 deletions(-) diff --git a/src/AutoPilotPlugins/APM/APMFlightModesComponent.qml b/src/AutoPilotPlugins/APM/APMFlightModesComponent.qml index cec7f22c6..f958c3c23 100644 --- a/src/AutoPilotPlugins/APM/APMFlightModesComponent.qml +++ b/src/AutoPilotPlugins/APM/APMFlightModesComponent.qml @@ -22,13 +22,16 @@ QGCView { id: rootQGCView viewPanel: panel + readonly property string _modeChannelParam: controller.modeChannelParam + readonly property string _modeParamPrefix: controller.modeParamPrefix + property real _margins: ScreenTools.defaultFontPixelHeight property bool _channel7OptionsAvailable: controller.parameterExists(-1, "CH7_OPT") // Not available in all firmware types property bool _channel9OptionsAvailable: controller.parameterExists(-1, "CH9_OPT") // Not available in all firmware types property int _channelOptionCount: _channel7OptionsAvailable ? (_channel9OptionsAvailable ? 6 : 2) : 0 property Fact _nullFact - property bool _fltmodeChExists: controller.parameterExists(-1, "FLTMODE_CH") - property Fact _fltmodeCh: _fltmodeChExists ? controller.getParameterFact(-1, "FLTMODE_CH") : _nullFact + property bool _fltmodeChExists: controller.parameterExists(-1, _modeChannelParam) + property Fact _fltmodeCh: _fltmodeChExists ? controller.getParameterFact(-1, _modeChannelParam) : _nullFact QGCPalette { id: qgcPal; colorGroupEnabled: panel.enabled } @@ -114,7 +117,7 @@ QGCView { FactComboBox { id: modeCombo width: ScreenTools.defaultFontPixelWidth * 15 - fact: controller.getParameterFact(-1, "FLTMODE" + index) + fact: controller.getParameterFact(-1, _modeParamPrefix + index) indexModel: false } diff --git a/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc b/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc index d3cfe58a7..970d9fced 100644 --- a/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc +++ b/src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc @@ -19,9 +19,13 @@ APMFlightModesComponentController::APMFlightModesComponentController(void) : _activeFlightMode(0) , _channelCount(Vehicle::cMaxRcChannels) { + _modeParamPrefix = _vehicle->rover() ? "MODE" : "FLTMODE"; + _modeChannelParam = _vehicle->rover() ? "MODE_CH" : "FLTMODE_CH"; + QStringList usedParams; - usedParams << QStringLiteral("FLTMODE1") << QStringLiteral("FLTMODE2") << QStringLiteral("FLTMODE3") - << QStringLiteral("FLTMODE4") << QStringLiteral("FLTMODE5") << QStringLiteral("FLTMODE6"); + for (int i=1; i<7; i++) { + usedParams << QStringLiteral("%1%2").arg(_modeParamPrefix).arg(i); + } if (!_allParametersExists(FactSystem::defaultComponentId, usedParams)) { return; } @@ -36,8 +40,8 @@ void APMFlightModesComponentController::_rcChannelsChanged(int channelCount, int { int flightModeChannel = 4; - if (parameterExists(FactSystem::defaultComponentId, QStringLiteral("FLTMODE_CH"))) { - flightModeChannel = getParameterFact(FactSystem::defaultComponentId, QStringLiteral("FLTMODE_CH"))->rawValue().toInt() - 1; + if (parameterExists(FactSystem::defaultComponentId, _modeChannelParam)) { + flightModeChannel = getParameterFact(FactSystem::defaultComponentId, _modeChannelParam)->rawValue().toInt() - 1; } if (flightModeChannel >= channelCount) { diff --git a/src/AutoPilotPlugins/APM/APMFlightModesComponentController.h b/src/AutoPilotPlugins/APM/APMFlightModesComponentController.h index 42e9ebf77..d193f7202 100644 --- a/src/AutoPilotPlugins/APM/APMFlightModesComponentController.h +++ b/src/AutoPilotPlugins/APM/APMFlightModesComponentController.h @@ -29,6 +29,8 @@ class APMFlightModesComponentController : public FactPanelController public: APMFlightModesComponentController(void); + Q_PROPERTY(QString modeParamPrefix MEMBER _modeParamPrefix CONSTANT) + Q_PROPERTY(QString modeChannelParam MEMBER _modeChannelParam CONSTANT) Q_PROPERTY(int activeFlightMode READ activeFlightMode NOTIFY activeFlightModeChanged) Q_PROPERTY(int channelCount MEMBER _channelCount CONSTANT) Q_PROPERTY(QVariantList channelOptionEnabled READ channelOptionEnabled NOTIFY channelOptionEnabledChanged) @@ -44,6 +46,8 @@ private slots: void _rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels]); private: + QString _modeParamPrefix; + QString _modeChannelParam; int _activeFlightMode; int _channelCount; QVariantList _rgChannelOptionEnabled; diff --git a/src/AutoPilotPlugins/APM/APMFlightModesComponentSummary.qml b/src/AutoPilotPlugins/APM/APMFlightModesComponentSummary.qml index 645f0590b..f0237a2e1 100644 --- a/src/AutoPilotPlugins/APM/APMFlightModesComponentSummary.qml +++ b/src/AutoPilotPlugins/APM/APMFlightModesComponentSummary.qml @@ -14,12 +14,14 @@ FactPanel { QGCPalette { id: qgcPal; colorGroupEnabled: enabled } FactPanelController { id: controller; factPanel: panel } - property Fact flightMode1: controller.getParameterFact(-1, "FLTMODE1") - property Fact flightMode2: controller.getParameterFact(-1, "FLTMODE2") - property Fact flightMode3: controller.getParameterFact(-1, "FLTMODE3") - property Fact flightMode4: controller.getParameterFact(-1, "FLTMODE4") - property Fact flightMode5: controller.getParameterFact(-1, "FLTMODE5") - property Fact flightMode6: controller.getParameterFact(-1, "FLTMODE6") + property var _vehicle: controller.vehicle + + property Fact flightMode1: controller.getParameterFact(-1, _vehicle.rover ? "MODE1" : "FLTMODE1") + property Fact flightMode2: controller.getParameterFact(-1, _vehicle.rover ? "MODE2" : "FLTMODE2") + property Fact flightMode3: controller.getParameterFact(-1, _vehicle.rover ? "MODE3" : "FLTMODE3") + property Fact flightMode4: controller.getParameterFact(-1, _vehicle.rover ? "MODE4" : "FLTMODE4") + property Fact flightMode5: controller.getParameterFact(-1, _vehicle.rover ? "MODE5" : "FLTMODE5") + property Fact flightMode6: controller.getParameterFact(-1, _vehicle.rover ? "MODE6" : "FLTMODE6") Column { anchors.fill: parent diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponent.cc b/src/AutoPilotPlugins/APM/APMSensorsComponent.cc index eb4dea092..d57c2f3df 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponent.cc +++ b/src/AutoPilotPlugins/APM/APMSensorsComponent.cc @@ -117,13 +117,23 @@ bool APMSensorsComponent::compassSetupNeeded(void) const bool APMSensorsComponent::accelSetupNeeded(void) const { - QStringList offsets; - - offsets << QStringLiteral("INS_ACCOFFS_X") << QStringLiteral("INS_ACCOFFS_Y") << QStringLiteral("INS_ACCOFFS_Z"); - - foreach(const QString& offset, offsets) { - if (_autopilot->getParameterFact(FactSystem::defaultComponentId, offset)->rawValue().toFloat() == 0.0f) { - return true; + const size_t cAccel = 3; + const size_t cOffset = 3; + QStringList rgUse; + QStringList rgOffsets[cAccel]; + + rgUse << QStringLiteral("INS_USE") << QStringLiteral("INS_USE2") << QStringLiteral("INS_USE3"); + rgOffsets[0] << QStringLiteral("INS_ACCOFFS_X") << QStringLiteral("INS_ACCOFFS_Y") << QStringLiteral("INS_ACCOFFS_Z"); + rgOffsets[1] << QStringLiteral("INS_ACC2OFFS_X") << QStringLiteral("INS_ACC2OFFS_Y") << QStringLiteral("INS_ACC2OFFS_Z"); + rgOffsets[2] << QStringLiteral("INS_ACC3OFFS_X") << QStringLiteral("INS_ACC3OFFS_Y") << QStringLiteral("INS_ACC3OFFS_Z"); + + for (size_t i=0; igetParameterFact(FactSystem::defaultComponentId, rgUse[i])->rawValue().toInt() != 0) { + for (size_t j=0; jgetParameterFact(FactSystem::defaultComponentId, rgOffsets[i][j])->rawValue().toFloat() == 0.0f) { + return true; + } + } } } diff --git a/src/FirmwarePlugin/APM/APMParameterMetaData.cc b/src/FirmwarePlugin/APM/APMParameterMetaData.cc index ee09f2462..443442cbf 100644 --- a/src/FirmwarePlugin/APM/APMParameterMetaData.cc +++ b/src/FirmwarePlugin/APM/APMParameterMetaData.cc @@ -88,7 +88,7 @@ QString APMParameterMetaData::mavTypeToString(MAV_TYPE vehicleTypeEnum) break; case MAV_TYPE_GROUND_ROVER: case MAV_TYPE_SURFACE_BOAT: - vehicleName = "ArduRover"; + vehicleName = "APMrover2"; break; case MAV_TYPE_SUBMARINE: vehicleName = "ArduSub"; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 821e29067..5463cea82 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1405,6 +1405,11 @@ bool Vehicle::fixedWing(void) const return vehicleType() == MAV_TYPE_FIXED_WING; } +bool Vehicle::rover(void) const +{ + return vehicleType() == MAV_TYPE_GROUND_ROVER; +} + bool Vehicle::multiRotor(void) const { switch (vehicleType()) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 3897c750a..2c7c4ec31 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -273,6 +273,7 @@ public: Q_PROPERTY(bool fixedWing READ fixedWing CONSTANT) Q_PROPERTY(bool multiRotor READ multiRotor CONSTANT) Q_PROPERTY(bool vtol READ vtol CONSTANT) + Q_PROPERTY(bool rover READ rover CONSTANT) Q_PROPERTY(bool autoDisconnect MEMBER _autoDisconnect NOTIFY autoDisconnectChanged) Q_PROPERTY(QString prearmError READ prearmError WRITE setPrearmError NOTIFY prearmErrorChanged) @@ -443,6 +444,7 @@ public: bool fixedWing(void) const; bool multiRotor(void) const; bool vtol(void) const; + bool rover(void) const; void setFlying(bool flying); void setGuidedMode(bool guidedMode); -- 2.22.0