diff --git a/src/FactSystem/Fact.cc b/src/FactSystem/Fact.cc index 8e3ec8a7c7e..50dda7858f1 100644 --- a/src/FactSystem/Fact.cc +++ b/src/FactSystem/Fact.cc @@ -323,6 +323,14 @@ QString Fact::_variantToString(const QVariant &variant, int decimalPlaces) const { QString valueString; + const auto stripNegativeZero = [](QString &candidate) { + static const QRegularExpression reNegativeZero(QStringLiteral("^-0\\.0+$")); + const auto match = reNegativeZero.match(candidate); + if (match.hasMatch() || candidate == QStringLiteral("-0")) { + candidate = candidate.mid(1); + } + }; + switch (type()) { case FactMetaData::valueTypeFloat: { @@ -331,6 +339,7 @@ QString Fact::_variantToString(const QVariant &variant, int decimalPlaces) const valueString = QStringLiteral("--.--"); } else { valueString = QStringLiteral("%1").arg(fValue, 0, 'f', decimalPlaces); + stripNegativeZero(valueString); } } break; @@ -341,6 +350,7 @@ QString Fact::_variantToString(const QVariant &variant, int decimalPlaces) const valueString = QStringLiteral("--.--"); } else { valueString = QStringLiteral("%1").arg(dValue, 0, 'f', decimalPlaces); + stripNegativeZero(valueString); } break; } diff --git a/src/Gimbal/GimbalController.cc b/src/Gimbal/GimbalController.cc index 1405828c5f4..3febf3175e7 100644 --- a/src/Gimbal/GimbalController.cc +++ b/src/Gimbal/GimbalController.cc @@ -16,6 +16,8 @@ #include "SettingsManager.h" #include "Vehicle.h" +#include + QGC_LOGGING_CATEGORY(GimbalControllerLog, "Gimbal.GimbalController") GimbalController::GimbalController(Vehicle *vehicle) @@ -234,15 +236,25 @@ void GimbalController::_handleGimbalDeviceAttitudeStatus(const mavlink_message_t gimbal->setYawLock((attitude_status.flags & GIMBAL_DEVICE_FLAGS_YAW_LOCK) > 0); gimbal->_neutral = (attitude_status.flags & GIMBAL_DEVICE_FLAGS_NEUTRAL) > 0; - float roll, pitch, yaw; - mavlink_quaternion_to_euler(attitude_status.q, &roll, &pitch, &yaw); - - gimbal->setAbsoluteRoll(qRadiansToDegrees(roll)); - gimbal->setAbsolutePitch(qRadiansToDegrees(pitch)); + // Convert from QQuaternion to Euler angles. We specifically don't use mavlink_quaternion_to euler + // because that seems to spew NaNs for boundary conditions. Whereas QQuaternion seems to handle things + // more cleanly. + QQuaternion q( + attitude_status.q[0], + attitude_status.q[1], + attitude_status.q[2], + attitude_status.q[3]); + auto vector3D = q.toEulerAngles(); + float roll = vector3D.z(); + float pitch = vector3D.y(); + float yaw = vector3D.x(); + + gimbal->setAbsoluteRoll(roll); + gimbal->setAbsolutePitch(pitch); const bool yaw_in_vehicle_frame = _yawInVehicleFrame(attitude_status.flags); if (yaw_in_vehicle_frame) { - const float bodyYaw = qRadiansToDegrees(yaw); + const float bodyYaw = yaw; float absoluteYaw = bodyYaw + _vehicle->heading()->rawValue().toFloat(); if (absoluteYaw > 180.0f) { absoluteYaw -= 360.0f; @@ -252,7 +264,7 @@ void GimbalController::_handleGimbalDeviceAttitudeStatus(const mavlink_message_t gimbal->setAbsoluteYaw(absoluteYaw); } else { - const float absoluteYaw = qRadiansToDegrees(yaw); + const float absoluteYaw = yaw; float bodyYaw = absoluteYaw - _vehicle->heading()->rawValue().toFloat(); if (bodyYaw < -180.0f) { bodyYaw += 360.0f; diff --git a/src/UI/toolbar/GimbalIndicator.qml b/src/UI/toolbar/GimbalIndicator.qml index cba0e086fad..562b121b63d 100644 --- a/src/UI/toolbar/GimbalIndicator.qml +++ b/src/UI/toolbar/GimbalIndicator.qml @@ -97,7 +97,7 @@ Item { QGCLabel { id: pitchLabel font.pointSize: ScreenTools.smallFontPointSize - text: activeGimbal ? qsTr("P: ") + activeGimbal.absolutePitch.rawValue.toFixed(1) : "" + text: activeGimbal ? qsTr("P: ") + activeGimbal.absolutePitch.valueString : "" color: qgcPal.windowTransparentText } QGCLabel { @@ -105,8 +105,8 @@ Item { font.pointSize: ScreenTools.smallFontPointSize text: activeGimbal ? (showAzimuth ? - (qsTr("Az: ") + activeGimbal.absoluteYaw.rawValue.toFixed(1)) : - (qsTr("Y: ") + activeGimbal.bodyYaw.rawValue.toFixed(1))) : + (qsTr("Az: ") + activeGimbal.absoluteYaw.valueString) : + (qsTr("Y: ") + activeGimbal.bodyYaw.valueString)) : "" color: qgcPal.windowTransparentText }