From 2d7fcb24653ee44aac80070df1ce3853fe11ffde Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
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 7846913..e9bdf62 100644
--- a/qgroundcontrol.qrc
+++ b/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>
diff --git a/src/FactSystem/Fact.cc b/src/FactSystem/Fact.cc
index 0216cd2..d3df47e 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 a0d3605..81411f1 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<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;
+}
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index 49aef16..e972702 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 0000000..881c5ee
--- /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"
+ }
+ ]
+}