diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index f331b4a..2faeab4 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -97,6 +97,7 @@ const char* Vehicle::_headingToHomeFactName = "headingToHome"; const char* Vehicle::_distanceToGCSFactName = "distanceToGCS"; const char* Vehicle::_hobbsFactName = "hobbs"; const char* Vehicle::_throttlePctFactName = "throttlePct"; +const char* Vehicle::_imuTempFactName = "imuTemp"; const char* Vehicle::_gpsFactGroupName = "gps"; const char* Vehicle::_gps2FactGroupName = "gps2"; @@ -159,6 +160,7 @@ Vehicle::Vehicle(LinkInterface* link, , _distanceToGCSFact (0, _distanceToGCSFactName, FactMetaData::valueTypeDouble) , _hobbsFact (0, _hobbsFactName, FactMetaData::valueTypeString) , _throttlePctFact (0, _throttlePctFactName, FactMetaData::valueTypeUint16) + , _imuTempFact (0, _imuTempFactName, FactMetaData::valueTypeInt16) , _gpsFactGroup (this) , _gps2FactGroup (this) , _windFactGroup (this) @@ -315,6 +317,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _distanceToGCSFact (0, _distanceToGCSFactName, FactMetaData::valueTypeDouble) , _hobbsFact (0, _hobbsFactName, FactMetaData::valueTypeString) , _throttlePctFact (0, _throttlePctFactName, FactMetaData::valueTypeUint16) + , _imuTempFact (0, _imuTempFactName, FactMetaData::valueTypeInt16) , _gpsFactGroup (this) , _gps2FactGroup (this) , _windFactGroup (this) @@ -441,6 +444,7 @@ void Vehicle::_commonInit() _addFact(&_headingToHomeFact, _headingToHomeFactName); _addFact(&_distanceToGCSFact, _distanceToGCSFactName); _addFact(&_throttlePctFact, _throttlePctFactName); + _addFact(&_imuTempFact, _imuTempFactName); _hobbsFact.setRawValue(QVariant(QString("0000:00:00"))); _addFact(&_hobbsFact, _hobbsFactName); @@ -694,7 +698,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes _handleSysStatus(message); break; case MAVLINK_MSG_ID_RAW_IMU: - emit mavlinkRawImu(message); + _handleRawImuTemp(message); break; case MAVLINK_MSG_ID_SCALED_IMU: emit mavlinkScaledImu1(message); @@ -3802,6 +3806,17 @@ void Vehicle::_handleADSBVehicle(const mavlink_message_t& message) } } +void Vehicle::_handleRawImuTemp(mavlink_message_t& message) +{ + // This is used by compass calibration + emit mavlinkRawImu(message); + + mavlink_raw_imu_t imuRaw; + mavlink_msg_raw_imu_decode(&message, &imuRaw); + + _imuTempFact.setRawValue(imuRaw.temperature == 0 ? 0 : imuRaw.temperature * 0.01); +} + void Vehicle::_updateDistanceHeadingToHome() { if (coordinate().isValid() && homePosition().isValid()) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index b2036f8..231e300 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -308,6 +308,7 @@ public: Q_PROPERTY(Fact* distanceToGCS READ distanceToGCS CONSTANT) Q_PROPERTY(Fact* hobbs READ hobbs CONSTANT) Q_PROPERTY(Fact* throttlePct READ throttlePct CONSTANT) + Q_PROPERTY(Fact* imuTemp READ imuTemp CONSTANT) Q_PROPERTY(FactGroup* gps READ gpsFactGroup CONSTANT) Q_PROPERTY(FactGroup* gps2 READ gps2FactGroup CONSTANT) @@ -684,6 +685,7 @@ public: Fact* distanceToGCS () { return &_distanceToGCSFact; } Fact* hobbs () { return &_hobbsFact; } Fact* throttlePct () { return &_throttlePctFact; } + Fact* imuTemp () { return &_imuTempFact; } FactGroup* gpsFactGroup () { return &_gpsFactGroup; } FactGroup* gps2FactGroup () { return &_gps2FactGroup; } @@ -1054,6 +1056,7 @@ private: #endif void _handleCameraImageCaptured (const mavlink_message_t& message); void _handleADSBVehicle (const mavlink_message_t& message); + void _handleRawImuTemp (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); @@ -1361,6 +1364,7 @@ private: Fact _distanceToGCSFact; Fact _hobbsFact; Fact _throttlePctFact; + Fact _imuTempFact; VehicleGPSFactGroup _gpsFactGroup; VehicleGPS2FactGroup _gps2FactGroup; @@ -1415,6 +1419,7 @@ private: static const char* _distanceToGCSFactName; static const char* _hobbsFactName; static const char* _throttlePctFactName; + static const char* _imuTempFactName; static const char* _gpsFactGroupName; static const char* _gps2FactGroupName; diff --git a/src/Vehicle/VehicleFact.json b/src/Vehicle/VehicleFact.json index f6688d3..e194c82 100644 --- a/src/Vehicle/VehicleFact.json +++ b/src/Vehicle/VehicleFact.json @@ -136,6 +136,12 @@ "shortDesc": "Throttle %", "type": "uint16", "units": "%" +}, +{ + "name": "imuTemp", + "shortDesc": "Imu temperature", + "type": "int16", + "units": "°C" } ] }