Merge pull request #2874 from DonLakeFlyer/VehicleWind

Add Vehicle.wind properties

qgroundcontrol.qrc

@@ -157,5 +157,6 @@
         <file alias="Vehicle/VehicleFact.json">src/Vehicle/VehicleFact.json</file>
         <file alias="Vehicle/BatteryFact.json">src/Vehicle/BatteryFact.json</file>
         <file alias="Vehicle/GPSFact.json">src/Vehicle/GPSFact.json</file>
+        <file alias="Vehicle/WindFact.json">src/Vehicle/WindFact.json</file>
     </qresource>
 </RCC>

src/FactSystem/Fact.cc

@@ -254,15 +254,29 @@ QString Fact::_variantToString(const QVariant& variant) const
     QString valueString;
 
     switch (type()) {
-        case FactMetaData::valueTypeFloat:
-            valueString = QString("%1").arg(variant.toFloat(), 0, 'f', decimalPlaces());
-            break;
-        case FactMetaData::valueTypeDouble:
-            valueString = QString("%1").arg(variant.toDouble(), 0, 'f', decimalPlaces());
-            break;
-        default:
-            valueString = variant.toString();
-            break;
+    case FactMetaData::valueTypeFloat:
+    {
+        float fValue = variant.toFloat();
+        if (qIsNaN(fValue)) {
+            valueString = QStringLiteral("--.--");
+        } else {
+            valueString = QString("%1").arg(fValue, 0, 'f', decimalPlaces());
+        }
+    }
+        break;
+    case FactMetaData::valueTypeDouble:
+    {
+        double dValue = variant.toDouble();
+        if (qIsNaN(dValue)) {
+            valueString = QStringLiteral("--.--");
+        } else {
+            valueString = QString("%1").arg(dValue, 0, 'f', decimalPlaces());
+        }
+    }
+        break;
+    default:
+        valueString = variant.toString();
+        break;
     }
 
     return valueString;

src/Vehicle/Vehicle.cc

@@ -57,6 +57,7 @@ const char* Vehicle::_altitudeAMSLFactName =        "altitudeAMSL";
 
 const char* Vehicle::_gpsFactGroupName =        "gps";
 const char* Vehicle::_batteryFactGroupName =    "battery";
+const char* Vehicle::_windFactGroupName =       "wind";
 
 const char* VehicleGPSFactGroup::_hdopFactName =                "hdop";
 const char* VehicleGPSFactGroup::_vdopFactName =                "vdop";
@@ -78,6 +79,10 @@ const int    VehicleBatteryFactGroup::_currentUnavailable =           -1;
 const double VehicleBatteryFactGroup::_temperatureUnavailable =       -1.0;
 const int    VehicleBatteryFactGroup::_cellCountUnavailable =         -1.0;
 
+const char* VehicleWindFactGroup::_directionFactName =      "direction";
+const char* VehicleWindFactGroup::_speedFactName =          "speed";
+const char* VehicleWindFactGroup::_verticalSpeedFactName =  "verticalSpeed";
+
 Vehicle::Vehicle(LinkInterface*             link,
                  int                        vehicleId,
                  MAV_AUTOPILOT              firmwareType,
@@ -144,6 +149,7 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _altitudeAMSLFact     (0, _altitudeAMSLFactName,      FactMetaData::valueTypeDouble)
     , _gpsFactGroup(this)
     , _batteryFactGroup(this)
+    , _windFactGroup(this)
 {
     _addLink(link);
 
@@ -225,8 +231,11 @@ Vehicle::Vehicle(LinkInterface*             link,
 
     _addFactGroup(&_gpsFactGroup,       _gpsFactGroupName);
     _addFactGroup(&_batteryFactGroup,   _batteryFactGroupName);
+    _addFactGroup(&_windFactGroup,      _windFactGroupName);
+
     _gpsFactGroup.setVehicle(this);
     _batteryFactGroup.setVehicle(this);
+    _windFactGroup.setVehicle(this);
 }
 
 Vehicle::~Vehicle()
@@ -324,6 +333,12 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     case MAVLINK_MSG_ID_SCALED_IMU3:
         emit mavlinkScaledImu3(message);
         break;
+
+    // Following are ArduPilot dialect messages
+
+    case MAVLINK_MSG_ID_WIND:
+        _handleWind(message);
+        break;
     }
 
     emit mavlinkMessageReceived(message);
@@ -331,6 +346,16 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     _uas->receiveMessage(message);
 }
 
