Merge pull request #5085 from DonLakeFlyer/Fixes

Fixes

src/FactSystem/FactMetaData.cc

@@ -34,9 +34,10 @@ const qreal FactMetaData::UnitConsts_s::inchesToCentimeters =   2.54;
 
 // Built in translations for all Facts
 const FactMetaData::BuiltInTranslation_s FactMetaData::_rgBuiltInTranslations[] = {
-    { "centi-degrees",  "deg",  FactMetaData::_centiDegreesToDegrees,   FactMetaData::_degreesToCentiDegrees },
-    { "radians",        "deg",  FactMetaData::_radiansToDegrees,        FactMetaData::_degreesToRadians },
-    { "norm",           "%",    FactMetaData::_normToPercent,           FactMetaData::_percentToNorm },
+    { "centi-degrees",  "deg",  FactMetaData::_centiDegreesToDegrees,                   FactMetaData::_degreesToCentiDegrees },
+    { "radians",        "deg",  FactMetaData::_radiansToDegrees,                        FactMetaData::_degreesToRadians },
+    { "gimbal-degrees", "deg",  FactMetaData::_mavlinkGimbalDegreesToUserGimbalDegrees, FactMetaData::_userGimbalDegreesToMavlinkGimbalDegrees },
+    { "norm",           "%",    FactMetaData::_normToPercent,                           FactMetaData::_percentToNorm },
 };
 
 // Translations driven by app settings
