From 74e57ec4b18e1d2e83d84ce608890ef14600807f Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 22 Nov 2015 18:35:37 -0800 Subject: [PATCH] Add default value support Also friendly editor for MAV_CMD_NAV_ROI and MAV_CMD_DO_SET_SERVO --- src/MissionManager/MavCmdInfo.json | 59 ++++++++++++++++++++++++++++-- src/MissionManager/MissionItem.cc | 58 ++++++++++++++++------------- src/MissionManager/MissionItem.h | 8 +++- 3 files changed, 95 insertions(+), 30 deletions(-) diff --git a/src/MissionManager/MavCmdInfo.json b/src/MissionManager/MavCmdInfo.json index cf621ef24..3bf8c6438 100644 --- a/src/MissionManager/MavCmdInfo.json +++ b/src/MissionManager/MavCmdInfo.json @@ -21,6 +21,7 @@ "param1": { "label": "Hold:", "units": "seconds", + "default": 0, "decimalPlaces": 0 } }, @@ -34,6 +35,7 @@ "param3": { "label": "Radius:", "units": "meters", + "default": 10.0, "decimalPlaces": 2 } }, @@ -46,11 +48,13 @@ "friendlyEdit": true, "param1": { "label": "Turns:", + "default": 1, "decimalPlaces": 0 }, "param3": { "label": "Radius:", "units": "meters", + "default": 10.0, "decimalPlaces": 2 } }, @@ -64,11 +68,13 @@ "param1": { "label": "Hold:", "units": "seconds", + "default": 30, "decimalPlaces": 0 }, "param3": { "label": "Radius:", "units": "meters", + "default": 10.0, "decimalPlaces": 2 } }, @@ -89,11 +95,13 @@ "param1": { "label": "Abort Alt:", "units": "meters", + "default": 25.0, "decimalPlaces": 3 }, "param4": { "label": "Heading:", "units": "degrees", + "default": 0.0, "decimalPlaces": 2 } }, @@ -107,11 +115,13 @@ "param1": { "label": "Pitch:", "units": "degrees", + "default": 0.26179939, "decimalPlaces": 2 }, "param4": { "label": "Heading:", "units": "degrees", + "default": 0.0, "decimalPlaces": 2 } }, @@ -120,7 +130,31 @@ { "id": 25, "rawName": "MAV_CMD_NAV_FOLLOW", "friendlyName": "MAV_CMD_NAV_FOLLOW" }, { "id": 30, "rawName": "MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT", "friendlyName": "MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT" }, { "id": 31, "rawName": "MAV_CMD_NAV_LOITER_TO_ALT" }, - { "id": 80, "rawName": "MAV_CMD_NAV_ROI", "friendlyName": "MAV_CMD_NAV_ROI" }, + { + "id": 80, + "rawName": "MAV_CMD_NAV_ROI", + "friendlyName": "Region of interest", + "description": "Sets the region of interest for cameras.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "param1": { + "label": "Mode:", + "enumStrings": "None,Next waypoint,Mission item,Location,ROI item", + "enumValues": "0,1,2,3,4", + "default": 3, + "decimalPlaces": 0 + }, + "param2": { + "label": "Mission Index:", + "default": 0, + "decimalPlaces": 0 + }, + "param3": { + "label": "ROI index:", + "default": 0, + "decimalPlaces": 0 + } + }, { "id": 81, "rawName": "MAV_CMD_NAV_PATHPLANNING", "friendlyName": "MAV_CMD_NAV_PATHPLANNING" }, { "id": 82, "rawName": "MAV_CMD_NAV_SPLINE_WAYPOINT", "friendlyName": "MAV_CMD_NAV_SPLINE_WAYPOINT" }, { "id": 83, "rawName": "MAV_CMD_NAV_ALTITUDE_WAIT", "friendlyName": "MAV_CMD_NAV_ALTITUDE_WAIT" }, @@ -134,6 +168,7 @@ "param1": { "label": "Hold:", "units": "seconds", + "default": 30, "decimalPlaces": 0 } }, @@ -149,11 +184,13 @@ "description": "Mission will continue at the specified item.", "friendlyEdit": true, "param1": { - "label": "Seq #:", + "label": "Item #:", + "default": 1, "decimalPlaces": 0 }, "param2": { "label": "Repeat:", + "default": 0, "decimalPlaces": 0 } }, @@ -162,7 +199,23 @@ { "id": 180, "rawName": "MAV_CMD_DO_SET_PARAMETER", "friendlyName": "MAV_CMD_DO_SET_PARAMETER" }, { "id": 181, "rawName": "MAV_CMD_DO_SET_RELAY", "friendlyName": "MAV_CMD_DO_SET_RELAY" }, { "id": 182, "rawName": "MAV_CMD_DO_REPEAT_RELAY", "friendlyName": "MAV_CMD_DO_REPEAT_RELAY" }, - { "id": 183, "rawName": "MAV_CMD_DO_SET_SERVO", "friendlyName": "MAV_CMD_DO_SET_SERVO" }, + { + "id": 183, + "rawName": "MAV_CMD_DO_SET_SERVO", + "friendlyName": "Set servo", + "description": "Set servo to specified PWM value.", + "friendlyEdit": true, + "param1": { + "label": "Servo:", + "default": 1, + "decimalPlaces": 0 + }, + "param2": { + "label": "PWM:", + "default": 1000, + "decimalPlaces": 0 + } + }, { "id": 184, "rawName": "MAV_CMD_DO_REPEAT_SERVO", "friendlyName": "MAV_CMD_DO_REPEAT_SERVO" }, { "id": 185, "rawName": "MAV_CMD_DO_FLIGHTTERMINATION", "friendlyName": "MAV_CMD_DO_FLIGHTTERMINATION" }, { "id": 189, "rawName": "MAV_CMD_DO_LAND_START", "friendlyName": "MAV_CMD_DO_LAND_START" }, diff --git a/src/MissionManager/MissionItem.cc b/src/MissionManager/MissionItem.cc index 543ed6d5a..a8194ee59 100644 --- a/src/MissionManager/MissionItem.cc +++ b/src/MissionManager/MissionItem.cc @@ -47,6 +47,7 @@ FactMetaData* MissionItem::_longitudeMetaData = NULL; FactMetaData* MissionItem::_supportedCommandMetaData = NULL; const QString MissionItem::_decimalPlacesJsonKey (QStringLiteral("decimalPlaces")); +const QString MissionItem::_defaultJsonKey (QStringLiteral("default")); const QString MissionItem::_descriptionJsonKey (QStringLiteral("description")); const QString MissionItem::_friendlyEditJsonKey (QStringLiteral("friendlyEdit")); const QString MissionItem::_friendlyNameJsonKey (QStringLiteral("friendlyName")); @@ -246,8 +247,8 @@ void MissionItem::_connectSignals(void) connect(&_param5Fact, &Fact::valueChanged, this, &MissionItem::_setDirtyFromSignal); connect(&_param6Fact, &Fact::valueChanged, this, &MissionItem::_setDirtyFromSignal); connect(&_param7Fact, &Fact::valueChanged, this, &MissionItem::_setDirtyFromSignal); - connect(this, &MissionItem::commandChanged, this, &MissionItem::_setDirtyFromSignal); - connect(this, &MissionItem::frameChanged, this, &MissionItem::_setDirtyFromSignal); + connect(&_frameFact, &Fact::valueChanged, this, &MissionItem::_setDirtyFromSignal); + connect(&_commandFact, &Fact::valueChanged, this, &MissionItem::_setDirtyFromSignal); connect(this, &MissionItem::sequenceNumberChanged, this, &MissionItem::_setDirtyFromSignal); // Values from these facts must propogate back and forth between the real object storage @@ -264,14 +265,18 @@ void MissionItem::_connectSignals(void) // The following changes may also change friendlyEditAllowed connect(&_autoContinueFact, &Fact::valueChanged, this, &MissionItem::_sendFriendlyEditAllowedChanged); connect(&_commandFact, &Fact::valueChanged, this, &MissionItem::_sendFriendlyEditAllowedChanged); - connect(this, &MissionItem::frameChanged, this, &MissionItem::_sendFriendlyEditAllowedChanged); + connect(&_frameFact, &Fact::valueChanged, this, &MissionItem::_sendFriendlyEditAllowedChanged); // Whenever these properties change the ui model changes as well connect(this, &MissionItem::commandChanged, this, &MissionItem::_sendUiModelChanged); connect(this, &MissionItem::rawEditChanged, this, &MissionItem::_sendUiModelChanged); + // These fact signals must alway signal out through MissionItem signals connect(&_commandFact, &Fact::valueChanged, this, &MissionItem::_sendCommandChanged); connect(&_frameFact, &Fact::valueChanged, this, &MissionItem::_sendFrameChanged); + + // When the command changes we need to set defaults + connect(&_commandFact, &Fact::valueChanged, this, &MissionItem::setDefaultsForCommand); } bool MissionItem::_validateKeyTypes(QJsonObject& jsonObject, const QStringList& keys, const QList& types) @@ -376,8 +381,8 @@ bool MissionItem::_loadMavCmdInfoJson(void) // Validate key types QStringList keys; QList types; - keys << _labelJsonKey << _unitsJsonKey << _decimalPlacesJsonKey; - types << QJsonValue::String << QJsonValue::String<< QJsonValue::Double; + keys << _labelJsonKey << _unitsJsonKey << _defaultJsonKey << _decimalPlacesJsonKey; + types << QJsonValue::String << QJsonValue::String << QJsonValue::Double << QJsonValue::Double; if (!_validateKeyTypes(paramObject, keys, types)) { return false; } @@ -389,7 +394,9 @@ bool MissionItem::_loadMavCmdInfoJson(void) return false; } + _mavCmdInfoMap[mavCmdInfo.command].paramInfoMap[i].param = i; _mavCmdInfoMap[mavCmdInfo.command].paramInfoMap[i].units = paramObject.value(_unitsJsonKey).toString(); + _mavCmdInfoMap[mavCmdInfo.command].paramInfoMap[i].defaultValue = paramObject.value(_defaultJsonKey).toDouble(0.0); _mavCmdInfoMap[mavCmdInfo.command].paramInfoMap[i].decimalPlaces = paramObject.value(_decimalPlacesJsonKey).toInt(FactMetaData::defaultDecimalPlaces); } } @@ -519,6 +526,7 @@ void MissionItem::setCommand(MAV_CMD command) { if ((MAV_CMD)this->command() != command) { _commandFact.setValue(command); + setDefaultsForCommand(); emit commandChanged(this->command()); } } @@ -533,6 +541,7 @@ void MissionItem::setFrame(MAV_FRAME frame) { if (this->frame() != frame) { _frameFact.setValue(frame); + frameChanged(frame); } } @@ -837,7 +846,6 @@ void MissionItem::_syncSupportedCommandToCommand(const QVariant& value) qDebug() << value.toInt(); _syncingSupportedCommandAndCommand = true; _commandFact.setValue(value.toInt()); - qDebug() << _commandFact.value().toInt(); _syncingSupportedCommandAndCommand = false; } } @@ -853,29 +861,29 @@ void MissionItem::_syncCommandToSupportedCommand(const QVariant& value) void MissionItem::setDefaultsForCommand(void) { - switch ((MAV_CMD)command()) { - case MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF: - setParam1(_degreesToRadians(defaultTakeoffPitch).toDouble()); - break; - case MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT: - setParam2(defaultAcceptanceRadius); - break; - case MavlinkQmlSingleton::MAV_CMD_NAV_LOITER_UNLIM: - case MavlinkQmlSingleton::MAV_CMD_NAV_LOITER_TIME: - setParam3(defaultLoiterOrbitRadius); - break; - case MavlinkQmlSingleton::MAV_CMD_NAV_LOITER_TURNS: - setParam1(defaultLoiterTurns); - setParam3(defaultLoiterOrbitRadius); - break; - default: - break; + foreach (ParamInfo_t paramInfo, _mavCmdInfoMap[(MAV_CMD)command()].paramInfoMap) { + double defaultValue = paramInfo.defaultValue; + + switch (paramInfo.param) { + case 1: + _param1Fact.setRawValue(defaultValue); + break; + case 2: + _param2Fact.setRawValue(defaultValue); + break; + case 3: + _param3Fact.setRawValue(defaultValue); + break; + case 4: + _param4Fact.setRawValue(defaultValue); + break; + } } - setParam4(_degreesToRadians(defaultHeading).toDouble()); setParam7(defaultAltitude); - + setAutoContinue(true); setFrame(specifiesCoordinate() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_MISSION); + setRawEdit(false); } void MissionItem::_sendUiModelChanged(void) diff --git a/src/MissionManager/MissionItem.h b/src/MissionManager/MissionItem.h index 9a1589f89..37f6d0fa0 100644 --- a/src/MissionManager/MissionItem.h +++ b/src/MissionManager/MissionItem.h @@ -93,8 +93,6 @@ public: /// are shown next to the part item in the ui. Q_PROPERTY(QmlObjectListModel* childItems READ childItems CONSTANT) - Q_INVOKABLE void setDefaultsForCommand(void); - // Property accesors MavlinkQmlSingleton::Qml_MAV_CMD command(void) const { return (MavlinkQmlSingleton::Qml_MAV_CMD)_commandFact.value().toInt(); }; @@ -172,6 +170,9 @@ public: static const double defaultLoiterOrbitRadius; static const double defaultLoiterTurns; +public slots: + void setDefaultsForCommand(void); + signals: void commandChanged (MavlinkQmlSingleton::Qml_MAV_CMD command); void coordinateChanged (const QGeoCoordinate& coordinate); @@ -210,8 +211,10 @@ private: private: typedef struct { + int param; QString label; QString units; + double defaultValue; int decimalPlaces; } ParamInfo_t; @@ -269,6 +272,7 @@ private: static QMap _mavCmdInfoMap; static const QString _decimalPlacesJsonKey; + static const QString _defaultJsonKey; static const QString _descriptionJsonKey; static const QString _friendlyNameJsonKey; static const QString _friendlyEditJsonKey; -- 2.22.0