Skip to content

Commit 8125e5f

Browse files
error414error414
committed
Smartport legacy sensors
1 parent 33baac3 commit 8125e5f

File tree

12 files changed

+848
-216
lines changed

12 files changed

+848
-216
lines changed

‎docs/Settings.md‎

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -604,7 +604,7 @@ CRSF telemetry link ratio
604604

605605
### crsf_telemetry_mode
606606

607-
Use extended custom telemetry sensors for CRSF
607+
Use extended custom or native telemetry sensors for CRSF
608608

609609
| Default | Min | Max |
610610
| --- | --- | --- |
@@ -6212,6 +6212,16 @@ _// TODO_
62126212

62136213
---
62146214

6215+
### smartport_telemetry_mode
6216+
6217+
Use legacy or standard telemetry sensors for SmartPort
6218+
6219+
| Default | Min | Max |
6220+
| --- | --- | --- |
6221+
| LEGACY | | |
6222+
6223+
---
6224+
62156225
### smith_predictor_delay
62166226

62176227
Expected delay of the gyro signal. In milliseconds

‎src/main/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -628,6 +628,8 @@ main_sources(COMMON_SRC
628628
telemetry/telemetry.h
629629
telemetry/sensors.c
630630
telemetry/sensors.h
631+
telemetry/smartport_legacy.c
632+
telemetry/smartport_legacy.h
631633
)
632634

633635
add_subdirectory(target)

‎src/main/fc/settings.yaml‎

Lines changed: 11 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -106,6 +106,9 @@ tables:
106106
- name: crsf_telemetry_modes
107107
values: [ "OFF", "NATIVE", "CUSTOM" ]
108108
enum: crsfTelemetryMode_e
109+
- name: smartport_telemetry_modes
110+
values: [ "LEGACY", "STANDARD" ]
111+
enum: smartportTelemetryMode_e
109112
- name: smartport_fuel_unit
110113
values: ["PERCENT", "MAH", "MWH"]
111114
enum: smartportFuelUnit_e
@@ -3117,11 +3120,17 @@ groups:
31173120
default_value: OFF
31183121
type: bool
31193122
- name: crsf_telemetry_mode
3120-
description: "Use extended custom telemetry sensors for CRSF"
3123+
description: "Use extended custom or native telemetry sensors for CRSF"
31213124
default_value: NATIVE
3122-
condition: USE_TELEMETRY_LTM
3125+
condition: USE_TELEMETRY_CRSF
31233126
table: crsf_telemetry_modes
31243127
type: uint8_t
3128+
- name: smartport_telemetry_mode
3129+
description: "Use legacy or standard telemetry sensors for SmartPort"
3130+
default_value: LEGACY
3131+
condition: USE_TELEMETRY_SMARTPORT
3132+
table: smartport_telemetry_modes
3133+
type: uint8_t
31253134
- name: crsf_telemetry_link_rate
31263135
description: "CRSF telemetry link rate"
31273136
default_value: 250

‎src/main/telemetry/crsf.c‎

Lines changed: 56 additions & 103 deletions
Original file line numberDiff line numberDiff line change
@@ -284,6 +284,34 @@ void crsfSensorEncodeLatLong(telemetrySensor_t *sensor, sbuf_t *buf)
284284
crsfSerialize32BE(buf, gpsSol.llh.lon);
285285
}
286286

287+
void crsfSensorEncodeEscRpm(telemetrySensor_t *sensor, sbuf_t *buf)
288+
{
289+
UNUSED(sensor);
290+
uint8_t motorCount = MAX(getMotorCount(), 1); //must send at least one motor, to avoid CRSF frame shifting
291+
motorCount = MIN(getMotorCount(), CRSF_PAYLOAD_SIZE_MAX / 3); // 3 bytes per RPM value
292+
motorCount = MIN(motorCount, MAX_SUPPORTED_MOTORS); // ensure we don't exceed available ESC telemetry data
293+
294+
for (uint8_t i = 0; i < motorCount; i++) {
295+
const escSensorData_t *escState = getEscTelemetry(i);
296+
crsfSerialize24BE(buf, escState->rpm & 0xFFFFFF);
297+
}
298+
}
299+
void crsfSensorEncodeEscTemperature(telemetrySensor_t *sensor, sbuf_t *buf)
300+
{
301+
UNUSED(sensor);
302+
303+
uint8_t motorCount = MAX(getMotorCount(), 1); //must send at least one motor, to avoid CRSF frame shifting
304+
motorCount = MIN(getMotorCount(), CRSF_PAYLOAD_SIZE_MAX / 3); // 3 bytes per RPM value
305+
motorCount = MIN(motorCount, MAX_SUPPORTED_MOTORS); // ensure we don't exceed available ESC telemetry data
306+
307+
for (uint8_t i = 0; i < motorCount; i++) {
308+
const escSensorData_t *escState = getEscTelemetry(i);
309+
uint32_t rpm = (escState) ? (escState->temperature * 10) & 0xFFFFFF : TEMPERATURE_INVALID_VALUE;
310+
crsfSerialize24BE(buf, rpm);
311+
}
312+
313+
}
314+
287315
///////////////////////////////////////////////////////////////////////////////////////////
288316

