diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 47cb97e848562d2f3931b8a3bab7547cbcc0091a..2a2f4ff407fbc09163b0b00dd4180d6370a036a0 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 52e89fb1b6f1ce1cd9d523d11464c1569012da38..ebc398bdff2ebd250318a9d5de73f62306726850 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);