diff --git a/src/FactSystem/FactGroup.cc b/src/FactSystem/FactGroup.cc index 4ff0fa628fa7270d8348ca1ce13d98a8c1d5b745..bea0959c5d495125a44dbfffb91ce9c510d33a61 100644 --- a/src/FactSystem/FactGroup.cc +++ b/src/FactSystem/FactGroup.cc @@ -177,7 +177,7 @@ void FactGroup::_loadMetaData(const QString& jsonFilename) // Make sure we have the required keys QString errorString; QStringList requiredKeys; - requiredKeys << _nameJsonKey << _decimalPlacesJsonKey << _typeJsonKey << _shortDescriptionJsonKey; + requiredKeys << _nameJsonKey << _typeJsonKey << _shortDescriptionJsonKey; if (!JsonHelper::validateRequiredKeys(jsonObject, requiredKeys, errorString)) { qWarning() << errorString; return; @@ -215,7 +215,7 @@ void FactGroup::_loadMetaData(const QString& jsonFilename) FactMetaData* metaData = new FactMetaData(type, this); - metaData->setDecimalPlaces(jsonObject.value(_decimalPlacesJsonKey).toInt()); + metaData->setDecimalPlaces(jsonObject.value(_decimalPlacesJsonKey).toInt(0)); metaData->setShortDescription(jsonObject.value(_shortDescriptionJsonKey).toString()); metaData->setRawUnits(jsonObject.value(_unitsJsonKey).toString()); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 58102584b9b2de27d2f86e658ea4f40e412bd229..7a9e01d820efc2585a4f023b4f167251dd371987 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -58,6 +58,7 @@ const char* Vehicle::_altitudeAMSLFactName = "altitudeAMSL"; const char* Vehicle::_gpsFactGroupName = "gps"; const char* Vehicle::_batteryFactGroupName = "battery"; const char* Vehicle::_windFactGroupName = "wind"; +const char* Vehicle::_vibrationFactGroupName = "vibration"; const char* VehicleGPSFactGroup::_hdopFactName = "hdop"; const char* VehicleGPSFactGroup::_vdopFactName = "vdop"; @@ -83,6 +84,13 @@ const char* VehicleWindFactGroup::_directionFactName = "direction"; const char* VehicleWindFactGroup::_speedFactName = "speed"; const char* VehicleWindFactGroup::_verticalSpeedFactName = "verticalSpeed"; +const char* VehicleVibrationFactGroup::_xAxisFactName = "xAxis"; +const char* VehicleVibrationFactGroup::_yAxisFactName = "yAxis"; +const char* VehicleVibrationFactGroup::_zAxisFactName = "zAxis"; +const char* VehicleVibrationFactGroup::_clipCount1FactName = "clipCount1"; +const char* VehicleVibrationFactGroup::_clipCount2FactName = "clipCount2"; +const char* VehicleVibrationFactGroup::_clipCount3FactName = "clipCount3"; + Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType, @@ -150,6 +158,7 @@ Vehicle::Vehicle(LinkInterface* link, , _gpsFactGroup(this) , _batteryFactGroup(this) , _windFactGroup(this) + , _vibrationFactGroup(this) { _addLink(link); @@ -232,10 +241,12 @@ Vehicle::Vehicle(LinkInterface* link, _addFactGroup(&_gpsFactGroup, _gpsFactGroupName); _addFactGroup(&_batteryFactGroup, _batteryFactGroupName); _addFactGroup(&_windFactGroup, _windFactGroupName); + _addFactGroup(&_vibrationFactGroup, _vibrationFactGroupName); _gpsFactGroup.setVehicle(this); _batteryFactGroup.setVehicle(this); _windFactGroup.setVehicle(this); + _vibrationFactGroup.setVehicle(this); } Vehicle::~Vehicle() @@ -333,6 +344,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_SCALED_IMU3: emit mavlinkScaledImu3(message); break; + case MAVLINK_MSG_ID_VIBRATION: + _handleVibration(message); + break; // Following are ArduPilot dialect messages @@ -346,6 +360,19 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes _uas->receiveMessage(message); } +void Vehicle::_handleVibration(mavlink_message_t& message) +{ + mavlink_vibration_t vibration; + mavlink_msg_vibration_decode(&message, &vibration); + + _vibrationFactGroup.xAxis()->setRawValue(vibration.vibration_x); + _vibrationFactGroup.yAxis()->setRawValue(vibration.vibration_y); + _vibrationFactGroup.zAxis()->setRawValue(vibration.vibration_z); + _vibrationFactGroup.clipCount1()->setRawValue(vibration.clipping_0); + _vibrationFactGroup.clipCount2()->setRawValue(vibration.clipping_1); + _vibrationFactGroup.clipCount3()->setRawValue(vibration.clipping_2); +} + void Vehicle::_handleWind(mavlink_message_t& message) { mavlink_wind_t wind; @@ -1357,3 +1384,31 @@ void VehicleWindFactGroup::setVehicle(Vehicle* vehicle) { _vehicle = vehicle; } + +VehicleVibrationFactGroup::VehicleVibrationFactGroup(QObject* parent) + : FactGroup(1000, ":/json/Vehicle/VibrationFact.json", parent) + , _vehicle(NULL) + , _xAxisFact (0, _xAxisFactName, FactMetaData::valueTypeDouble) + , _yAxisFact (0, _yAxisFactName, FactMetaData::valueTypeDouble) + , _zAxisFact (0, _zAxisFactName, FactMetaData::valueTypeDouble) + , _clipCount1Fact (0, _clipCount1FactName, FactMetaData::valueTypeUint32) + , _clipCount2Fact (0, _clipCount2FactName, FactMetaData::valueTypeUint32) + , _clipCount3Fact (0, _clipCount3FactName, FactMetaData::valueTypeUint32) +{ + _addFact(&_xAxisFact, _xAxisFactName); + _addFact(&_yAxisFact, _yAxisFactName); + _addFact(&_zAxisFact, _zAxisFactName); + _addFact(&_clipCount1Fact, _clipCount1FactName); + _addFact(&_clipCount2Fact, _clipCount2FactName); + _addFact(&_clipCount3Fact, _clipCount3FactName); + + // Start out as not available "--.--" + _xAxisFact.setRawValue(std::numeric_limits::quiet_NaN()); + _yAxisFact.setRawValue(std::numeric_limits::quiet_NaN()); + _zAxisFact.setRawValue(std::numeric_limits::quiet_NaN()); +} + +void VehicleVibrationFactGroup::setVehicle(Vehicle* vehicle) +{ + _vehicle = vehicle; +} diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 6b477187921ea0d15ffd5dc88dbac230caec5eed..84522cd6c4dfbf4d53f82c914713cd40ba550353 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -52,6 +52,46 @@ Q_DECLARE_LOGGING_CATEGORY(VehicleLog) class Vehicle; +class VehicleVibrationFactGroup : public FactGroup +{ + Q_OBJECT + +public: + VehicleVibrationFactGroup(QObject* parent = NULL); + + Q_PROPERTY(Fact* xAxis READ xAxis CONSTANT) + Q_PROPERTY(Fact* yAxis READ yAxis CONSTANT) + Q_PROPERTY(Fact* zAxis READ zAxis CONSTANT) + Q_PROPERTY(Fact* clipCount1 READ clipCount1 CONSTANT) + Q_PROPERTY(Fact* clipCount2 READ clipCount2 CONSTANT) + Q_PROPERTY(Fact* clipCount3 READ clipCount3 CONSTANT) + + Fact* xAxis(void) { return &_xAxisFact; } + Fact* yAxis(void) { return &_yAxisFact; } + Fact* zAxis(void) { return &_zAxisFact; } + Fact* clipCount1(void) { return &_clipCount1Fact; } + Fact* clipCount2(void) { return &_clipCount2Fact; } + Fact* clipCount3(void) { return &_clipCount3Fact; } + + void setVehicle(Vehicle* vehicle); + + static const char* _xAxisFactName; + static const char* _yAxisFactName; + static const char* _zAxisFactName; + static const char* _clipCount1FactName; + static const char* _clipCount2FactName; + static const char* _clipCount3FactName; + +private: + Vehicle* _vehicle; + Fact _xAxisFact; + Fact _yAxisFact; + Fact _zAxisFact; + Fact _clipCount1Fact; + Fact _clipCount2Fact; + Fact _clipCount3Fact; +}; + class VehicleWindFactGroup : public FactGroup { Q_OBJECT @@ -73,8 +113,6 @@ public: static const char* _speedFactName; static const char* _verticalSpeedFactName; -private slots: - private: Vehicle* _vehicle; Fact _directionFact; @@ -242,7 +280,8 @@ public: Q_PROPERTY(FactGroup* gps READ gpsFactGroup CONSTANT) Q_PROPERTY(FactGroup* battery READ batteryFactGroup CONSTANT) - Q_PROPERTY(FactGroup* wind READ windFactGroup CONSTANT) + Q_PROPERTY(FactGroup* wind READ windFactGroup CONSTANT) + Q_PROPERTY(FactGroup* vibration READ vibrationFactGroup CONSTANT) /// Resets link status counters Q_INVOKABLE void resetCounters (); @@ -389,9 +428,10 @@ public: Fact* altitudeRelative (void) { return &_altitudeRelativeFact; } Fact* altitudeAMSL (void) { return &_altitudeAMSLFact; } - FactGroup* gpsFactGroup (void) { return &_gpsFactGroup; } - FactGroup* batteryFactGroup (void) { return &_batteryFactGroup; } - FactGroup* windFactGroup (void) { return &_windFactGroup; } + FactGroup* gpsFactGroup (void) { return &_gpsFactGroup; } + FactGroup* batteryFactGroup (void) { return &_batteryFactGroup; } + FactGroup* windFactGroup (void) { return &_windFactGroup; } + FactGroup* vibrationFactGroup (void) { return &_vibrationFactGroup; } void setConnectionLostEnabled(bool connectionLostEnabled); @@ -495,6 +535,7 @@ private: void _handleBatteryStatus(mavlink_message_t& message); void _handleSysStatus(mavlink_message_t& message); void _handleWind(mavlink_message_t& message); + void _handleVibration(mavlink_message_t& message); void _missionManagerError(int errorCode, const QString& errorMsg); void _mapTrajectoryStart(void); void _mapTrajectoryStop(void); @@ -605,9 +646,10 @@ private: Fact _altitudeRelativeFact; Fact _altitudeAMSLFact; - VehicleGPSFactGroup _gpsFactGroup; - VehicleBatteryFactGroup _batteryFactGroup; - VehicleWindFactGroup _windFactGroup; + VehicleGPSFactGroup _gpsFactGroup; + VehicleBatteryFactGroup _batteryFactGroup; + VehicleWindFactGroup _windFactGroup; + VehicleVibrationFactGroup _vibrationFactGroup; static const char* _rollFactName; static const char* _pitchFactName; @@ -621,6 +663,7 @@ private: static const char* _gpsFactGroupName; static const char* _batteryFactGroupName; static const char* _windFactGroupName; + static const char* _vibrationFactGroupName; static const int _vehicleUIUpdateRateMSecs = 100; diff --git a/src/Vehicle/VibrationFact.json b/src/Vehicle/VibrationFact.json new file mode 100644 index 0000000000000000000000000000000000000000..c7185584a37adf5f10fde58599b84a7be947f4e0 --- /dev/null +++ b/src/Vehicle/VibrationFact.json @@ -0,0 +1,39 @@ +{ + "version": 1, + + "properties": [ + { + "name": "xAxis", + "shortDescription": "Vibe xAxis", + "type": "double", + "decimalPlaces": 1 + }, + { + "name": "yAxis", + "shortDescription": "Vibe yAxis", + "type": "double", + "decimalPlaces": 1 + }, + { + "name": "zAxis", + "shortDescription": "Vibe zAxis", + "type": "double", + "decimalPlaces": 1 + }, + { + "name": "clipCount1", + "shortDescription": "Clip Count (1)", + "type": "uint32" + }, + { + "name": "clipCount2", + "shortDescription": "Clip Count (2)", + "type": "uint32" + }, + { + "name": "clipCount3", + "shortDescription": "Clip Count (3)", + "type": "uint32" + } + ] +}