Skip to content

Commit a0aec17

Browse files
3djc3djc
authored
feat(telem): add support for latest CRSF/ELRS sensor definition (#6274)
1 parent 878e3b8 commit a0aec17

File tree

5 files changed

+74
-22
lines changed

5 files changed

+74
-22
lines changed

‎radio/src/telemetry/crossfire.cpp

Lines changed: 62 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -63,6 +63,10 @@ const CrossfireSensor crossfireSensors[] = {
6363
CS(FLIGHT_MODE_ID, 0, STR_SENSOR_FLIGHT_MODE, UNIT_TEXT, 0),
6464
CS(CF_VARIO_ID, 0, STR_SENSOR_VSPD, UNIT_METERS_PER_SECOND, 2),
6565
CS(BARO_ALT_ID, 0, STR_SENSOR_ALT, UNIT_METERS, 2),
66+
CS(AIRSPEED_ID, 0, STR_SENSOR_ASPD, UNIT_KMH, 1),
67+
CS(CF_RPM_ID, 0, STR_SENSOR_RPM, UNIT_RPMS, 0),
68+
CS(TEMP_ID, 0, STR_SENSOR_TEMP, UNIT_DEGREE, 1),
69+
CS(CELLS_ID, 0, STR_SENSOR_CELLS, UNIT_CELLS, 2),
6670
CS(0, 0, "UNKNOWN", UNIT_RAW, 0),
6771
};
6872
// clang-format on
@@ -89,6 +93,14 @@ const CrossfireSensor & getCrossfireSensor(uint8_t id, uint8_t subId)
8993
return crossfireSensors[FLIGHT_MODE_INDEX];
9094
else if (id == BARO_ALT_ID)
9195
return crossfireSensors[BARO_ALTITUDE_INDEX];
96+
else if (id == AIRSPEED_ID)
97+
return crossfireSensors[AIRSPEED_INDEX];
98+
else if (id == CF_RPM_ID)
99+
return crossfireSensors[CF_RPM_INDEX];
100+
else if (id == TEMP_ID)
101+
return crossfireSensors[TEMP_INDEX];
102+
else if (id == CELLS_ID)
103+
return crossfireSensors[CELLS_INDEX];
92104
else
93105
return crossfireSensors[UNKNOWN_INDEX];
94106
}
@@ -172,6 +184,55 @@ void processCrossfireTelemetryFrame(uint8_t module, uint8_t* rxBuffer,
172184
processCrossfireTelemetryValue(VERTICAL_SPEED_INDEX, value);
173185
break;
174186

187+
case AIRSPEED_ID:
188+
if (getCrossfireTelemetryValue<2>(3, value, rxBuffer)) {
189+
// Airspeed in 0.1 * km/h (hectometers/h)
190+
// Converstion to KMH is done through PREC1
191+
processCrossfireTelemetryValue(AIRSPEED_INDEX, value);
192+
}
193+
break;
194+
195+
case CF_RPM_ID:
196+
{
197+
getCrossfireTelemetryValue<1>(3, value, rxBuffer);
198+
uint8_t sensorID = value;
199+
for(uint8_t i = 0; i * 3 < (crsfPayloadLen - 4); i++) {
200+
getCrossfireTelemetryValue<3>(4 + i * 3, value, rxBuffer);
201+
const CrossfireSensor & sensor = crossfireSensors[CF_RPM_INDEX];
202+
setTelemetryValue(PROTOCOL_TELEMETRY_CROSSFIRE, sensor.id + (sensorID << 8), 0, i,
203+
value, sensor.unit, sensor.precision);
204+
}
205+
break;
206+
}
207+
208+
case TEMP_ID:
209+
{
210+
getCrossfireTelemetryValue<1>(3, value, rxBuffer);
211+
uint8_t sensorID = value;
212+
for(uint8_t i = 0; i * 2 < (crsfPayloadLen - 4); i++) {
213+
getCrossfireTelemetryValue<2>(4 + i * 2, value, rxBuffer);
214+
const CrossfireSensor & sensor = crossfireSensors[TEMP_INDEX];
215+
setTelemetryValue(PROTOCOL_TELEMETRY_CROSSFIRE, sensor.id + (sensorID << 8), 0, i,
216+
value, sensor.unit, sensor.precision);
217+
}
218+
break;
219+
}
220+
221+
case CELLS_ID:
222+
{
223+
getCrossfireTelemetryValue<1>(3, value, rxBuffer);
224+
uint8_t sensorID = value;
225+
226+
// We can handle only up to 8 cells
227+
for(uint8_t i = 0; i * 2 < min(16, crsfPayloadLen - 4); i++) {
228+
getCrossfireTelemetryValue<2>(4 + i * 2, value, rxBuffer);
229+
const CrossfireSensor & sensor = crossfireSensors[CELLS_INDEX];
230+
setTelemetryValue(PROTOCOL_TELEMETRY_CROSSFIRE, sensor.id + (sensorID << 8), 0, 0,
231+
i << 16 | value / 10, sensor.unit, sensor.precision);
232+
}
233+
break;
234+
}
235+
175236
case LINK_ID:
176237
for (unsigned int i=0; i<=TX_SNR_INDEX; i++) {
177238
if (getCrossfireTelemetryValue<1>(3+i, value, rxBuffer)) {
@@ -318,7 +379,7 @@ void processCrossfireTelemetryFrame(uint8_t module, uint8_t* rxBuffer,
318379
}
319380
}
320381

321-
void crossfireSetDefault(int index, uint8_t id, uint8_t subId)
382+
void crossfireSetDefault(int index, uint16_t id, uint8_t subId)
322383
{
323384
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
324385

‎radio/src/telemetry/crossfire.h

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,10 @@
3535
#define CF_VARIO_ID 0x07
3636
#define BATTERY_ID 0x08
3737
#define BARO_ALT_ID 0x09
38+
#define AIRSPEED_ID 0x0A
39+
#define CF_RPM_ID 0x0C
40+
#define TEMP_ID 0x0D
41+
#define CELLS_ID 0x0E
3842
#define LINK_ID 0x14
3943
#define CHANNELS_ID 0x16
4044
#define LINK_RX_ID 0x1C
@@ -94,6 +98,10 @@ enum CrossfireSensorIndexes {
9498
FLIGHT_MODE_INDEX,
9599
VERTICAL_SPEED_INDEX,
96100
BARO_ALTITUDE_INDEX,
101+
AIRSPEED_INDEX,
102+
CF_RPM_INDEX,
103+
TEMP_INDEX,
104+
CELLS_INDEX,
97105
UNKNOWN_INDEX,
98106
};
99107

@@ -117,7 +125,7 @@ extern CrossfireModuleStatus crossfireModuleStatus[2];
117125

118126
void processCrossfireTelemetryFrame(uint8_t module, uint8_t* rxBuffer,
119127
uint8_t rxBufferCount);
120-
void crossfireSetDefault(int index, uint8_t id, uint8_t subId);
128+
void crossfireSetDefault(int index, uint16_t id, uint8_t subId);
121129

122130
const uint32_t CROSSFIRE_BAUDRATES[] = {
123131
115200,

‎radio/src/telemetry/telemetry.h

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -110,10 +110,8 @@ PACK(struct CellValue
110110

111111
void set(uint16_t newValue)
112112
{
113-
if (newValue > 50) {
114-
value = newValue;
115-
state = 1;
116-
}
113+
value = newValue;
114+
state = 1;
117115
}
118116
});
119117

‎radio/src/tests/frsky.cpp

Lines changed: 0 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -321,22 +321,6 @@ TEST(FrSkySPORT, frskySetCellVoltage)
321321
EXPECT_EQ(telemetryItems[0].valueMax, 2071);
322322
}
323323

324-
TEST(FrSkySPORT, StrangeCellsBug)
325-
{
326-
MODEL_RESET();
327-
TELEMETRY_RESET();
328-
telemetryStreaming = TELEMETRY_TIMEOUT10ms;
329-
telemetryData.telemetryValid = 0x07;
330-
allowNewSensors = true;
331-
332-
uint8_t pkt[] = { 0x7E, 0x48, 0x10, 0x00, 0x03, 0x30, 0x15, 0x50, 0x81, 0xD5 };
333-
EXPECT_EQ(checkSportPacket(pkt+1), true);
334-
sportProcessTelemetryPacket(0, pkt+1, sizeof(pkt) - 1);
335-
EXPECT_EQ(telemetryItems[0].cells.count, 3);
336-
EXPECT_EQ(telemetryItems[0].cells.values[0].value, 0); // now we ignore such low values
337-
EXPECT_EQ(telemetryItems[0].cells.values[1].value, 413);
338-
}
339-
340324
TEST(FrSkySPORT, frskySetCellVoltageTwoSensors)
341325
{
342326
uint8_t packet[FRSKY_SPORT_PACKET_SIZE];

‎radio/src/translations/untranslated.h

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -118,6 +118,7 @@
118118
#define STR_SENSOR_A4 "A4"
119119
#define STR_SENSOR_BATT "RxBt"
120120
#define STR_SENSOR_ALT "Alt"
121+
#define STR_SENSOR_TEMP "Temp"
121122
#define STR_SENSOR_TEMP1 "Tmp1"
122123
#define STR_SENSOR_TEMP2 "Tmp2"
123124
#define STR_SENSOR_TEMP3 "Tmp3"

0 commit comments

Comments
 (0)