diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 5de21a05c3a..dc552482c63 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -745,6 +745,8 @@ HEADERS += \ src/Vehicle/VehicleVibrationFactGroup.h \ src/Vehicle/VehicleWindFactGroup.h \ src/Vehicle/VehicleHygrometerFactGroup.h \ + src/Vehicle/VehicleGeneratorFactGroup.h \ + src/Vehicle/VehicleEFIFactGroup.h \ src/VehicleSetup/JoystickConfigController.h \ src/comm/LinkConfiguration.h \ src/comm/LinkInterface.h \ @@ -1000,6 +1002,8 @@ SOURCES += \ src/Vehicle/VehicleTemperatureFactGroup.cc \ src/Vehicle/VehicleVibrationFactGroup.cc \ src/Vehicle/VehicleHygrometerFactGroup.cc \ + src/Vehicle/VehicleGeneratorFactGroup.cc \ + src/Vehicle/VehicleEFIFactGroup.cc \ src/Vehicle/VehicleWindFactGroup.cc \ src/VehicleSetup/JoystickConfigController.cc \ src/comm/LinkConfiguration.cc \ diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 5877af05917..99940a5b094 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -347,6 +347,8 @@ src/Vehicle/VibrationFact.json src/Vehicle/WindFact.json src/Vehicle/HygrometerFact.json + src/Vehicle/GeneratorFact.json + src/Vehicle/EFIFact.json src/Settings/Video.SettingsGroup.json src/MissionManager/VTOLLandingPattern.FactMetaData.json diff --git a/src/Vehicle/EFIFact.json b/src/Vehicle/EFIFact.json new file mode 100644 index 00000000000..e7c7f3d3a56 --- /dev/null +++ b/src/Vehicle/EFIFact.json @@ -0,0 +1,122 @@ +{ + "version": 1, + "fileType": "FactMetaData", + "QGC.MetaData.Facts": +[ +{ + "name": "health", + "shortDesc": "Health", + "type": "int8" +}, +{ + "name": "ecuIndex", + "shortDesc": "Ecu Index", + "type": "float", + "decimalPlaces": 1, + "units": "A" +}, +{ + "name": "rpm", + "shortDesc": "Rpm", + "type": "float", + "decimalPlaces": 1 +}, +{ + "name": "fuelConsumed", + "shortDesc": "Fuel Consumed", + "type": "float", + "decimalPlaces": 1, + "units": "cm^3" +}, +{ + "name": "fuelFlow", + "shortDesc": "Fuel Flow", + "type": "float", + "decimalPlaces": 1, + "units": "cm^3/min" +}, +{ + "name": "engineLoad", + "shortDesc": "Engine Load", + "type": "float", + "decimalPlaces": 1, + "units": "%" +}, +{ + "name": "throttlePos", + "shortDesc": "Throttle Position", + "type": "float", + "decimalPlaces": 1, + "units": "%" +}, +{ + "name": "sparkTime", + "shortDesc": "Spark dwell time", + "type": "float", + "decimalPlaces": 1, + "units": "ms" +}, +{ + "name": "baroPress", + "shortDesc": "BarometricPressure", + "type": "float", + "decimalPlaces": 1, + "units": "kPa" +}, +{ + "name": "intakePress", + "shortDesc": "Intake mainfold pressure", + "type": "float", + "decimalPlaces": 1, + "units": "kPa" +}, +{ + "name": "intakeTemp", + "shortDesc": "Intake mainfold temperature", + "type": "float", + "decimalPlaces": 1, + "units": "°C" +}, +{ + "name": "cylinderTemp", + "shortDesc": "Cylinder head temperature", + "type": "float", + "decimalPlaces": 1, + "units": "°C" +}, +{ + "name": "ignTime", + "shortDesc": "Ignition Timing", + "type": "float", + "decimalPlaces": 1, + "units": "deg" +}, +{ + "name": "injTime", + "shortDesc": "Injection Time", + "type": "float", + "decimalPlaces": 1, + "units": "ms" +}, +{ + "name": "exGasTemp", + "shortDesc": "Exhaust gas Temperature", + "type": "float", + "decimalPlaces": 1, + "units": "°C" +}, +{ + "name": "throttleOut", + "shortDesc": "Throttle Out", + "type": "float", + "decimalPlaces": 1, + "units": "%" +}, +{ + "name": "ptComp", + "shortDesc": "Pt Compensation", + "type": "float", + "decimalPlaces": 1 +} +] +} \ No newline at end of file diff --git a/src/Vehicle/GeneratorFact.json b/src/Vehicle/GeneratorFact.json new file mode 100644 index 00000000000..d5f8e6f68b5 --- /dev/null +++ b/src/Vehicle/GeneratorFact.json @@ -0,0 +1,77 @@ +{ + "version": 1, + "fileType": "FactMetaData", + "QGC.MetaData.Facts": +[ +{ + "name": "status", + "shortDesc": "Status", + "type": "uint64" +}, +{ + "name": "genSpeed", + "shortDesc": "Generator Speed", + "type": "uint16", + "units": "rpm" +}, +{ + "name": "batteryCurrent", + "shortDesc": "Battery Current", + "type": "float", + "decimalPlaces": 1, + "units": "A" +}, +{ + "name": "loadCurrent", + "shortDesc": "Load Current", + "type": "float", + "decimalPlaces": 1, + "units": "A" +}, +{ + "name": "powerGenerated", + "shortDesc": "Power Generated", + "type": "float", + "decimalPlaces": 1, + "units": "W" +}, +{ + "name": "busVoltage", + "shortDesc": "Bus Voltage", + "type": "float", + "decimalPlaces": 1, + "units": "V" +}, +{ + "name": "rectifierTemp", + "shortDesc": "Rectifier Temperature", + "type": "int16", + "units": "°C" +}, +{ + "name": "batCurrentSetpoint", + "shortDesc": "Battery Current Setpoint", + "type": "float", + "decimalPlaces": 1, + "units": "A" +}, +{ + "name": "genTemp", + "shortDesc": "Generator Temperature", + "type": "int16", + "units": "°C" +}, +{ + "name": "runtime", + "shortDesc": "runtime", + "type": "uint32", + "units": "sec" +}, +{ + "name": "timeMaintenance", + "shortDesc": "Time until Maintenance", + "type": "int32", + "units": "sec" +} +] +} \ No newline at end of file diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 570e3ab6935..fc42c2bc8f7 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -112,6 +112,8 @@ const char* Vehicle::_escStatusFactGroupName = "escStatus"; const char* Vehicle::_estimatorStatusFactGroupName = "estimatorStatus"; const char* Vehicle::_terrainFactGroupName = "terrain"; const char* Vehicle::_hygrometerFactGroupName = "hygrometer"; +const char* Vehicle::_generatorFactGroupName = "generator"; +const char* Vehicle::_efiFactGroupName = "efi"; // Standard connected vehicle Vehicle::Vehicle(LinkInterface* link, @@ -174,6 +176,8 @@ Vehicle::Vehicle(LinkInterface* link, , _escStatusFactGroup (this) , _estimatorStatusFactGroup (this) , _hygrometerFactGroup (this) + , _generatorFactGroup (this) + , _efiFactGroup (this) , _terrainFactGroup (this) , _terrainProtocolHandler (new TerrainProtocolHandler(this, &_terrainFactGroup, this)) { @@ -455,6 +459,8 @@ void Vehicle::_commonInit() _addFactGroup(&_escStatusFactGroup, _escStatusFactGroupName); _addFactGroup(&_estimatorStatusFactGroup, _estimatorStatusFactGroupName); _addFactGroup(&_hygrometerFactGroup, _hygrometerFactGroupName); + _addFactGroup(&_generatorFactGroup, _generatorFactGroupName); + _addFactGroup(&_efiFactGroup, _efiFactGroupName); _addFactGroup(&_terrainFactGroup, _terrainFactGroupName); // Add firmware-specific fact groups, if provided diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 5a61c786aa6..c20eb9c331a 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -49,6 +49,8 @@ #include "HealthAndArmingCheckReport.h" #include "TerrainQuery.h" #include "StandardModes.h" +#include "VehicleGeneratorFactGroup.h" +#include "VehicleEFIFactGroup.h" class Actuators; class EventHandler; @@ -322,6 +324,8 @@ class Vehicle : public FactGroup Q_PROPERTY(FactGroup* localPosition READ localPositionFactGroup CONSTANT) Q_PROPERTY(FactGroup* localPositionSetpoint READ localPositionSetpointFactGroup CONSTANT) Q_PROPERTY(FactGroup* hygrometer READ hygrometerFactGroup CONSTANT) + Q_PROPERTY(FactGroup* generator READ generatorFactGroup CONSTANT) + Q_PROPERTY(FactGroup* efi READ efiFactGroup CONSTANT) Q_PROPERTY(QmlObjectListModel* batteries READ batteries CONSTANT) Q_PROPERTY(Actuators* actuators READ actuators CONSTANT) Q_PROPERTY(HealthAndArmingCheckReport* healthAndArmingCheckReport READ healthAndArmingCheckReport CONSTANT) @@ -713,6 +717,8 @@ class Vehicle : public FactGroup FactGroup* estimatorStatusFactGroup () { return &_estimatorStatusFactGroup; } FactGroup* terrainFactGroup () { return &_terrainFactGroup; } FactGroup* hygrometerFactGroup () { return &_hygrometerFactGroup; } + FactGroup* generatorFactGroup () { return &_generatorFactGroup; } + FactGroup* efiFactGroup () { return &_efiFactGroup; } QmlObjectListModel* batteries () { return &_batteryFactGroupListModel; } MissionManager* missionManager () { return _missionManager; } @@ -1417,6 +1423,8 @@ private slots: VehicleEscStatusFactGroup _escStatusFactGroup; VehicleEstimatorStatusFactGroup _estimatorStatusFactGroup; VehicleHygrometerFactGroup _hygrometerFactGroup; + VehicleGeneratorFactGroup _generatorFactGroup; + VehicleEFIFactGroup _efiFactGroup; TerrainFactGroup _terrainFactGroup; QmlObjectListModel _batteryFactGroupListModel; @@ -1474,6 +1482,8 @@ private slots: static const char* _escStatusFactGroupName; static const char* _estimatorStatusFactGroupName; static const char* _hygrometerFactGroupName; + static const char* _generatorFactGroupName; + static const char* _efiFactGroupName; static const char* _terrainFactGroupName; static const int _vehicleUIUpdateRateMSecs = 100; diff --git a/src/Vehicle/VehicleEFIFactGroup.cc b/src/Vehicle/VehicleEFIFactGroup.cc new file mode 100644 index 00000000000..72d9b480fb1 --- /dev/null +++ b/src/Vehicle/VehicleEFIFactGroup.cc @@ -0,0 +1,114 @@ +#include "VehicleEFIFactGroup.h" +#include "Vehicle.h" + +const char* VehicleEFIFactGroup::_healthFactName = "health"; +const char* VehicleEFIFactGroup::_ecuIndexFactName = "ecuIndex"; +const char* VehicleEFIFactGroup::_rpmFactName = "rpm"; +const char* VehicleEFIFactGroup::_fuelConsumedFactName = "fuelConsumed"; +const char* VehicleEFIFactGroup::_fuelFlowFactName = "fuelFlow"; +const char* VehicleEFIFactGroup::_engineLoadFactName = "engineLoad"; +const char* VehicleEFIFactGroup::_throttlePosFactName = "throttlePos"; +const char* VehicleEFIFactGroup::_sparkTimeFactName = "sparkTime"; +const char* VehicleEFIFactGroup::_baroPressFactName = "baroPress"; +const char* VehicleEFIFactGroup::_intakePressFactName = "intakePress"; +const char* VehicleEFIFactGroup::_intakeTempFactName = "intakeTemp"; +const char* VehicleEFIFactGroup::_cylinderTempFactName = "cylinderTemp"; +const char* VehicleEFIFactGroup::_ignTimeFactName = "ignTime"; +const char* VehicleEFIFactGroup::_injTimeFactName = "injTime"; +const char* VehicleEFIFactGroup::_exGasTempFactName = "exGasTemp"; +const char* VehicleEFIFactGroup::_throttleOutFactName = "throttleOut"; +const char* VehicleEFIFactGroup::_ptCompFactName = "ptComp"; + + +VehicleEFIFactGroup::VehicleEFIFactGroup(QObject* parent) + : FactGroup(1000, ":/json/Vehicle/EFIFact.json", parent) + , _healthFact (0, _healthFactName, FactMetaData::valueTypeInt8) + , _ecuIndexFact (0, _ecuIndexFactName, FactMetaData::valueTypeFloat) + , _rpmFact (0, _rpmFactName, FactMetaData::valueTypeFloat) + , _fuelConsumedFact (0, _fuelConsumedFactName, FactMetaData::valueTypeFloat) + , _fuelFlowFact (0, _fuelFlowFactName, FactMetaData::valueTypeFloat) + , _engineLoadFact (0, _engineLoadFactName, FactMetaData::valueTypeFloat) + , _throttlePosFact (0, _throttlePosFactName, FactMetaData::valueTypeFloat) + , _sparkTimeFact (0, _sparkTimeFactName, FactMetaData::valueTypeFloat) + , _baroPressFact (0, _baroPressFactName, FactMetaData::valueTypeFloat) + , _intakePressFact (0, _intakePressFactName, FactMetaData::valueTypeFloat) + , _intakeTempFact (0, _intakeTempFactName, FactMetaData::valueTypeFloat) + , _cylinderTempFact (0, _cylinderTempFactName, FactMetaData::valueTypeFloat) + , _ignTimeFact (0, _ignTimeFactName, FactMetaData::valueTypeFloat) + , _injTimeFact (0, _injTimeFactName, FactMetaData::valueTypeFloat) + , _exGasTempFact (0, _exGasTempFactName, FactMetaData::valueTypeFloat) + , _throttleOutFact (0, _throttleOutFactName, FactMetaData::valueTypeFloat) + , _ptCompFact (0, _ptCompFactName, FactMetaData::valueTypeFloat) +{ + _addFact(&_healthFact, _healthFactName); + _addFact(&_ecuIndexFact, _ecuIndexFactName); + _addFact(&_rpmFact, _rpmFactName); + _addFact(&_fuelConsumedFact, _fuelConsumedFactName); + _addFact(&_fuelFlowFact, _fuelFlowFactName); + _addFact(&_engineLoadFact, _engineLoadFactName); + _addFact(&_sparkTimeFact, _sparkTimeFactName); + _addFact(&_throttlePosFact, _throttlePosFactName); + _addFact(&_baroPressFact, _baroPressFactName); + _addFact(&_intakePressFact, _intakePressFactName); + _addFact(&_intakeTempFact, _intakeTempFactName); + _addFact(&_cylinderTempFact, _cylinderTempFactName); + _addFact(&_ignTimeFact, _ignTimeFactName); + _addFact(&_exGasTempFact, _exGasTempFactName); + _addFact(&_injTimeFact, _injTimeFactName); + _addFact(&_throttleOutFact, _throttleOutFactName); + _addFact(&_ptCompFact, _ptCompFactName); + + // Start out as not available "--.--" + _healthFact.setRawValue(qQNaN()); + _ecuIndexFact.setRawValue(qQNaN()); + _rpmFact.setRawValue(qQNaN()); + _fuelConsumedFact.setRawValue(qQNaN()); + _fuelFlowFact.setRawValue(qQNaN()); + _engineLoadFact.setRawValue(qQNaN()); + _sparkTimeFact.setRawValue(qQNaN()); + _throttlePosFact.setRawValue(qQNaN()); + _baroPressFact.setRawValue(qQNaN()); + _intakePressFact.setRawValue(qQNaN()); + _intakeTempFact.setRawValue(qQNaN()); + _cylinderTempFact.setRawValue(qQNaN()); + _ignTimeFact.setRawValue(qQNaN()); + _exGasTempFact.setRawValue(qQNaN()); + _injTimeFact.setRawValue(qQNaN()); + _throttleOutFact.setRawValue(qQNaN()); + _ptCompFact.setRawValue(qQNaN()); +} + +void VehicleEFIFactGroup::handleMessage(Vehicle* /* vehicle */, mavlink_message_t& message) +{ + switch (message.msgid) { + case MAVLINK_MSG_ID_EFI_STATUS: + _handleEFIStatus(message); + break; + default: + break; + } +} + +void VehicleEFIFactGroup::_handleEFIStatus(mavlink_message_t& message) +{ + mavlink_efi_status_t efi; + mavlink_msg_efi_status_decode(&message, &efi); + + health()->setRawValue (efi.health == INT8_MAX ? qQNaN() : efi.health); + ecuIndex()->setRawValue (efi.ecu_index); + rpm()->setRawValue (efi.rpm); + fuelConsumed()->setRawValue (efi.fuel_consumed); + fuelFlow()->setRawValue (efi.fuel_flow); + engineLoad()->setRawValue (efi.engine_load); + throttlePos()->setRawValue (efi.throttle_position); + sparkTime()->setRawValue (efi.spark_dwell_time); + baroPress()->setRawValue (efi.barometric_pressure); + intakePress()->setRawValue (efi.intake_manifold_pressure); + intakeTemp()->setRawValue (efi.intake_manifold_temperature); + cylinderTemp()->setRawValue (efi.cylinder_head_temperature); + ignTime()->setRawValue (efi.ignition_timing); + injTime()->setRawValue (efi.injection_time); + exGasTemp()->setRawValue (efi.exhaust_gas_temperature); + throttleOut()->setRawValue (efi.throttle_out); + ptComp()->setRawValue (efi.pt_compensation); +} diff --git a/src/Vehicle/VehicleEFIFactGroup.h b/src/Vehicle/VehicleEFIFactGroup.h new file mode 100644 index 00000000000..879a7a91c74 --- /dev/null +++ b/src/Vehicle/VehicleEFIFactGroup.h @@ -0,0 +1,94 @@ +#pragma once + +#include "FactGroup.h" +#include "QGCMAVLink.h" + +class VehicleEFIFactGroup : public FactGroup +{ + Q_OBJECT + +public: + VehicleEFIFactGroup(QObject* parent = nullptr); + + Q_PROPERTY(Fact* health READ health CONSTANT) + Q_PROPERTY(Fact* ecuIndex READ ecuIndex CONSTANT) + Q_PROPERTY(Fact* rpm READ rpm CONSTANT) + Q_PROPERTY(Fact* fuelConsumed READ fuelConsumed CONSTANT) + Q_PROPERTY(Fact* fuelFlow READ fuelFlow CONSTANT) + Q_PROPERTY(Fact* engineLoad READ engineLoad CONSTANT) + Q_PROPERTY(Fact* throttlePos READ throttlePos CONSTANT) + Q_PROPERTY(Fact* sparkTime READ sparkTime CONSTANT) + Q_PROPERTY(Fact* baroPress READ baroPress CONSTANT) + Q_PROPERTY(Fact* intakePress READ intakePress CONSTANT) + Q_PROPERTY(Fact* intakeTemp READ intakeTemp CONSTANT) + Q_PROPERTY(Fact* cylinderTemp READ cylinderTemp CONSTANT) + Q_PROPERTY(Fact* ignTime READ ignTime CONSTANT) + Q_PROPERTY(Fact* injTime READ injTime CONSTANT) + Q_PROPERTY(Fact* exGasTemp READ exGasTemp CONSTANT) + Q_PROPERTY(Fact* throttleOut READ throttleOut CONSTANT) + Q_PROPERTY(Fact* ptComp READ ptComp CONSTANT) + Q_PROPERTY(Fact* ignVoltage READ ignVoltage CONSTANT) + + Fact* health () { return &_healthFact; } + Fact* ecuIndex () { return &_ecuIndexFact; } + Fact* rpm () { return &_rpmFact; } + Fact* fuelConsumed () { return &_fuelConsumedFact; } + Fact* fuelFlow () { return &_fuelFlowFact; } + Fact* engineLoad () { return &_engineLoadFact; } + Fact* throttlePos () { return &_throttlePosFact; } + Fact* sparkTime () { return &_sparkTimeFact; } + Fact* baroPress () { return &_baroPressFact; } + Fact* intakePress () { return &_intakePressFact; } + Fact* intakeTemp () { return &_intakeTempFact; } + Fact* cylinderTemp () { return &_cylinderTempFact; } + Fact* ignTime () { return &_ignTimeFact; } + Fact* injTime () { return &_injTimeFact; } + Fact* exGasTemp () { return &_exGasTempFact; } + Fact* throttleOut () { return &_throttleOutFact; } + Fact* ptComp () { return &_ptCompFact; } + Fact* ignVoltage () { return &_ignVoltageFact; } + + // Overrides from FactGroup + virtual void handleMessage(Vehicle* vehicle, mavlink_message_t& message) override; + + static const char* _healthFactName; + static const char* _ecuIndexFactName; + static const char* _rpmFactName; + static const char* _fuelConsumedFactName; + static const char* _fuelFlowFactName; + static const char* _engineLoadFactName; + static const char* _throttlePosFactName; + static const char* _sparkTimeFactName; + static const char* _baroPressFactName; + static const char* _intakePressFactName; + static const char* _intakeTempFactName; + static const char* _cylinderTempFactName; + static const char* _ignTimeFactName; + static const char* _injTimeFactName; + static const char* _exGasTempFactName; + static const char* _throttleOutFactName; + static const char* _ptCompFactName; + static const char* _ignVoltageFactName; + +protected: + void _handleEFIStatus(mavlink_message_t& message); + + Fact _healthFact; + Fact _ecuIndexFact; + Fact _rpmFact; + Fact _fuelConsumedFact; + Fact _fuelFlowFact; + Fact _engineLoadFact; + Fact _throttlePosFact; + Fact _sparkTimeFact; + Fact _baroPressFact; + Fact _intakePressFact; + Fact _intakeTempFact; + Fact _cylinderTempFact; + Fact _ignTimeFact; + Fact _injTimeFact; + Fact _exGasTempFact; + Fact _throttleOutFact; + Fact _ptCompFact; + Fact _ignVoltageFact; +}; diff --git a/src/Vehicle/VehicleGeneratorFactGroup.cc b/src/Vehicle/VehicleGeneratorFactGroup.cc new file mode 100644 index 00000000000..9cfbd7d936f --- /dev/null +++ b/src/Vehicle/VehicleGeneratorFactGroup.cc @@ -0,0 +1,110 @@ +#include "VehicleGeneratorFactGroup.h" +#include "Vehicle.h" +#include + +const char* VehicleGeneratorFactGroup::_statusFactName = "status"; +const char* VehicleGeneratorFactGroup::_genSpeedFactName = "genSpeed"; +const char* VehicleGeneratorFactGroup::_batteryCurrentFactName = "batteryCurrent"; +const char* VehicleGeneratorFactGroup::_loadCurrentFactName = "loadCurrent"; +const char* VehicleGeneratorFactGroup::_powerGeneratedFactName = "powerGenerated"; +const char* VehicleGeneratorFactGroup::_busVoltageFactName = "busVoltage"; +const char* VehicleGeneratorFactGroup::_rectifierTempFactName = "rectifierTemp"; +const char* VehicleGeneratorFactGroup::_batCurrentSetpointFactName = "batCurrentSetpoint"; +const char* VehicleGeneratorFactGroup::_genTempFactName = "genTemp"; +const char* VehicleGeneratorFactGroup::_runtimeFactName = "runtime"; +const char* VehicleGeneratorFactGroup::_timeMaintenanceFactName = "timeMaintenance"; + +VehicleGeneratorFactGroup::VehicleGeneratorFactGroup(QObject* parent) + : FactGroup(1000, ":/json/Vehicle/GeneratorFact.json", parent) + , _statusFact (0, _statusFactName, FactMetaData::valueTypeUint64) + , _genSpeedFact (0, _genSpeedFactName, FactMetaData::valueTypeUint16) + , _batteryCurrentFact (0, _batteryCurrentFactName, FactMetaData::valueTypeFloat) + , _loadCurrentFact (0, _loadCurrentFactName, FactMetaData::valueTypeFloat) + , _powerGeneratedFact (0, _powerGeneratedFactName, FactMetaData::valueTypeFloat) + , _busVoltageFact (0, _busVoltageFactName, FactMetaData::valueTypeFloat) + , _rectifierTempFact (0, _rectifierTempFactName, FactMetaData::valueTypeInt16) + , _batCurrentSetpointFact (0, _batCurrentSetpointFactName, FactMetaData::valueTypeFloat) + , _genTempFact (0, _genTempFactName, FactMetaData::valueTypeInt16) + , _runtimeFact (0, _runtimeFactName, FactMetaData::valueTypeUint32) + , _timeMaintenanceFact (0, _timeMaintenanceFactName, FactMetaData::valueTypeInt32) +{ + _addFact(&_statusFact, _statusFactName); + _addFact(&_genSpeedFact, _genSpeedFactName); + _addFact(&_batteryCurrentFact, _batteryCurrentFactName); + _addFact(&_loadCurrentFact, _loadCurrentFactName); + _addFact(&_powerGeneratedFact, _powerGeneratedFactName); + _addFact(&_busVoltageFact, _busVoltageFactName); + _addFact(&_batCurrentSetpointFact, _batCurrentSetpointFactName); + _addFact(&_rectifierTempFact, _rectifierTempFactName); + _addFact(&_genTempFact, _genTempFactName); + _addFact(&_runtimeFact, _runtimeFactName); + _addFact(&_timeMaintenanceFact, _timeMaintenanceFactName); + + // Start out as not available "--.--" + _statusFact.setRawValue(qQNaN()); + _genSpeedFact.setRawValue(qQNaN()); + _batteryCurrentFact.setRawValue(qQNaN()); + _loadCurrentFact.setRawValue(qQNaN()); + _powerGeneratedFact.setRawValue(qQNaN()); + _busVoltageFact.setRawValue(qQNaN()); + _batCurrentSetpointFact.setRawValue(qQNaN()); + _rectifierTempFact.setRawValue(qQNaN()); + _genTempFact.setRawValue(qQNaN()); + _runtimeFact.setRawValue(qQNaN()); + _timeMaintenanceFact.setRawValue(qQNaN()); +} + +void VehicleGeneratorFactGroup::handleMessage(Vehicle* /* vehicle */, mavlink_message_t& message) +{ + switch (message.msgid) { + case MAVLINK_MSG_ID_GENERATOR_STATUS: + _handleGeneratorStatus(message); + break; + default: + break; + } +} + +void VehicleGeneratorFactGroup::_handleGeneratorStatus(mavlink_message_t& message) +{ + mavlink_generator_status_t generator; + mavlink_msg_generator_status_decode(&message, &generator); + + status()->setRawValue (generator.status == UINT16_MAX ? qQNaN() : generator.status); + _updateGeneratorFlags(); + genSpeed()->setRawValue (generator.generator_speed == UINT16_MAX ? qQNaN() : generator.generator_speed); + batteryCurrent()->setRawValue (generator.battery_current); + loadCurrent()->setRawValue (generator.load_current); + powerGenerated()->setRawValue (generator.power_generated); + busVoltage()->setRawValue (generator.bus_voltage); + rectifierTemp()->setRawValue (generator.rectifier_temperature == INT16_MAX ? qQNaN() : generator.rectifier_temperature); + batCurrentSetpoint()->setRawValue (generator.bat_current_setpoint); + genTemp()->setRawValue (generator.generator_temperature == INT16_MAX ? qQNaN() : generator.generator_temperature); + runtime()->setRawValue (generator.runtime == UINT32_MAX ? qQNaN() : generator.runtime); + timeMaintenance()->setRawValue (generator.time_until_maintenance == INT32_MAX ? qQNaN() : generator.time_until_maintenance); +} + +void VehicleGeneratorFactGroup::_updateGeneratorFlags() { + + // Check the status received, and convert it to a List with the state of each flag + int statusFlag = _statusFact.rawValue().toInt(); + + // No need to update the list if we have the same flags + if ( statusFlag == _prevFlag) { + return; + } + + _prevFlag = statusFlag; + _flagsListGenerator.clear(); + + std::bitset<23> bitsetFlags(statusFlag); + + for (size_t i=0; i