@@ -256,7 +257,7 @@ bool FactMetaData::convertAndValidateRaw(const QVariant& rawValue, bool convertO
     case FactMetaData::valueTypeInt32:
         typedValue = QVariant(rawValue.toInt(&convertOk));
         if (!convertOnly && convertOk) {
-            if (rawMin() > typedValue || typedValue > rawMax()) {
+            if (typedValue < rawMin() || typedValue > rawMax()) {
                 errorString = QString("Value must be within %1 and %2").arg(cookedMin().toInt()).arg(cookedMax().toInt());
             }
         }
@@ -267,7 +268,7 @@ bool FactMetaData::convertAndValidateRaw(const QVariant& rawValue, bool convertO
     case FactMetaData::valueTypeUint32:
         typedValue = QVariant(rawValue.toUInt(&convertOk));
         if (!convertOnly && convertOk) {
-            if (rawMin() > typedValue || typedValue > rawMax()) {
+            if (typedValue < rawMin() || typedValue > rawMax()) {
                 errorString = QString("Value must be within %1 and %2").arg(cookedMin().toUInt()).arg(cookedMax().toUInt());
             }
         }
@@ -276,7 +277,7 @@ bool FactMetaData::convertAndValidateRaw(const QVariant& rawValue, bool convertO
     case FactMetaData::valueTypeFloat:
         typedValue = QVariant(rawValue.toFloat(&convertOk));
         if (!convertOnly && convertOk) {
-            if (rawMin() > typedValue || typedValue > rawMax()) {
+            if (typedValue < rawMin() || typedValue > rawMax()) {
                 errorString = QString("Value must be within %1 and %2").arg(cookedMin().toFloat()).arg(cookedMax().toFloat());
             }
         }
@@ -285,7 +286,7 @@ bool FactMetaData::convertAndValidateRaw(const QVariant& rawValue, bool convertO
     case FactMetaData::valueTypeDouble:
         typedValue = QVariant(rawValue.toDouble(&convertOk));
         if (!convertOnly && convertOk) {
-            if (rawMin() > typedValue || typedValue > rawMax()) {
+            if (typedValue < rawMin() || typedValue > rawMax()) {
                 errorString = QString("Value must be within %1 and %2").arg(cookedMin().toDouble()).arg(cookedMax().toDouble());
             }
         }
@@ -455,6 +456,20 @@ QVariant FactMetaData::_degreesToCentiDegrees(const QVariant& degrees)
     return QVariant(qRound(degrees.toReal() * 100.0));
 }
 
+QVariant FactMetaData::_userGimbalDegreesToMavlinkGimbalDegrees(const QVariant& userGimbalDegrees)
+{
+    // User facing gimbal degree values are from 0 (level) to 90 (straight down)
+    // Mavlink gimbal degree values are from 0 (level) to -90 (straight down)
+    return userGimbalDegrees.toDouble() * -1.0;
+}
+
+QVariant FactMetaData::_mavlinkGimbalDegreesToUserGimbalDegrees(const QVariant& mavlinkGimbalDegrees)
+{
+    // User facing gimbal degree values are from 0 (level) to 90 (straight down)
+    // Mavlink gimbal degree values are from 0 (level) to -90 (straight down)
+    return mavlinkGimbalDegrees.toDouble() * -1.0;
+}
+
 QVariant FactMetaData::_metersToFeet(const QVariant& meters)
 {
     return QVariant(meters.toDouble() * 1.0/constants.feetToMeters);
@@ -908,3 +923,29 @@ QMap<QString, FactMetaData*> FactMetaData::createMapFromJsonFile(const QString&
 
     return metaDataMap;
 }
+
+QVariant FactMetaData::cookedMax(void) const
+{
+    // We have to be careful with cooked min/max. Running the raw values through the translator could flip min and max.
+    QVariant cookedMax = _rawTranslator(_rawMax);
+    QVariant cookedMin = _rawTranslator(_rawMin);
+    if (cookedMax < cookedMin) {
+        // We need to flip
+        return cookedMin;
+    } else {
+        return cookedMax;
+    }
+}
+
+QVariant FactMetaData::cookedMin(void) const
+{
+    // We have to be careful with cooked min/max. Running the raw values through the translator could flip min and max.
+    QVariant cookedMax = _rawTranslator(_rawMax);
+    QVariant cookedMin = _rawTranslator(_rawMin);
+    if (cookedMax < cookedMin) {
+        // We need to flip
+        return cookedMax;
+    } else {
+        return cookedMin;
+    }
+}

src/FactSystem/FactMetaData.h

@@ -83,10 +83,10 @@ public:
     QString         group                   (void) const { return _group; }
     QString         longDescription         (void) const { return _longDescription;}
     QVariant        rawMax                  (void) const { return _rawMax; }
-    QVariant        cookedMax               (void) const { return _rawTranslator(_rawMax); }
+    QVariant        cookedMax               (void) const;
     bool            maxIsDefaultForType     (void) const { return _maxIsDefaultForType; }
     QVariant        rawMin                  (void) const { return _rawMin; }
-    QVariant        cookedMin               (void) const { return _rawTranslator(_rawMin); }
+    QVariant        cookedMin               (void) const;
     bool            minIsDefaultForType     (void) const { return _minIsDefaultForType; }
     QString         name                    (void) const { return _name; }
     QString         shortDescription        (void) const { return _shortDescription; }
@@ -155,6 +155,8 @@ private:
     static QVariant _radiansToDegrees(const QVariant& radians);
     static QVariant _centiDegreesToDegrees(const QVariant& centiDegrees);
     static QVariant _degreesToCentiDegrees(const QVariant& degrees);
+    static QVariant _userGimbalDegreesToMavlinkGimbalDegrees(const QVariant& userGimbalDegrees);
+    static QVariant _mavlinkGimbalDegreesToUserGimbalDegrees(const QVariant& mavlinkGimbalDegrees);
     static QVariant _metersToFeet(const QVariant& meters);
     static QVariant _feetToMeters(const QVariant& feet);
     static QVariant _squareMetersToSquareKilometers(const QVariant& squareMeters);

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc

@@ -139,7 +139,7 @@ void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle)
 
 void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
 {
-    if (!_armVehicle(vehicle)) {
+    if (!_armVehicleAndValidate(vehicle)) {
         qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm."));
         return;
     }

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -435,7 +435,7 @@ bool FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle)
     return vehicle->multiRotor() ? false : true;
 }
 
-bool FirmwarePlugin::_armVehicle(Vehicle* vehicle)
+bool FirmwarePlugin::_armVehicleAndValidate(Vehicle* vehicle)
 {
     if (!vehicle->armed()) {
         vehicle->setArmed(true);

src/FirmwarePlugin/FirmwarePlugin.h

@@ -303,7 +303,7 @@ public:
 protected:
     // Arms the vehicle, waiting for the arm state to change.
     // @return: true - vehicle armed, false - vehicle failed to arm
-    bool _armVehicle(Vehicle* vehicle);
+    bool _armVehicleAndValidate(Vehicle* vehicle);
 
 private:
     QVariantList _toolBarIndicatorList;

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -352,6 +352,29 @@ void PX4FirmwarePlugin::guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate&
                             centerCoord.isValid() ? centerCoord.altitude()  : NAN);
 }
 
+void PX4FirmwarePlugin::_mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle)
+{
+    Q_UNUSED(vehicleId);
+    Q_UNUSED(component);
+    Q_UNUSED(noReponseFromVehicle);
+
+    Vehicle* vehicle = dynamic_cast<Vehicle*>(sender());
+    if (!vehicle) {
+        qWarning() << "Dynamic cast failed!";
+        return;
+    }
+
+    if (command == MAV_CMD_NAV_TAKEOFF && result == MAV_RESULT_ACCEPTED) {
+        // Now that we are in takeoff mode we can arm the vehicle which will cause it to takeoff.
+        // We specifically don't retry arming if it fails. This way we don't fight with the user if
+        // They are trying to disarm.
+        disconnect(vehicle, &Vehicle::mavCommandResult, this, &PX4FirmwarePlugin::_mavCommandResult);
+        if (!vehicle->armed()) {
+            vehicle->setArmed(true);
+        }
+    }
+}
+
 void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
 {
     QString takeoffAltParam("MIS_TAKEOFF_ALT");
@@ -367,11 +390,7 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
     }
     Fact* takeoffAlt = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam);
 
-    if (!_armVehicle(vehicle)) {
-        qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm."));
-        return;
-    }
-
+    connect(vehicle, &Vehicle::mavCommandResult, this, &PX4FirmwarePlugin::_mavCommandResult);
     vehicle->sendMavCommand(vehicle->defaultComponentId(),
                             MAV_CMD_NAV_TAKEOFF,
                             true,                           // show error is fails
@@ -436,7 +455,7 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
 
 void PX4FirmwarePlugin::startMission(Vehicle* vehicle)
 {
-    if (!_armVehicle(vehicle)) {
+    if (!_armVehicleAndValidate(vehicle)) {
         qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to arm."));
         return;
     }

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

@@ -103,6 +103,9 @@ protected:
     QString _followMeFlightMode;
     QString _simpleFlightMode;
 
+private slots:
+    void _mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle);
+
 private:
     void _handleAutopilotVersion(Vehicle* vehicle, mavlink_message_t* message);
 

src/MissionManager/CameraSection.FactMetaData.json

@@ -29,7 +29,7 @@
     "name":             "GimbalPitch",
     "shortDescription": "Gimbal pitch rotation.",
     "type":             "double",
-    "units":            "deg",
+    "units":            "gimbal-degrees",
     "min":              -90,
     "max":              0,
     "decimalPlaces":    0,

src/QmlControls/ParameterEditorDialog.qml

@@ -138,7 +138,9 @@ QGCViewDialog {
                 }
 
                 onCurrentIndexChanged: {
-                    valueField.text = fact.enumValues[currentIndex]
+                    if (currentIndex >=0 && currentIndex < model.length) {
+                        valueField.text = fact.enumValues[currentIndex]
+                    }
                 }
             }
 

src/Vehicle/Vehicle.h

@@ -621,6 +621,7 @@ public:
     ///     @param component Component to send to
     ///     @param command MAV_CMD to send
     ///     @param showError true: Display error to user if command failed, false:  no error shown
+    /// Signals: mavCommandResult on success or failure
     void sendMavCommand(int component, MAV_CMD command, bool showError, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f);
 
     int firmwareMajorVersion(void) const { return _firmwareMajorVersion; }