Skip to content

Commit cf486a7

Browse files
authored
Add RPM Sensor Fact Group implementation and related files (mavlink#13775)
1 parent 51c61bb commit cf486a7

4 files changed

Lines changed: 52 additions & 5 deletions

File tree

src/Vehicle/FactGroups/RPMFact.json

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,20 @@
3030
"type": "double",
3131
"decimalPlaces": 2,
3232
"units": "rpm"
33+
},
34+
{
35+
"name": "rpmSensor1",
36+
"shortDesc": "RPM Sensor 1",
37+
"type": "double",
38+
"decimalPlaces": 2,
39+
"units": "rpm"
40+
},
41+
{
42+
"name": "rpmSensor2",
43+
"shortDesc": "RPM Sensor 2",
44+
"type": "double",
45+
"decimalPlaces": 2,
46+
"units": "rpm"
3347
}
3448
]
3549
}

src/Vehicle/FactGroups/VehicleRPMFactGroup.cc

Lines changed: 28 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@
77
*
88
****************************************************************************/
99

10-
#include "VehicleSetpointFactGroup.h"
10+
#include "VehicleRPMFactGroup.h"
1111
#include "Vehicle.h"
1212

1313
VehicleRPMFactGroup::VehicleRPMFactGroup(QObject *parent)
@@ -17,21 +17,35 @@ VehicleRPMFactGroup::VehicleRPMFactGroup(QObject *parent)
1717
_addFact(&_rpm2Fact);
1818
_addFact(&_rpm3Fact);
1919
_addFact(&_rpm4Fact);
20+
_addFact(&_rpmSensor1Fact);
21+
_addFact(&_rpmSensor2Fact);
2022

2123
_rpm1Fact.setRawValue(qQNaN());
2224
_rpm2Fact.setRawValue(qQNaN());
2325
_rpm3Fact.setRawValue(qQNaN());
2426
_rpm4Fact.setRawValue(qQNaN());
27+
_rpmSensor1Fact.setRawValue(qQNaN());
28+
_rpmSensor2Fact.setRawValue(qQNaN());
2529
}
2630

2731
void VehicleRPMFactGroup::handleMessage(Vehicle *vehicle, const mavlink_message_t &message)
2832
{
2933
Q_UNUSED(vehicle);
3034

31-
if (message.msgid != MAVLINK_MSG_ID_RAW_RPM) {
32-
return;
35+
switch (message.msgid) {
36+
case MAVLINK_MSG_ID_RAW_RPM:
37+
_handleRawRPM(message);
38+
break;
39+
case MAVLINK_MSG_ID_RPM:
40+
_handleRPMSensor(message);
41+
break;
42+
default:
43+
break;
3344
}
45+
}
3446

47+
void VehicleRPMFactGroup::_handleRawRPM(const mavlink_message_t &message)
48+
{
3549
mavlink_raw_rpm_t raw_rpm{};
3650
mavlink_msg_raw_rpm_decode(&message, &raw_rpm);
3751
switch (raw_rpm.index) {
@@ -49,8 +63,18 @@ void VehicleRPMFactGroup::handleMessage(Vehicle *vehicle, const mavlink_message_
4963
break;
5064
default:
5165
break;
52-
5366
}
5467

5568
_setTelemetryAvailable(true);
5669
}
70+
71+
void VehicleRPMFactGroup::_handleRPMSensor(const mavlink_message_t &message)
72+
{
73+
mavlink_rpm_t rpm{};
74+
mavlink_msg_rpm_decode(&message, &rpm);
75+
76+
_rpmSensor1Fact.setRawValue(rpm.rpm1);
77+
_rpmSensor2Fact.setRawValue(rpm.rpm2);
78+
79+
_setTelemetryAvailable(true);
80+
}

src/Vehicle/FactGroups/VehicleRPMFactGroup.h

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,8 @@ class VehicleRPMFactGroup : public FactGroup
1818
Q_PROPERTY(Fact *rpm2 READ rpm2 CONSTANT)
1919
Q_PROPERTY(Fact *rpm3 READ rpm3 CONSTANT)
2020
Q_PROPERTY(Fact *rpm4 READ rpm4 CONSTANT)
21+
Q_PROPERTY(Fact *rpmSensor1 READ rpmSensor1 CONSTANT)
22+
Q_PROPERTY(Fact *rpmSensor2 READ rpmSensor2 CONSTANT)
2123

2224
public:
2325
explicit VehicleRPMFactGroup(QObject *parent = nullptr);
@@ -26,13 +28,20 @@ class VehicleRPMFactGroup : public FactGroup
2628
Fact *rpm2() { return &_rpm2Fact; }
2729
Fact *rpm3() { return &_rpm3Fact; }
2830
Fact *rpm4() { return &_rpm4Fact; }
31+
Fact *rpmSensor1() { return &_rpmSensor1Fact; }
32+
Fact *rpmSensor2() { return &_rpmSensor2Fact; }
2933

3034
// Overrides from FactGroup
3135
void handleMessage(Vehicle *vehicle, const mavlink_message_t &message) final;
3236

3337
private:
38+
void _handleRawRPM(const mavlink_message_t &message);
39+
void _handleRPMSensor(const mavlink_message_t &message);
40+
3441
Fact _rpm1Fact = Fact(0, QStringLiteral("rpm1"), FactMetaData::valueTypeDouble);
3542
Fact _rpm2Fact = Fact(0, QStringLiteral("rpm2"), FactMetaData::valueTypeDouble);
3643
Fact _rpm3Fact = Fact(0, QStringLiteral("rpm3"), FactMetaData::valueTypeDouble);
3744
Fact _rpm4Fact = Fact(0, QStringLiteral("rpm4"), FactMetaData::valueTypeDouble);
45+
Fact _rpmSensor1Fact = Fact(0, QStringLiteral("rpmSensor1"), FactMetaData::valueTypeDouble);
46+
Fact _rpmSensor2Fact = Fact(0, QStringLiteral("rpmSensor2"), FactMetaData::valueTypeDouble);
3847
};

src/Vehicle/FactGroups/VehicleTemperatureFactGroup.cc

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@
77
*
88
****************************************************************************/
99

10-
#include "VehicleSetpointFactGroup.h"
10+
#include "VehicleTemperatureFactGroup.h"
1111
#include "Vehicle.h"
1212

1313
VehicleTemperatureFactGroup::VehicleTemperatureFactGroup(QObject *parent)

0 commit comments

Comments
 (0)