From 2d7fcb24653ee44aac80070df1ce3853fe11ffde Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 23 Feb 2016 14:59:57 -0800 Subject: [PATCH] Add Vehicle.wind properties --- qgroundcontrol.qrc | 1 + src/FactSystem/Fact.cc | 32 ++++++++++++++++++-------- src/Vehicle/Vehicle.cc | 47 +++++++++++++++++++++++++++++++++++++++ src/Vehicle/Vehicle.h | 36 ++++++++++++++++++++++++++++++ src/Vehicle/WindFact.json | 27 ++++++++++++++++++++++ 5 files changed, 134 insertions(+), 9 deletions(-) create mode 100644 src/Vehicle/WindFact.json diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 7846913c9..e9bdf62ea 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -157,5 +157,6 @@ src/Vehicle/VehicleFact.json src/Vehicle/BatteryFact.json src/Vehicle/GPSFact.json + src/Vehicle/WindFact.json diff --git a/src/FactSystem/Fact.cc b/src/FactSystem/Fact.cc index 0216cd2da..d3df47e99 100644 --- a/src/FactSystem/Fact.cc +++ b/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; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index a0d360521..81411f152 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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; @@ -1315,3 +1340,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::quiet_NaN()); + _speedFact.setRawValue (std::numeric_limits::quiet_NaN()); + _verticalSpeedFact.setRawValue (std::numeric_limits::quiet_NaN()); +} + +void VehicleWindFactGroup::setVehicle(Vehicle* vehicle) +{ + _vehicle = vehicle; +} diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 49aef16b6..e97270246 100644 --- a/src/Vehicle/Vehicle.h +++ b/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 @@ -214,6 +244,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 (); @@ -363,6 +394,7 @@ public: FactGroup* gpsFactGroup (void) { return &_gpsFactGroup; } FactGroup* batteryFactGroup (void) { return &_batteryFactGroup; } + FactGroup* windFactGroup (void) { return &_windFactGroup; } void setConnectionLostEnabled(bool connectionLostEnabled); @@ -465,6 +497,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); @@ -577,6 +610,7 @@ private: VehicleGPSFactGroup _gpsFactGroup; VehicleBatteryFactGroup _batteryFactGroup; + VehicleWindFactGroup _windFactGroup; static const char* _rollFactName; static const char* _pitchFactName; @@ -586,8 +620,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; diff --git a/src/Vehicle/WindFact.json b/src/Vehicle/WindFact.json new file mode 100644 index 000000000..881c5ee98 --- /dev/null +++ b/src/Vehicle/WindFact.json @@ -0,0 +1,27 @@ +{ + "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" + } + ] +} -- 2.22.0