289317

@@ -335,16 +363,21 @@ static sbuf_t * crsfInitializeSbuf(void)
335363
.encode = (telemetryEncode_f)crsfSensorEncode##ENCODER, \
336364
}
337365

366+
///////////////////////////////////////////////////////////////////////
367+
////// LEGACY sensors, don't add new sensors here /////////////////////
338368
static telemetrySensor_t crsfNativeTelemetrySensors[] =
339369
{
340370
TLM_SENSOR(FLIGHT_MODE, 0, 100, 100, 0, Nil),
341371
TLM_SENSOR(BATTERY, 0, 100, 100, 0, Nil),
342372
TLM_SENSOR(ATTITUDE, 0, 100, 100, 0, Nil),
373+
#ifdef USE_BARO
343374
TLM_SENSOR(ALTITUDE, 0, 100, 100, 0, Nil),
375+
#endif
376+
#ifdef USE_GPS
344377
TLM_SENSOR(GPS, 0, 100, 100, 0, Nil),
345-
TLM_SENSOR(ESC_RPM, 0, 100, 100, 0, Nil),
346-
TLM_SENSOR(ESC_TEMPERATURE, 0, 100, 100, 0, Nil),
378+
#endif
347379
};
380+
///////////////////////////////////////////////////////////////////////
348381

349382
static telemetrySensor_t crsfCustomTelemetrySensors[] =
350383
{
@@ -383,26 +416,26 @@ static telemetrySensor_t crsfCustomTelemetrySensors[] =
383416
TLM_SENSOR(GPS_GROUNDSPEED, 0x1128, 200, 3000, 0, U16),
384417
TLM_SENSOR(GPS_HOME_DISTANCE, 0x1129, 200, 3000, 0, U16),
385418
TLM_SENSOR(GPS_HOME_DIRECTION, 0x112A, 200, 3000, 0, U16),
419+
TLM_SENSOR(GPS_AZIMUTH, 0x112B, 200, 3000, 0, U16),
386420
#endif
387421

388422
#ifdef USE_ESC_SENSOR
389-
TLM_SENSOR(ESC1_RPM, 0x1131, 200, 3000, 0, U16),
390-
TLM_SENSOR(ESC2_RPM, 0x1132, 200, 3000, 0, U16),
391-
TLM_SENSOR(ESC3_RPM, 0x1133, 200, 3000, 0, U16),
392-
TLM_SENSOR(ESC4_RPM, 0x1134, 200, 3000, 0, U16),
423+
TLM_SENSOR(ESC_RPM, 0x1131, 200, 3000, 0, EscRpm),
424+
TLM_SENSOR(ESC1_RPM, 0x1132, 200, 3000, 0, U16),
425+
TLM_SENSOR(ESC2_RPM, 0x1133, 200, 3000, 0, U16),
426+
TLM_SENSOR(ESC3_RPM, 0x1134, 200, 3000, 0, U16),
427+
TLM_SENSOR(ESC4_RPM, 0x1135, 200, 3000, 0, U16),
428+
429+
TLM_SENSOR(ESC_TEMPERATURE, 0x1136, 200, 3000, 0, U8),
430+
TLM_SENSOR(ESC1_TEMPERATURE, 0x1137, 200, 3000, 0, U8),
431+
TLM_SENSOR(ESC2_TEMPERATURE, 0x1138, 200, 3000, 0, U8),
432+
TLM_SENSOR(ESC3_TEMPERATURE, 0x1139, 200, 3000, 0, U8),
433+
TLM_SENSOR(ESC4_TEMPERATURE, 0x113A, 200, 3000, 0, U8),
393434
#endif
394435

395-
#ifdef USE_TEMPERATURE_SENSOR
396-
TLM_SENSOR(ESC1_TEMPERATURE, 0x1135, 200, 3000, 0, U8),
397-
TLM_SENSOR(ESC2_TEMPERATURE, 0x1136, 200, 3000, 0, U8),
398-
TLM_SENSOR(ESC3_TEMPERATURE, 0x1137, 200, 3000, 0, U8),
399-
TLM_SENSOR(ESC4_TEMPERATURE, 0x1138, 200, 3000, 0, U8),
400-
#endif
401-
402-
TLM_SENSOR(CPU_LOAD, 0x1141, 500, 3000, 10, U8),
403-
404-
TLM_SENSOR(FLIGHT_MODE, 0x1201, 200, 3000, 0, U16),
405-
TLM_SENSOR(ARMING_FLAGS, 0x1202, 200, 3000, 0, U8),
436+
TLM_SENSOR(CPU_LOAD, 0x1150, 500, 3000, 10, U8),
437+
TLM_SENSOR(FLIGHT_MODE, 0x1251, 200, 3000, 0, U16),
438+
TLM_SENSOR(ARMING_FLAGS, 0x1252, 200, 3000, 0, U8),
406439
};
407440

