From 34dc935afd6ce8218cba6d3fbbf367c133443d5b Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sat, 2 Jul 2016 23:10:30 -0400
Subject: [PATCH] mavlink WIND_COV -> windFactGroup

---
 src/Vehicle/Vehicle.cc | 16 ++++++++++++++++
 src/Vehicle/Vehicle.h  |  1 +
 2 files changed, 17 insertions(+)

diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 47cb97e848..2a2f4ff407 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -443,6 +443,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
         _handleAutopilotVersion(message);
         break;
+    case MAVLINK_MSG_ID_WIND_COV:
+        _handleWindCov(message);
+        break;
 
     // Following are ArduPilot dialect messages
 
@@ -542,6 +545,19 @@ void Vehicle::_handleVibration(mavlink_message_t& message)
     _vibrationFactGroup.clipCount3()->setRawValue(vibration.clipping_2);
 }
 
+void Vehicle::_handleWindCov(mavlink_message_t& message)
+{
+    mavlink_wind_cov_t wind;
+    mavlink_msg_wind_cov_decode(&message, &wind);
+
+    float direction = qRadiansToDegrees(qAtan2(wind.wind_y, wind.wind_x));
+    float speed = qSqrt(qPow(wind.wind_x, 2) + qPow(wind.wind_y, 2));
+
+    _windFactGroup.direction()->setRawValue(direction);
+    _windFactGroup.speed()->setRawValue(speed);
+    _windFactGroup.verticalSpeed()->setRawValue(0);
+}
+
 void Vehicle::_handleWind(mavlink_message_t& message)
 {
     mavlink_wind_t wind;
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index 52e89fb1b6..ebc398bdff 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -639,6 +639,7 @@ private:
     void _handleRCChannelsRaw(mavlink_message_t& message);
     void _handleBatteryStatus(mavlink_message_t& message);
     void _handleSysStatus(mavlink_message_t& message);
+    void _handleWindCov(mavlink_message_t& message);
     void _handleWind(mavlink_message_t& message);
     void _handleVibration(mavlink_message_t& message);
     void _handleExtendedSysState(mavlink_message_t& message);
-- 
GitLab