diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc
index c61200b5bf93204626dd3e8cae158a04ada7acce..c9620f62cd94bed04edab4d3914fd0710cad5d94 100644
--- a/qgroundcontrol.qrc
+++ b/qgroundcontrol.qrc
@@ -187,6 +187,7 @@
src/MissionManager/FWLandingPattern.FactMetaData.json
src/MissionManager/Survey.FactMetaData.json
src/comm/USBBoardInfo.json
+ src/Vehicle/TemperatureFact.json
src/comm/APMArduCopterMockLink.params
diff --git a/src/Vehicle/TemperatureFact.json b/src/Vehicle/TemperatureFact.json
new file mode 100644
index 0000000000000000000000000000000000000000..01cfb25e7702b34dc28111e250374286cc3b9ae8
--- /dev/null
+++ b/src/Vehicle/TemperatureFact.json
@@ -0,0 +1,23 @@
+[
+{
+ "name": "temperature1",
+ "shortDescription": "Temperature (1)",
+ "type": "double",
+ "decimalPlaces": 2,
+ "units": "C"
+},
+{
+ "name": "temperature2",
+ "shortDescription": "Temperature (2)",
+ "type": "double",
+ "decimalPlaces": 2,
+ "units": "C"
+},
+{
+ "name": "temperature3",
+ "shortDescription": "Temperature (3)",
+ "type": "double",
+ "decimalPlaces": 2,
+ "units": "C"
+}
+]
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index fd27daee39a4650e9f9d93f6441f9970fbfd7969..0794337dcc1bbbe9aa328462cf48b71b0fdffa97 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -54,6 +54,7 @@ const char* Vehicle::_gpsFactGroupName = "gps";
const char* Vehicle::_batteryFactGroupName = "battery";
const char* Vehicle::_windFactGroupName = "wind";
const char* Vehicle::_vibrationFactGroupName = "vibration";
+const char* Vehicle::_temperatureFactGroupName = "temperature";
const int Vehicle::_lowBatteryAnnounceRepeatMSecs = 30 * 1000;
@@ -149,6 +150,7 @@ Vehicle::Vehicle(LinkInterface* link,
, _batteryFactGroup(this)
, _windFactGroup(this)
, _vibrationFactGroup(this)
+ , _temperatureFactGroup(this)
{
_addLink(link);
@@ -343,6 +345,7 @@ void Vehicle::_commonInit(void)
_addFactGroup(&_batteryFactGroup, _batteryFactGroupName);
_addFactGroup(&_windFactGroup, _windFactGroupName);
_addFactGroup(&_vibrationFactGroup, _vibrationFactGroupName);
+ _addFactGroup(&_temperatureFactGroup, _temperatureFactGroupName);
}
Vehicle::~Vehicle()
@@ -561,6 +564,15 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_VFR_HUD:
_handleVfrHud(message);
break;
+ case MAVLINK_MSG_ID_SCALED_PRESSURE:
+ _handleScaledPressure(message);
+ break;
+ case MAVLINK_MSG_ID_SCALED_PRESSURE2:
+ _handleScaledPressure2(message);
+ break;
+ case MAVLINK_MSG_ID_SCALED_PRESSURE3:
+ _handleScaledPressure3(message);
+ break;
// Following are ArduPilot dialect messages
@@ -990,6 +1002,24 @@ void Vehicle::_handleRCChannelsRaw(mavlink_message_t& message)
emit rcChannelsChanged(channelCount, pwmValues);
}
+void Vehicle::_handleScaledPressure(mavlink_message_t& message) {
+ mavlink_scaled_pressure_t pressure;
+ mavlink_msg_scaled_pressure_decode(&message, &pressure);
+ _temperatureFactGroup.temperature1()->setRawValue(pressure.temperature / 100.0);
+}
+
+void Vehicle::_handleScaledPressure2(mavlink_message_t& message) {
+ mavlink_scaled_pressure2_t pressure;
+ mavlink_msg_scaled_pressure2_decode(&message, &pressure);
+ _temperatureFactGroup.temperature2()->setRawValue(pressure.temperature / 100.0);
+}
+
+void Vehicle::_handleScaledPressure3(mavlink_message_t& message) {
+ mavlink_scaled_pressure3_t pressure;
+ mavlink_msg_scaled_pressure3_decode(&message, &pressure);
+ _temperatureFactGroup.temperature3()->setRawValue(pressure.temperature / 100.0);
+}
+
bool Vehicle::_containsLink(LinkInterface* link)
{
return _links.contains(link);
@@ -2394,3 +2424,24 @@ VehicleVibrationFactGroup::VehicleVibrationFactGroup(QObject* parent)
_yAxisFact.setRawValue(std::numeric_limits::quiet_NaN());
_zAxisFact.setRawValue(std::numeric_limits::quiet_NaN());
}
+
+
+const char* VehicleTemperatureFactGroup::_temperature1FactName = "temperature1";
+const char* VehicleTemperatureFactGroup::_temperature2FactName = "temperature2";
+const char* VehicleTemperatureFactGroup::_temperature3FactName = "temperature3";
+
+VehicleTemperatureFactGroup::VehicleTemperatureFactGroup(QObject* parent)
+ : FactGroup(1000, ":/json/Vehicle/TemperatureFact.json", parent)
+ , _temperature1Fact (0, _temperature1FactName, FactMetaData::valueTypeDouble)
+ , _temperature2Fact (0, _temperature2FactName, FactMetaData::valueTypeDouble)
+ , _temperature3Fact (0, _temperature3FactName, FactMetaData::valueTypeDouble)
+{
+ _addFact(&_temperature1Fact, _temperature1FactName);
+ _addFact(&_temperature2Fact, _temperature2FactName);
+ _addFact(&_temperature3Fact, _temperature3FactName);
+
+ // Start out as not available "--.--"
+ _temperature1Fact.setRawValue (std::numeric_limits::quiet_NaN());
+ _temperature2Fact.setRawValue (std::numeric_limits::quiet_NaN());
+ _temperature3Fact.setRawValue (std::numeric_limits::quiet_NaN());
+}
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index 9b634e0be3d5a5b6ca50b05b855cc8644f151e42..89516ba87cc159560e1e9af5c417ab7b3c52133d 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -185,6 +185,35 @@ private:
Fact _cellCountFact;
};
+class VehicleTemperatureFactGroup : public FactGroup
+{
+ Q_OBJECT
+
+public:
+ VehicleTemperatureFactGroup(QObject* parent = NULL);
+
+ Q_PROPERTY(Fact* temperature1 READ temperature1 CONSTANT)
+ Q_PROPERTY(Fact* temperature2 READ temperature2 CONSTANT)
+ Q_PROPERTY(Fact* temperature3 READ temperature3 CONSTANT)
+
+ Fact* temperature1 (void) { return &_temperature1Fact; }
+ Fact* temperature2 (void) { return &_temperature2Fact; }
+ Fact* temperature3 (void) { return &_temperature3Fact; }
+
+ static const char* _temperature1FactName;
+ static const char* _temperature2FactName;
+ static const char* _temperature3FactName;
+
+ static const char* _settingsGroup;
+
+ static const double _temperatureUnavailable;
+
+private:
+ Fact _temperature1Fact;
+ Fact _temperature2Fact;
+ Fact _temperature3Fact;
+};
+
class Vehicle : public FactGroup
{
Q_OBJECT
@@ -306,10 +335,11 @@ public:
Q_PROPERTY(Fact* altitudeRelative READ altitudeRelative CONSTANT)
Q_PROPERTY(Fact* altitudeAMSL READ altitudeAMSL CONSTANT)
- Q_PROPERTY(FactGroup* gps READ gpsFactGroup CONSTANT)
- Q_PROPERTY(FactGroup* battery READ batteryFactGroup CONSTANT)
- Q_PROPERTY(FactGroup* wind READ windFactGroup CONSTANT)
- Q_PROPERTY(FactGroup* vibration READ vibrationFactGroup CONSTANT)
+ Q_PROPERTY(FactGroup* gps READ gpsFactGroup CONSTANT)
+ Q_PROPERTY(FactGroup* battery READ batteryFactGroup CONSTANT)
+ Q_PROPERTY(FactGroup* wind READ windFactGroup CONSTANT)
+ Q_PROPERTY(FactGroup* vibration READ vibrationFactGroup CONSTANT)
+ Q_PROPERTY(FactGroup* temperature READ temperatureFactGroup CONSTANT)
Q_PROPERTY(int firmwareMajorVersion READ firmwareMajorVersion NOTIFY firmwareMajorVersionChanged)
Q_PROPERTY(int firmwareMinorVersion READ firmwareMinorVersion NOTIFY firmwareMinorVersionChanged)
@@ -560,6 +590,7 @@ public:
FactGroup* batteryFactGroup (void) { return &_batteryFactGroup; }
FactGroup* windFactGroup (void) { return &_windFactGroup; }
FactGroup* vibrationFactGroup (void) { return &_vibrationFactGroup; }
+ FactGroup* temperatureFactGroup (void) { return &_temperatureFactGroup; }
void setConnectionLostEnabled(bool connectionLostEnabled);
@@ -754,6 +785,9 @@ private:
void _handleGlobalPositionInt(mavlink_message_t& message);
void _handleAltitude(mavlink_message_t& message);
void _handleVfrHud(mavlink_message_t& message);
+ void _handleScaledPressure(mavlink_message_t& message);
+ void _handleScaledPressure2(mavlink_message_t& message);
+ void _handleScaledPressure3(mavlink_message_t& message);
void _missionManagerError(int errorCode, const QString& errorMsg);
void _geoFenceManagerError(int errorCode, const QString& errorMsg);
void _rallyPointManagerError(int errorCode, const QString& errorMsg);
@@ -924,6 +958,7 @@ private:
VehicleBatteryFactGroup _batteryFactGroup;
VehicleWindFactGroup _windFactGroup;
VehicleVibrationFactGroup _vibrationFactGroup;
+ VehicleTemperatureFactGroup _temperatureFactGroup;
static const char* _rollFactName;
static const char* _pitchFactName;
@@ -938,6 +973,7 @@ private:
static const char* _batteryFactGroupName;
static const char* _windFactGroupName;
static const char* _vibrationFactGroupName;
+ static const char* _temperatureFactGroupName;
static const int _vehicleUIUpdateRateMSecs = 100;