408441
telemetrySensor_t * crsfGetNativeSensor(sensor_id_e id)
@@ -429,68 +462,6 @@ telemetrySensor_t * crsfGetCustomSensor(sensor_id_e id)
429462

430463
///////////////////////////////////////////////////////////////////////////////////////////
431464
///////////////////////////////////////////////////////////////////////////////////////////
432-
/*
433-
0x0C RPM
434-
Payload:
435-
uint8_t rpm_source_id; // Identifies the source of the RPM data (e.g., 0 = Motor 1, 1 = Motor 2, etc.)
436-
int24_t rpm_value[]; // 1 - 19 RPM values with negative ones representing the motor spinning in reverse
437-
*/
438-
static void crsfRpm(sbuf_t *dst)
439-
{
440-
uint8_t motorCount = getMotorCount();
441-
442-
if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) {
443-
sbufWriteU8(dst, 1 + (motorCount * 3) + CRSF_FRAME_LENGTH_TYPE_CRC);
444-
crsfSerialize8(dst, CRSF_FRAMETYPE_RPM);
445-
// 0 = FC including all ESCs
446-
crsfSerialize8(dst, 0);
447-
448-
for (uint8_t i = 0; i < motorCount; i++) {
449-
const escSensorData_t *escState = getEscTelemetry(i);
450-
crsfSerialize24BE(dst, (escState) ? escState->rpm : 0);
451-
}
452-
}
453-
}
454-
455-
/*
456-
0x0D TEMP
457-
Payload:
458-
uint8_t temp_source_id; // Identifies the source of the temperature data (e.g., 0 = FC including all ESCs, 1 = Ambient, etc.)
459-
int16_t temperature[]; // up to 20 temperature values in deci-degree (tenths of a degree) Celsius (e.g., 250 = 25.0°C, -50 = -5.0°C)
460-
*/
461-
static void crsfTemperature(sbuf_t *dst)
462-
{
463-
uint8_t tempCount = 0;
464-
int16_t temperatures[20];
465-
466-
#ifdef USE_ESC_SENSOR
467-
uint8_t motorCount = getMotorCount();
468-
if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) {
469-
for (uint8_t i = 0; i < motorCount; i++) {
470-
const escSensorData_t *escState = getEscTelemetry(i);
471-
temperatures[tempCount++] = (escState) ? escState->temperature * 10 : TEMPERATURE_INVALID_VALUE;
472-
}
473-
}
474-
#endif
475-
476-
#ifdef USE_TEMPERATURE_SENSOR
477-
for (uint8_t i = 0; i < MAX_TEMP_SENSORS; i++) {
478-
int16_t value;
479-
if (getSensorTemperature(i, &value))
480-
temperatures[tempCount++] = value;
481-
}
482-
#endif
483-
484-
if (tempCount > 0) {
485-
sbufWriteU8(dst, 1 + (tempCount * 2) + CRSF_FRAME_LENGTH_TYPE_CRC);
486-
crsfSerialize8(dst, CRSF_FRAMETYPE_TEMP);
487-
// 0 = FC including all ESCs
488-
crsfSerialize8(dst, 0);
489-
for (uint8_t i = 0; i < tempCount; i++)
490-
crsfSerialize16BE(dst, temperatures[i]);
491-
}
492-
}
493-
494465
/*
495466
CRSF frame has the structure:
496467
<Device address> <Frame length> <Type> <Payload> <CRC>
@@ -770,7 +741,7 @@ void crsfSendMspResponse(uint8_t *payload, const uint8_t payloadSize)
770741

771742
static bool crsfSendNativeTelemetry(void)
772743
{
773-
if (crsfTelemetryState != TELEMETRY_STATE_NATIVE)
744+
if (crsfTelemetryState != CRSFR_TELEMETRY_STATE_NATIVE)
774745
{
775746
return false;
776747
}
@@ -813,24 +784,6 @@ static bool crsfSendNativeTelemetry(void)
813784
crsfFrameGps(dst);
814785
crsfFinalize(dst);
815786
break;
816-
#endif
817-
#ifdef USE_ESC_SENSOR
818-
case TELEM_ESC_RPM:
819-
if(STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0) {
820-
crsfInitializeFrame(dst);
821-
crsfRpm(dst);
822-
crsfFinalize(dst);
823-
}
824-
break;
825-
#endif
826-
#ifdef USE_TEMPERATURE_SENSOR
827-
case TELEM_ESC_TEMPERATURE:
828-
if(STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0) {
829-
crsfInitializeFrame(dst);
830-
crsfTemperature(dst);
831-
crsfFinalize(dst);
832-
}
833-
break;
834787
#endif
835788
default:
836789
crsfInitializeFrame(dst); // write sync byte [CRSF_TELEMETRY_SYNC_BYTE]
@@ -847,7 +800,7 @@ static bool crsfSendNativeTelemetry(void)
847800

848801
static bool crsfSendCustomTelemetry(void)
849802
{
850-
if (crsfTelemetryState == TELEMETRY_STATE_CUSTOM)
803+
if (crsfTelemetryState == CRSFR_TELEMETRY_STATE_CUSTOM)
851804
{
852805
size_t sensor_count = 0;
853806
sbuf_t *dst = crsfInitializeSbuf(); // prepare buffer
@@ -887,7 +840,7 @@ static bool crsfSendCustomTelemetry(void)
887840

888841
static bool crsfPopulateCustomTelemetry(void)
889842
{
890-
if (crsfTelemetryState == TELEMETRY_STATE_POPULATE)
843+
if (crsfTelemetryState == CRSFR_TELEMETRY_STATE_POPULATE)
891844
{
892845
static int slot = -10;
893846

@@ -924,7 +877,7 @@ static bool crsfPopulateCustomTelemetry(void)
924877
}
925878
}
926879

927-
crsfTelemetryState = TELEMETRY_STATE_CUSTOM;
880+
crsfTelemetryState = CRSFR_TELEMETRY_STATE_CUSTOM;
928881
}
929882

930883
return false;
@@ -1008,7 +961,7 @@ void initCrsfTelemetry(void)
1008961
{
1009962
// check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX)
1010963
// and feature is enabled, if so, set CRSF telemetry enabled
1011-
crsfTelemetryState = !crsfRxIsActive() ? TELEMETRY_STATE_OFF : (telemetryConfig()->crsf_telemetry_mode == TELEMETRY_STATE_NATIVE ? TELEMETRY_STATE_NATIVE : TELEMETRY_STATE_POPULATE);
964+
crsfTelemetryState = !crsfRxIsActive() ? CRSFR_TELEMETRY_STATE_OFF : (telemetryConfig()->crsf_telemetry_mode == CRSFR_TELEMETRY_STATE_NATIVE ? CRSFR_TELEMETRY_STATE_NATIVE : CRSFR_TELEMETRY_STATE_POPULATE);
1012965

1013966
if(crsfTelemetryState) {
1014967
deviceInfoReplyPending = false;
@@ -1024,7 +977,7 @@ void initCrsfTelemetry(void)
1024977
mspReplyPending = false;
1025978
#endif
1026979

1027-
if (crsfTelemetryState == TELEMETRY_STATE_NATIVE) {
980+
if (crsfTelemetryState == CRSFR_TELEMETRY_STATE_NATIVE) {
1028981
telemetryScheduleInit(crsfNativeTelemetrySensors, ARRAYLEN(crsfNativeTelemetrySensors), false);
1029982
} else {
1030983
initCrsfCustomSensors();

0 commit comments

Comments
 (0)