+void Vehicle::_handleWind(mavlink_message_t& message)
+{
+    mavlink_wind_t wind;
+    mavlink_msg_wind_decode(&message, &wind);
+
+    _windFactGroup.direction()->setRawValue(wind.direction);
+    _windFactGroup.speed()->setRawValue(wind.speed);
+    _windFactGroup.verticalSpeed()->setRawValue(wind.speed_z);
+}
+
 void Vehicle::_handleSysStatus(mavlink_message_t& message)
 {
     mavlink_sys_status_t sysStatus;
@@ -1310,3 +1335,25 @@ void VehicleBatteryFactGroup::setVehicle(Vehicle* vehicle)
 {
     _vehicle = vehicle;
 }
+
+VehicleWindFactGroup::VehicleWindFactGroup(QObject* parent)
+    : FactGroup(1000, ":/json/Vehicle/WindFact.json", parent)
+    , _vehicle(NULL)
+    , _directionFact    (0, _directionFactName,     FactMetaData::valueTypeDouble)
+    , _speedFact        (0, _speedFactName,         FactMetaData::valueTypeDouble)
+    , _verticalSpeedFact(0, _verticalSpeedFactName, FactMetaData::valueTypeDouble)
+{
+    _addFact(&_directionFact,       _directionFactName);
+    _addFact(&_speedFact,           _speedFactName);
+    _addFact(&_verticalSpeedFact,   _verticalSpeedFactName);
+
+    // Start out as not available "--.--"
+    _directionFact.setRawValue      (std::numeric_limits<float>::quiet_NaN());
+    _speedFact.setRawValue          (std::numeric_limits<float>::quiet_NaN());
+    _verticalSpeedFact.setRawValue  (std::numeric_limits<float>::quiet_NaN());
+}
+
+void VehicleWindFactGroup::setVehicle(Vehicle* vehicle)
+{
+    _vehicle = vehicle;
+}

src/Vehicle/Vehicle.h

@@ -52,6 +52,36 @@ Q_DECLARE_LOGGING_CATEGORY(VehicleLog)
 
 class Vehicle;
 
+class VehicleWindFactGroup : public FactGroup
+{
+    Q_OBJECT
+
+public:
+    VehicleWindFactGroup(QObject* parent = NULL);
+
+    Q_PROPERTY(Fact* direction      READ direction      CONSTANT)
+    Q_PROPERTY(Fact* speed          READ speed          CONSTANT)
+    Q_PROPERTY(Fact* verticalSpeed  READ verticalSpeed  CONSTANT)
+
+    Fact* direction(void)       { return &_directionFact; }
+    Fact* speed(void)           { return &_speedFact; }
+    Fact* verticalSpeed(void)   { return &_verticalSpeedFact; }
+
+    void setVehicle(Vehicle* vehicle);
+
+    static const char* _directionFactName;
+    static const char* _speedFactName;
+    static const char* _verticalSpeedFactName;
+
+private slots:
+
+private:
+    Vehicle*    _vehicle;
+    Fact        _directionFact;
+    Fact        _speedFact;
+    Fact        _verticalSpeedFact;
+};
+
 class VehicleGPSFactGroup : public FactGroup
 {
     Q_OBJECT
@@ -212,6 +242,7 @@ public:
 
     Q_PROPERTY(FactGroup* gps       READ gpsFactGroup       CONSTANT)
     Q_PROPERTY(FactGroup* battery   READ batteryFactGroup   CONSTANT)
+    Q_PROPERTY(FactGroup* wind      READ windFactGroup   CONSTANT)
 
     /// Resets link status counters
     Q_INVOKABLE void resetCounters  ();
@@ -360,6 +391,7 @@ public:
 
     FactGroup* gpsFactGroup     (void) { return &_gpsFactGroup; }
     FactGroup* batteryFactGroup (void) { return &_batteryFactGroup; }
+    FactGroup* windFactGroup    (void) { return &_windFactGroup; }
 
     void setConnectionLostEnabled(bool connectionLostEnabled);
 
@@ -462,6 +494,7 @@ private:
     void _handleRCChannelsRaw(mavlink_message_t& message);
     void _handleBatteryStatus(mavlink_message_t& message);
     void _handleSysStatus(mavlink_message_t& message);
+    void _handleWind(mavlink_message_t& message);
     void _missionManagerError(int errorCode, const QString& errorMsg);
     void _mapTrajectoryStart(void);
     void _mapTrajectoryStop(void);
@@ -574,6 +607,7 @@ private:
 
     VehicleGPSFactGroup     _gpsFactGroup;
     VehicleBatteryFactGroup _batteryFactGroup;
+    VehicleWindFactGroup    _windFactGroup;
 
     static const char* _rollFactName;
     static const char* _pitchFactName;
@@ -583,8 +617,10 @@ private:
     static const char* _climbRateFactName;
     static const char* _altitudeRelativeFactName;
     static const char* _altitudeAMSLFactName;
+
     static const char* _gpsFactGroupName;
     static const char* _batteryFactGroupName;
+    static const char* _windFactGroupName;
 
     static const int _vehicleUIUpdateRateMSecs = 100;
 

src/Vehicle/WindFact.json

+{
+    "version": 1,
+
+    "properties": [
+        {
+            "name":             "direction",
+            "shortDescription": "Wind Direction",
+            "type":             "double",
+            "decimalPlaces":    1,
+            "units":            "degrees"
+        },
+        {
+            "name":             "speed",
+            "shortDescription": "Wind Spd",
+            "type":             "double",
+            "decimalPlaces":    1,
+            "units":            "m/s"
+        },
+        {
+            "name":             "verticalSpeed",
+            "shortDescription": "Wind Spd (vert)",
+            "type":             "double",
+            "decimalPlaces":    1,
+            "units":            "m/s"
+        }
+    ]
+}