diff --git a/make/source.mk b/make/source.mk
index 57196f5b05..c3d9cbf864 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -191,6 +191,7 @@ COMMON_SRC = \
telemetry/msp_shared.c \
telemetry/ibus.c \
telemetry/ibus_shared.c \
+ telemetry/sensors.c \
sensors/esc_sensor.c \
io/vtx.c \
io/vtx_rtc6705.c \
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index d2fa2e1a8a..d552efa9dc 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -484,26 +484,14 @@ const char * const lookupTableSwashType[] = {
"NONE", "PASSTHROUGH", "CP120", "CP135", "CP140", "FP90L", "FP90V",
};
-const char * const lookupTableCrsfFmReuse[] = {
- "NONE", "GOVERNOR", "HEADSPEED", "THROTTLE", "ESC_TEMP", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", "BEC_VOLTAGE", "BUS_VOLTAGE", "MCU_VOLTAGE", "ADJFUNC", "GOV_ADJFUNC",
-};
-
-const char * const lookupTableCrsfAttReuse[] = {
- "NONE", "THROTTLE", "ESC_TEMP", "ESC_PWM", "ESC_BEC_VOLTAGE", "ESC_BEC_CURRENT", "ESC_BEC_TEMP", "ESC_STATUS", "ESC_STATUS2", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", "BEC_VOLTAGE", "BUS_VOLTAGE", "MCU_VOLTAGE",
-};
-
-const char * const lookupTableCrsfGpsReuse[] = {
- "NONE", "HEADSPEED", "THROTTLE", "ESC_TEMP", "ESC_PWM", "ESC_THROTTLE", "ESC_BEC_VOLTAGE", "ESC_BEC_CURRENT", "ESC_BEC_TEMP", "ESC_STATUS", "ESC_STATUS2", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", "BEC_VOLTAGE", "BUS_VOLTAGE", "MCU_VOLTAGE",
-};
-
-const char * const lookupTableCrsfGpsSatsReuse[] = {
- "NONE", "ESC_TEMP", "MCU_TEMP", "PROFILE", "RATE_PROFILE", "LED_PROFILE", "MODEL_ID",
-};
-
const char * const lookupTableDtermMode[] = {
"GYRO", "ERROR",
};
+const char * const lookupTableTelemMode[] = {
+ "NATIVE", "CUSTOM",
+};
+
#define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
const lookupTableEntry_t lookupTables[] = {
@@ -613,11 +601,8 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableRescueMode),
LOOKUP_TABLE_ENTRY(lookupTableSwashType),
- LOOKUP_TABLE_ENTRY(lookupTableCrsfFmReuse),
- LOOKUP_TABLE_ENTRY(lookupTableCrsfAttReuse),
- LOOKUP_TABLE_ENTRY(lookupTableCrsfGpsReuse),
- LOOKUP_TABLE_ENTRY(lookupTableCrsfGpsSatsReuse),
LOOKUP_TABLE_ENTRY(lookupTableDtermMode),
+ LOOKUP_TABLE_ENTRY(lookupTableTelemMode),
};
#undef LOOKUP_TABLE_ENTRY
@@ -1200,15 +1185,11 @@ const clivalue_t valueTable[] = {
// Set to $size_of_battery to get a percentage of battery used.
{ "mavlink_mah_as_heading_divisor", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 30000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, mavlink_mah_as_heading_divisor) },
#endif
- { "crsf_flight_mode_reuse", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRSF_FM_REUSE }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_flight_mode_reuse) },
- { "crsf_att_pitch_reuse", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRSF_ATT_REUSE }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_att_pitch_reuse) },
- { "crsf_att_roll_reuse", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRSF_ATT_REUSE }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_att_roll_reuse) },
- { "crsf_att_yaw_reuse", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRSF_ATT_REUSE }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_att_yaw_reuse) },
-
- { "crsf_gps_heading_reuse", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRSF_GPS_REUSE }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_gps_heading_reuse) },
- { "crsf_gps_ground_speed_reuse", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRSF_GPS_REUSE }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_gps_ground_speed_reuse) },
- { "crsf_gps_altitude_reuse", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRSF_GPS_REUSE }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_gps_altitude_reuse) },
- { "crsf_gps_sats_reuse", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRSF_GPS_SATS_REUSE }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_gps_sats_reuse) },
+ { "crsf_telemetry_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_TELEM_MODE }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_telemetry_mode) },
+ { "crsf_telemetry_link_rate", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_telemetry_link_rate) },
+ { "crsf_telemetry_link_ratio", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_telemetry_link_ratio) },
+ { "crsf_telemetry_sensors", VAR_UINT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = TELEM_SENSOR_SLOT_COUNT, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_telemetry_sensors)},
+ { "crsf_telemetry_interval", VAR_UINT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = TELEM_SENSOR_SLOT_COUNT, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_telemetry_interval)},
#ifdef USE_TELEMETRY_ENABLE_SENSORS
{ "telemetry_enable_voltage", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_VOLTAGE), PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
@@ -1235,7 +1216,7 @@ const clivalue_t valueTable[] = {
{ "telemetry_enable_adjustment", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_ADJUSTMENT), PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
{ "telemetry_enable_gov_mode", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_GOV_MODE), PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
#else
- { "telemetry_enable_sensors", VAR_UINT32 | MASTER_VALUE, .config.u32Max = SENSOR_ALL, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
+ { "telemetry_enable_sensors", VAR_UINT32 | MASTER_VALUE, .config.u32Max = SENSOR_ALL, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
#endif
#endif // USE_TELEMETRY
diff --git a/src/main/cli/settings.h b/src/main/cli/settings.h
index 0e9777c5d9..da4a0d42e5 100644
--- a/src/main/cli/settings.h
+++ b/src/main/cli/settings.h
@@ -127,11 +127,8 @@ typedef enum {
#endif
TABLE_RESCUE_MODE,
TABLE_SWASH_TYPE,
- TABLE_CRSF_FM_REUSE,
- TABLE_CRSF_ATT_REUSE,
- TABLE_CRSF_GPS_REUSE,
- TABLE_CRSF_GPS_SATS_REUSE,
TABLE_DTERM_MODE,
+ TABLE_TELEM_MODE,
LOOKUP_TABLE_COUNT
} lookupTableIndex_e;
diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c
index 9e779bd3d9..4546ddeee8 100644
--- a/src/main/fc/tasks.c
+++ b/src/main/fc/tasks.c
@@ -503,8 +503,8 @@ void tasksInit(void)
// Reschedule telemetry to 500hz for Jeti Exbus
rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500));
} else if (rxRuntimeState.serialrxProvider == SERIALRX_CRSF) {
- // Reschedule telemetry to 500hz, 2ms for CRSF
- rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500));
+ // Reschedule telemetry to 1000hz, 1ms for CRSF
+ rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(1000));
}
}
#endif
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index ad1dc4e1ee..e08321b8f7 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -1581,6 +1581,12 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, telemetryConfig()->halfDuplex);
sbufWriteU32(dst, telemetryConfig()->enableSensors);
sbufWriteU8(dst, telemetryConfig()->pinSwap);
+ sbufWriteU8(dst, telemetryConfig()->crsf_telemetry_mode);
+ sbufWriteU16(dst, telemetryConfig()->crsf_telemetry_link_rate);
+ sbufWriteU16(dst, telemetryConfig()->crsf_telemetry_link_ratio);
+ for (int i = 0; i < TELEM_SENSOR_SLOT_COUNT; i++) {
+ sbufWriteU8(dst, telemetryConfig()->crsf_telemetry_sensors[i]);
+ }
break;
case MSP_SERIAL_CONFIG:
@@ -3089,6 +3095,14 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
if (sbufBytesRemaining(src) >= 1) {
telemetryConfigMutable()->pinSwap = sbufReadU8(src);
}
+ if (sbufBytesRemaining(src) >= 5 + TELEM_SENSOR_SLOT_COUNT) {
+ telemetryConfigMutable()->crsf_telemetry_mode = sbufReadU8(src);
+ telemetryConfigMutable()->crsf_telemetry_link_rate = sbufReadU16(src);
+ telemetryConfigMutable()->crsf_telemetry_link_ratio = sbufReadU16(src);
+ for (int i = 0; i < TELEM_SENSOR_SLOT_COUNT; i++) {
+ telemetryConfigMutable()->crsf_telemetry_sensors[i] = sbufReadU8(src);
+ }
+ }
break;
case MSP_SET_SERIAL_CONFIG:
diff --git a/src/main/pg/telemetry.c b/src/main/pg/telemetry.c
index de13595eeb..b2e4eb7317 100644
--- a/src/main/pg/telemetry.c
+++ b/src/main/pg/telemetry.c
@@ -22,14 +22,11 @@
#include "common/unit.h"
-#include "config/config.h"
-
+#include "pg/pg_ids.h"
#include "pg/telemetry.h"
-#include "telemetry/crsf.h"
-
-PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 5);
+PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 6);
PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
.telemetry_inverted = false,
@@ -44,9 +41,9 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
.pidValuesAsTelemetry = 0,
.report_cell_voltage = false,
.flysky_sensors = {
- IBUS_SENSOR_TYPE_TEMPERATURE,
- IBUS_SENSOR_TYPE_RPM_FLYSKY,
- IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE
+ IBUS_SENSOR_TYPE_TEMPERATURE,
+ IBUS_SENSOR_TYPE_RPM_FLYSKY,
+ IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE
},
.enableSensors =
SENSOR_VOLTAGE |
@@ -57,10 +54,11 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
ESC_SENSOR_RPM |
ESC_SENSOR_TEMPERATURE,
.mavlink_mah_as_heading_divisor = 0,
- .crsf_flight_mode_reuse = CRSF_FM_REUSE_NONE,
- .crsf_att_pitch_reuse = CRSF_ATT_REUSE_NONE,
- .crsf_att_roll_reuse = CRSF_ATT_REUSE_NONE,
- .crsf_att_yaw_reuse = CRSF_ATT_REUSE_NONE,
+ .crsf_telemetry_mode = CRSF_TELEMETRY_MODE_NATIVE,
+ .crsf_telemetry_sensors = INIT_ZERO,
+ .crsf_telemetry_interval = INIT_ZERO,
+ .crsf_telemetry_link_rate = 250,
+ .crsf_telemetry_link_ratio = 8,
);
#endif
diff --git a/src/main/pg/telemetry.h b/src/main/pg/telemetry.h
index 87d91baa8d..a41233860e 100644
--- a/src/main/pg/telemetry.h
+++ b/src/main/pg/telemetry.h
@@ -23,35 +23,31 @@
#include "pg/pg.h"
+
typedef enum {
- SENSOR_VOLTAGE = 1 << 0,
- SENSOR_CURRENT = 1 << 1,
- SENSOR_FUEL = 1 << 2,
- SENSOR_MODE = 1 << 3,
- SENSOR_ACC_X = 1 << 4,
- SENSOR_ACC_Y = 1 << 5,
- SENSOR_ACC_Z = 1 << 6,
- SENSOR_PITCH = 1 << 7,
- SENSOR_ROLL = 1 << 8,
- SENSOR_HEADING = 1 << 9,
- SENSOR_ALTITUDE = 1 << 10,
- SENSOR_VARIO = 1 << 11,
- SENSOR_LAT_LONG = 1 << 12,
- SENSOR_GROUND_SPEED = 1 << 13,
- SENSOR_DISTANCE = 1 << 14,
- ESC_SENSOR_CURRENT = 1 << 15,
- ESC_SENSOR_VOLTAGE = 1 << 16,
- ESC_SENSOR_RPM = 1 << 17,
- ESC_SENSOR_TEMPERATURE = 1 << 18,
- ESC_SENSOR_ALL = ESC_SENSOR_CURRENT \
- | ESC_SENSOR_VOLTAGE \
- | ESC_SENSOR_RPM \
- | ESC_SENSOR_TEMPERATURE,
- SENSOR_TEMPERATURE = 1 << 19,
- SENSOR_CAP_USED = 1 << 20,
- SENSOR_ADJUSTMENT = 1 << 21,
- SENSOR_GOV_MODE = 1 << 22,
- SENSOR_ALL = (1 << 23) - 1,
+ SENSOR_VOLTAGE = BIT(0),
+ SENSOR_CURRENT = BIT(1),
+ SENSOR_FUEL = BIT(2),
+ SENSOR_MODE = BIT(3),
+ SENSOR_ACC_X = BIT(4),
+ SENSOR_ACC_Y = BIT(5),
+ SENSOR_ACC_Z = BIT(6),
+ SENSOR_PITCH = BIT(7),
+ SENSOR_ROLL = BIT(8),
+ SENSOR_HEADING = BIT(9),
+ SENSOR_ALTITUDE = BIT(10),
+ SENSOR_VARIO = BIT(11),
+ SENSOR_LAT_LONG = BIT(12),
+ SENSOR_GROUND_SPEED = BIT(13),
+ SENSOR_DISTANCE = BIT(14),
+ ESC_SENSOR_CURRENT = BIT(15),
+ ESC_SENSOR_VOLTAGE = BIT(16),
+ ESC_SENSOR_RPM = BIT(17),
+ ESC_SENSOR_TEMPERATURE = BIT(18),
+ SENSOR_TEMPERATURE = BIT(19),
+ SENSOR_CAP_USED = BIT(20),
+ SENSOR_ADJUSTMENT = BIT(21),
+ SENSOR_GOV_MODE = BIT(22),
} sensor_e;
typedef enum {
@@ -59,7 +55,14 @@ typedef enum {
FRSKY_FORMAT_NMEA
} frskyGpsCoordFormat_e;
-typedef struct {
+enum {
+ CRSF_TELEMETRY_MODE_NATIVE = 0,
+ CRSF_TELEMETRY_MODE_CUSTOM,
+};
+
+#define TELEM_SENSOR_SLOT_COUNT 40
+
+typedef struct telemetryConfig_s {
int16_t gpsNoFixLatitude;
int16_t gpsNoFixLongitude;
uint8_t telemetry_inverted;
@@ -73,15 +76,13 @@ typedef struct {
uint8_t report_cell_voltage;
uint8_t flysky_sensors[IBUS_SENSOR_COUNT];
uint16_t mavlink_mah_as_heading_divisor;
- uint8_t crsf_flight_mode_reuse;
- uint8_t crsf_att_pitch_reuse;
- uint8_t crsf_att_roll_reuse;
- uint8_t crsf_att_yaw_reuse;
- uint8_t crsf_gps_heading_reuse;
- uint8_t crsf_gps_ground_speed_reuse;
- uint8_t crsf_gps_altitude_reuse;
- uint8_t crsf_gps_sats_reuse;
uint32_t enableSensors;
+ uint8_t crsf_telemetry_mode;
+ uint16_t crsf_telemetry_sensors[TELEM_SENSOR_SLOT_COUNT];
+ uint16_t crsf_telemetry_interval[TELEM_SENSOR_SLOT_COUNT];
+ uint16_t crsf_telemetry_link_rate;
+ uint16_t crsf_telemetry_link_ratio;
} telemetryConfig_t;
PG_DECLARE(telemetryConfig_t, telemetryConfig);
+
diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c
index 348c6d08b6..b7437de345 100644
--- a/src/main/rx/crsf.c
+++ b/src/main/rx/crsf.c
@@ -70,8 +70,6 @@ STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL];
static serialPort_t *serialPort;
static timeUs_t crsfFrameStartAtUs = 0;
-static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
-static uint8_t telemetryBufLen = 0;
static float channelScale = CRSF_RC_CHANNEL_SCALE_LEGACY;
#ifdef USE_RX_LINK_UPLINK_POWER
@@ -337,6 +335,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
return crc;
}
+#if defined(USE_CRSF_V3)
STATIC_UNIT_TESTED uint8_t crsfFrameCmdCRC(void)
{
// CRC includes type and payload
@@ -346,6 +345,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameCmdCRC(void)
}
return crc;
}
+#endif
// Receive ISR callback, called back from serial port
STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
@@ -412,9 +412,6 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
}
#endif
#if defined(USE_CRSF_CMS_TELEMETRY)
- case CRSF_FRAMETYPE_DEVICE_PING:
- crsfScheduleDeviceInfoResponse();
- break;
case CRSF_FRAMETYPE_DISPLAYPORT_CMD: {
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
crsfProcessDisplayPortCmd(frameStart);
@@ -422,7 +419,6 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
}
#endif
#if defined(USE_CRSF_LINK_STATISTICS)
-
case CRSF_FRAMETYPE_LINK_STATISTICS: {
// if to FC and 10 bytes + CRSF_FRAME_ORIGIN_DEST_SIZE
if ((rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) &&
@@ -456,6 +452,9 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
}
break;
#endif
+ case CRSF_FRAMETYPE_DEVICE_PING:
+ crsfScheduleDeviceInfoResponse();
+ break;
default:
break;
}
@@ -598,27 +597,13 @@ STATIC_UNIT_TESTED float crsfReadRawRC(const rxRuntimeState_t *rxRuntimeState, u
}
}
-void crsfRxWriteTelemetryData(const void *data, int len)
-{
- len = MIN(len, (int)sizeof(telemetryBuf));
- memcpy(telemetryBuf, data, len);
- telemetryBufLen = len;
-}
-
-void crsfRxSendTelemetryData(void)
+void crsfRxTransmitTelemetryData(const void *data, int len)
{
- // if there is telemetry data to write
- if (telemetryBufLen > 0) {
- serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
- telemetryBufLen = 0; // reset telemetry buffer
+ if (len > 0 && len <= CRSF_FRAME_SIZE_MAX) {
+ serialWriteBuf(serialPort, data, len);
}
}
-bool crsfRxIsTelemetryBufEmpty(void)
-{
- return telemetryBufLen == 0;
-}
-
bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
{
for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) {
diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h
index a1ae3c80be..038735aee4 100644
--- a/src/main/rx/crsf.h
+++ b/src/main/rx/crsf.h
@@ -78,9 +78,7 @@ typedef union crsfFrame_u {
crsfFrameDef_t frame;
} crsfFrame_t;
-void crsfRxWriteTelemetryData(const void *data, int len);
-void crsfRxSendTelemetryData(void);
-bool crsfRxIsTelemetryBufEmpty(void); // check this function before using crsfRxWriteTelemetryData()
+void crsfRxTransmitTelemetryData(const void *data, int len);
struct rxConfig_s;
struct rxRuntimeState_s;
diff --git a/src/main/rx/crsf_protocol.h b/src/main/rx/crsf_protocol.h
index 99fef0d1a1..439bace241 100644
--- a/src/main/rx/crsf_protocol.h
+++ b/src/main/rx/crsf_protocol.h
@@ -36,7 +36,9 @@ enum { CRSF_PAYLOAD_SIZE_MAX = CRSF_FRAME_SIZE_MAX - 6 };
typedef enum {
CRSF_FRAMETYPE_GPS = 0x02,
+ CRSF_FRAMETYPE_VARIO_SENSOR = 0x07,
CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
+ CRSF_FRAMETYPE_ALTITUDE_SENSOR = 0x09,
CRSF_FRAMETYPE_HEARTBEAT = 0x0B,
CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
@@ -57,6 +59,7 @@ typedef enum {
CRSF_FRAMETYPE_MSP_RESP = 0x7B, // reply with 58 byte chunked binary
CRSF_FRAMETYPE_MSP_WRITE = 0x7C, // write with 8 byte chunked binary (OpenTX outbound telemetry buffer limit)
CRSF_FRAMETYPE_DISPLAYPORT_CMD = 0x7D, // displayport control command
+ CRSF_FRAMETYPE_CUSTOM_TELEM = 0x88, // custom telemetry
} crsfFrameType_e;
enum {
@@ -83,7 +86,9 @@ enum {
enum {
CRSF_FRAME_GPS_PAYLOAD_SIZE = 15,
+ CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2,
CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8,
+ CRSF_FRAME_ALTITUDE_SENSOR_PAYLOAD_SIZE = 4,
CRSF_FRAME_HEARTBEAT_PAYLOAD_SIZE = 2,
CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10,
CRSF_FRAME_LINK_STATISTICS_TX_PAYLOAD_SIZE = 6,
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index 040b469af0..031cfafcb2 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -54,6 +54,8 @@
#include "pg/pg_ids.h"
#include "pg/rx.h"
+#include "telemetry/sensors.h"
+
#include "rx/rx.h"
#include "rx/pwm.h"
#include "rx/fport.h"
diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c
index f22abc592a..32a22e1371 100644
--- a/src/main/sensors/battery.c
+++ b/src/main/sensors/battery.c
@@ -139,6 +139,12 @@ uint8_t getBatteryCellCount(void)
return batteryCellCount;
}
+uint16_t getBatteryCellVoltage(uint8_t cell)
+{
+ UNUSED(cell);
+ return getBatteryAverageCellVoltage(); // Not implemented
+}
+
uint16_t getBatteryAverageCellVoltage(void)
{
return (batteryCellCount > 0) ? getBatteryVoltage() / batteryCellCount : 0;
diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h
index 35d857e9f8..00aa51eea8 100644
--- a/src/main/sensors/battery.h
+++ b/src/main/sensors/battery.h
@@ -63,6 +63,7 @@ const char * getBatteryStateString(void);
bool isBatteryVoltageConfigured(void);
uint8_t getBatteryCellCount(void);
+uint16_t getBatteryCellVoltage(uint8_t cell);
uint16_t getBatteryVoltage(void);
uint16_t getBatteryVoltageSample(void);
uint16_t getLegacyBatteryVoltage(void);
diff --git a/src/main/target/STM32_UNIFIED/target.h b/src/main/target/STM32_UNIFIED/target.h
index e616b044b7..9987d45b77 100644
--- a/src/main/target/STM32_UNIFIED/target.h
+++ b/src/main/target/STM32_UNIFIED/target.h
@@ -385,6 +385,8 @@
#define USE_CMS
+#undef USE_CRSF_V3
+
#undef USE_OSD
#undef USE_MAX7456
#undef USE_RCDEVICE
diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index b26f72230c..e1eff2cf90 100644
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -1,21 +1,18 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Rotorflight.
*
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software under the terms of the
- * GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option)
- * any later version.
+ * Rotorflight is free software. You can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * will be useful, but WITHOUT ANY WARRANTY; without even the implied
- * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * Rotorflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
- * along with this software.
- *
- * If not, see .
+ * along with this software. If not, see .
*/
#include
@@ -46,11 +43,11 @@
#include "drivers/persistent.h"
#include "fc/rc_modes.h"
-#include "fc/runtime_config.h"
#include "fc/rc_adjustments.h"
+#include "fc/runtime_config.h"
#include "flight/imu.h"
-#include "flight/motors.h"
+#include "flight/mixer.h"
#include "flight/position.h"
#include "flight/governor.h"
@@ -65,12 +62,11 @@
#include "rx/crsf.h"
#include "rx/crsf_protocol.h"
-#include "scheduler/scheduler.h"
-
#include "sensors/battery.h"
#include "sensors/sensors.h"
-#include "sensors/adcinternal.h"
-#include "sensors/esc_sensor.h"
+#include "sensors/acceleration.h"
+
+#include "scheduler/scheduler.h"
#include "telemetry/telemetry.h"
#include "telemetry/msp_shared.h"
@@ -78,39 +74,138 @@
#include "crsf.h"
-#define CRSF_CYCLETIME_US 100000 // 100ms, 10 Hz
#define CRSF_DEVICEINFO_VERSION 0x01
#define CRSF_DEVICEINFO_PARAMETER_COUNT 0
-#define CRSF_MSP_BUFFER_SIZE 96
-#define CRSF_MSP_LENGTH_OFFSET 1
+#define CRSF_MSP_BUFFER_SIZE 128
+#define CRSF_MSP_LENGTH_OFFSET 1
-static bool crsfTelemetryEnabled;
-static bool deviceInfoReplyPending;
-static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX];
+#define CRSF_HEARTBEAT_RATE 10
+#define CRSF_HEARTBEAT_PERIOD (1000000 / CRSF_HEARTBEAT_RATE)
-#if defined(USE_MSP_OVER_TELEMETRY)
-typedef struct mspBuffer_s {
- uint8_t bytes[CRSF_MSP_BUFFER_SIZE];
- int len;
-} mspBuffer_t;
+enum {
+ TELEMETRY_STATE_OFF = 0,
+ TELEMETRY_STATE_NATIVE,
+ TELEMETRY_STATE_CUSTOM,
+ TELEMETRY_STATE_POPULATE,
+};
+
+static uint8_t crsfTelemetryState;
+
+#if defined(USE_CRSF_V3)
+static float crsfHeartBeatRateBucket;
+#endif
+
+static float crsfTelemetryRateBucket;
+static float crsfTelemetryRateQuanta;
+
+static uint8_t crsfTelemetryFrameId;
+static timeUs_t crsfTelemetryUpdateTime;
+
+static bool deviceInfoReplyPending = false;
+
+static sbuf_t crsfSbuf;
+static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX + 32];
+
+
+bool checkCrsfTelemetryState(void)
+{
+ return (crsfTelemetryState != TELEMETRY_STATE_OFF);
+}
+
+static inline size_t crsfLinkFrameSlots(size_t bytes)
+{
+ // Telemetry data is send in 5 byte slots, with 1 slot overhead
+ return (bytes + 9) / 5;
+}
+
+static inline void crsfTelemetryRateConsume(size_t slots)
+{
+ crsfTelemetryRateBucket -= slots;
+#if defined(USE_CRSF_V3)
+ crsfHeartBeatRateBucket -= 1;
+#endif
+}
-static mspBuffer_t mspRxBuffer;
+static void crsfTelemetryRateUpdate(timeUs_t currentTimeUs)
+{
+ timeDelta_t delta = cmpTimeUs(currentTimeUs, crsfTelemetryUpdateTime);
#if defined(USE_CRSF_V3)
+ crsfHeartBeatRateBucket += (1e-6f * CRSF_HEARTBEAT_RATE) * delta;
+ crsfHeartBeatRateBucket = constrainf(crsfHeartBeatRateBucket, -2, 1);
+#endif
+ crsfTelemetryRateBucket += crsfTelemetryRateQuanta * delta;
+ crsfTelemetryRateBucket = constrainf(crsfTelemetryRateBucket, -25, 1);
+
+ crsfTelemetryUpdateTime = currentTimeUs;
+
+ telemetryScheduleUpdate(currentTimeUs);
+}
+
+static bool crsfCanTransmitTelemetry(void)
+{
+ return (crsfTelemetryRateBucket >= 0) &&
+ !(getArmingDisableFlags() & ARMING_DISABLED_BOOT_GRACE_TIME);
+}
+
+static size_t crsfSbufLen(sbuf_t *buf)
+{
+ return buf->ptr - crsfFrame;
+}
+
+static sbuf_t * crsfInitializeSbuf(void)
+{
+ sbuf_t * dst = &crsfSbuf;
+
+ dst->ptr = crsfFrame;
+ dst->end = crsfFrame + CRSF_FRAME_SIZE_MAX;
+
+ sbufWriteU8(dst, CRSF_SYNC_BYTE);
+ sbufWriteU8(dst, CRSF_FRAME_LENGTH_TYPE_CRC); // placeholder
+
+ return dst;
+}
+
+static size_t crsfTransmitSbuf(sbuf_t *dst)
+{
+ // Frame length including CRC
+ const size_t frameLength = crsfSbufLen(dst) + 1;
+
+ if (frameLength <= CRSF_FRAME_SIZE_MAX)
+ {
+ // Set frame length (without device address and frame length) into the placeholder
+ crsfFrame[1] = frameLength - 2;
+
+ // CRC does not include device address and frame length
+ crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]);
+
+ // Transmit the telemetry frame to the receiver
+ crsfRxTransmitTelemetryData(crsfFrame, frameLength);
+
+ // Consume telemetry rate
+ crsfTelemetryRateConsume(crsfLinkFrameSlots(frameLength));
+
+ return frameLength;
+ }
-#define CRSF_TELEMETRY_FRAME_INTERVAL_MAX_US 20000 // 20ms
+ return 0;
+}
+
+#if defined(USE_CRSF_V3)
static bool isCrsfV3Running = false;
+
typedef struct {
- uint8_t hasPendingReply:1;
- uint8_t isNewSpeedValid:1;
+ bool hasPendingReply;
+ bool isNewSpeedValid;
uint8_t portID:3;
uint8_t index;
- uint32_t confirmationTime;
-} crsfSpeedControl_s;
+ timeUs_t confirmationTime;
+} crsfSpeedControl_t;
+
+static crsfSpeedControl_t crsfSpeed = INIT_ZERO;
-static crsfSpeedControl_s crsfSpeed = {0};
uint32_t getCrsfCachedBaudrate(void)
{
@@ -126,7 +221,7 @@ uint32_t getCrsfCachedBaudrate(void)
bool checkCrsfCustomizedSpeed(void)
{
- return crsfSpeed.index < BAUD_COUNT ? true : false;
+ return crsfSpeed.index < BAUD_COUNT;
}
uint32_t getCrsfDesiredSpeed(void)
@@ -148,380 +243,196 @@ bool crsfBaudNegotiationInProgress(void)
{
return crsfSpeed.hasPendingReply || crsfSpeed.isNewSpeedValid;
}
-#endif
-void initCrsfMspBuffer(void)
+#endif /* USE_CRSF_V3 */
+
+
+#if defined(USE_MSP_OVER_TELEMETRY)
+
+static bool mspRespPending = false;
+
+static uint8_t mspRequestOriginID = 0;
+
+static uint8_t mspRequestDataBuffer[CRSF_MSP_BUFFER_SIZE];
+
+static int mspRequestDataLength = 0;
+
+
+static void crsfSendMspResponse(uint8_t *payload, const uint8_t payloadSize)
+{
+ sbuf_t *dst = crsfInitializeSbuf();
+ sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP);
+ sbufWriteU8(dst, mspRequestOriginID);
+ sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
+ sbufWriteData(dst, payload, payloadSize);
+ crsfTransmitSbuf(dst);
+ crsfTelemetryRateConsume(2);
+}
+
+void crsfScheduleMspResponse(uint8_t requestOriginID)
{
- mspRxBuffer.len = 0;
+ mspRequestOriginID = requestOriginID;
}
bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength)
{
- if (mspRxBuffer.len + CRSF_MSP_LENGTH_OFFSET + frameLength > CRSF_MSP_BUFFER_SIZE) {
- return false;
- } else {
- uint8_t *p = mspRxBuffer.bytes + mspRxBuffer.len;
+ if (mspRequestDataLength + CRSF_MSP_LENGTH_OFFSET + frameLength <= CRSF_MSP_BUFFER_SIZE) {
+ uint8_t *p = mspRequestDataBuffer + mspRequestDataLength;
*p++ = frameLength;
memcpy(p, frameStart, frameLength);
- mspRxBuffer.len += CRSF_MSP_LENGTH_OFFSET + frameLength;
+ mspRequestDataLength += CRSF_MSP_LENGTH_OFFSET + frameLength;
return true;
}
+ return false;
}
bool handleCrsfMspFrameBuffer(mspResponseFnPtr responseFn)
{
- static bool replyPending = false;
- if (replyPending) {
- if (crsfRxIsTelemetryBufEmpty()) {
- replyPending = sendMspReply(CRSF_FRAME_TX_MSP_FRAME_SIZE, responseFn);
- }
- return replyPending;
- }
- if (!mspRxBuffer.len) {
- return false;
- }
- int pos = 0;
- while (true) {
- const uint8_t mspFrameLength = mspRxBuffer.bytes[pos];
- if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength, NULL)) {
- if (crsfRxIsTelemetryBufEmpty()) {
- replyPending = sendMspReply(CRSF_FRAME_TX_MSP_FRAME_SIZE, responseFn);
- } else {
- replyPending = true;
+ if (!mspRespPending) {
+ bool loop = true;
+ int pos = 0;
+
+ do {
+ ATOMIC_BLOCK(NVIC_PRIO_SERIALUART1) {
+ loop = (pos < mspRequestDataLength);
+ }
+ if (loop) {
+ const uint8_t mspFrameLength = mspRequestDataBuffer[pos];
+ mspRespPending = handleMspFrame(&mspRequestDataBuffer[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength, NULL);
+ pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;
}
}
- pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;
+ while (loop && !mspRespPending);
+
ATOMIC_BLOCK(NVIC_PRIO_SERIALUART1) {
- if (pos >= mspRxBuffer.len) {
- mspRxBuffer.len = 0;
- return replyPending;
- }
+ mspRequestDataLength = 0;
}
}
- return replyPending;
-}
-#endif
-static void crsfInitializeFrame(sbuf_t *dst)
-{
- dst->ptr = crsfFrame;
- dst->end = ARRAYEND(crsfFrame);
+ if (mspRespPending) {
+ mspRespPending = sendMspReply(CRSF_FRAME_TX_MSP_FRAME_SIZE, responseFn);
+ return true;
+ }
- sbufWriteU8(dst, CRSF_SYNC_BYTE);
+ return false;
}
+#endif /* USE_MSP_OVER_TELEMETRY */
-static void crsfFinalize(sbuf_t *dst)
-{
- crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
- sbufSwitchToReader(dst, crsfFrame);
- // write the telemetry frame to the receiver.
- crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
-}
/*
-CRSF frame has the structure:
-
-Device address: (uint8_t)
-Frame length: length in bytes including Type (uint8_t)
-Type: (uint8_t)
-CRC: (uint8_t), crc of and
-*/
+ * CRSF frame has the structure:
+ *
+ * Device address: (uint8_t)
+ * Frame length: length in bytes including Type (uint8_t)
+ * Type: (uint8_t)
+ * CRC: (uint8_t), crc of and
+ */
/*
-0x02 GPS
-Payload:
-int32_t Latitude ( degree / 10`000`000 )
-int32_t Longitude (degree / 10`000`000 )
-uint16_t Groundspeed ( km/h / 10 )
-uint16_t GPS heading ( degree / 100 )
-uint16 Altitude ( meter 1000m offset )
-uint8_t Satellites in use ( counter )
-*/
-
-static int getVoltageMeter(voltageMeterId_e id)
-{
- voltageMeter_t meter;
-
- voltageMeterRead(id, &meter);
-
- // Use ratio 200 in EdgeTx 2.9.3 and 20 in earlier versions
- // Max voltage 25.5V
- return meter.voltage * 255 / 200;
-}
-
-static int16_t crsfGpsReuse(uint8_t reuse, int16_t value)
-{
- escSensorData_t *escData;
-
- switch (reuse) {
- case CRSF_GPS_REUSE_NONE:
- return value;
- case CRSF_GPS_REUSE_HEADSPEED:
- return getHeadSpeed();
- case CRSF_GPS_REUSE_THROTTLE:
- return getGovernorOutput() * 1000;
- case CRSF_GPS_REUSE_ESC_TEMP:
- escData = getEscSensorData(ESC_SENSOR_COMBINED);
- return (escData) ? escData->temperature : 0;
- case CRSF_GPS_REUSE_ESC_PWM:
- escData = getEscSensorData(ESC_SENSOR_COMBINED);
- return (escData) ? escData->pwm : 0;
- case CRSF_GPS_REUSE_ESC_THROTTLE:
- escData = getEscSensorData(ESC_SENSOR_COMBINED);
- return (escData) ? escData->throttle : 0;
- case CRSF_GPS_REUSE_ESC_BEC_VOLTAGE:
- escData = getEscSensorData(ESC_SENSOR_COMBINED);
- return (escData) ? escData->bec_voltage : 0;
- case CRSF_GPS_REUSE_ESC_BEC_CURRENT:
- escData = getEscSensorData(ESC_SENSOR_COMBINED);
- return (escData) ? escData->bec_current : 0;
- case CRSF_GPS_REUSE_ESC_BEC_TEMP:
- escData = getEscSensorData(ESC_SENSOR_COMBINED);
- return (escData) ? escData->temperature2 : 0;
- case CRSF_GPS_REUSE_ESC_STATUS:
- escData = getEscSensorData(ESC_SENSOR_COMBINED);
- return (escData) ? (escData->status & 0xFFFF) : 0;
- case CRSF_GPS_REUSE_ESC_STATUS2:
- escData = getEscSensorData(ESC_SENSOR_COMBINED);
- return (escData) ? (escData->status >> 16) : 0;
- case CRSF_GPS_REUSE_MCU_TEMP:
- return getCoreTemperatureCelsius() * 10;
- case CRSF_GPS_REUSE_MCU_LOAD:
- return getAverageCPULoad();
- case CRSF_GPS_REUSE_SYS_LOAD:
- return getAverageSystemLoad();
- case CRSF_GPS_REUSE_RT_LOAD:
- return getMaxRealTimeLoad();
- case CRSF_GPS_REUSE_BEC_VOLTAGE:
- return getVoltageMeter(VOLTAGE_METER_ID_BEC);
- case CRSF_GPS_REUSE_BUS_VOLTAGE:
- return getVoltageMeter(VOLTAGE_METER_ID_BUS);
- case CRSF_GPS_REUSE_MCU_VOLTAGE:
- return getVoltageMeter(VOLTAGE_METER_ID_MCU);
- }
-
- return 0;
-}
-
-static int16_t crsfGpsAltitudeReuse(uint8_t reuse, int32_t altitude)
-{
- escSensorData_t *escData;
-
- switch (reuse) {
- case CRSF_GPS_REUSE_NONE:
- return constrain(altitude / 100, -1000, 5000); // constrain altitude from -1000 to 5,000m
- case CRSF_GPS_REUSE_HEADSPEED:
- return getHeadSpeed();
- case CRSF_GPS_REUSE_THROTTLE:
- return getGovernorOutput() * 100;
- case CRSF_GPS_REUSE_ESC_TEMP:
- escData = getEscSensorData(ESC_SENSOR_COMBINED);
- return (escData) ? escData->temperature / 10 : 0;
- case CRSF_GPS_REUSE_MCU_TEMP:
- return getCoreTemperatureCelsius();
- case CRSF_GPS_REUSE_MCU_LOAD:
- return getAverageCPULoadPercent();
- case CRSF_GPS_REUSE_SYS_LOAD:
- return getAverageSystemLoadPercent();
- case CRSF_GPS_REUSE_RT_LOAD:
- return getMaxRealTimeLoadPercent();
- }
-
- return 0;
+ * 0x02 GPS
+ * Payload:
+ * int32_t Latitude ( degree / 10`000`000 )
+ * int32_t Longitude (degree / 10`000`000 )
+ * uint16_t Groundspeed ( km/h / 10 )
+ * uint16_t GPS heading ( degree / 100 )
+ * uint16 Altitude ( meter 1000m offset )
+ * uint8_t Satellites in use ( counter )
+ */
+static void crsfFrameGps(sbuf_t *dst)
+{
+ sbufWriteU8(dst, CRSF_FRAMETYPE_GPS);
+ sbufWriteS32BE(dst, gpsSol.llh.lat);
+ sbufWriteS32BE(dst, gpsSol.llh.lon);
+ sbufWriteU16BE(dst, (gpsSol.groundSpeed * 36 + 50) / 100); // cm/s
+ sbufWriteU16BE(dst, gpsSol.groundCourse * 10); // degrees * 10
+ sbufWriteU16BE(dst, getEstimatedAltitudeCm() / 100 + 1000);
+ sbufWriteU8(dst, gpsSol.numSat);
}
-static int8_t crsfGpsSatsReuse(uint8_t reuse, int8_t value)
-{
- escSensorData_t *escData;
-
- switch (reuse) {
- case CRSF_GPS_SATS_REUSE_NONE:
- return value;
- case CRSF_GPS_SATS_REUSE_ESC_TEMP:
- escData = getEscSensorData(ESC_SENSOR_COMBINED);
- return (escData) ? MAX(escData->temperature, 0) / 10 : 0;
- case CRSF_GPS_SATS_REUSE_MCU_TEMP:
- return MAX(getCoreTemperatureCelsius(), 0);
- case CRSF_GPS_SATS_REUSE_PROFILE:
- return getCurrentPidProfileIndex() + 1;
- case CRSF_GPS_SATS_REUSE_RATE_PROFILE:
- return getCurrentControlRateProfileIndex() + 1;
- case CRSF_GPS_SATS_REUSE_MODEL_ID:
- return pilotConfig()->modelId;
- case CRSF_GPS_SATS_REUSE_LED_PROFILE:
-#ifdef USE_LED_STRIP
- return getLedProfile() + 1;
-#else
- return 0;
-#endif
- }
-
- return 0;
+/*
+ * 0x07 Variometer sensor
+ * Payload:
+ * int16_t Variometer
+*/
+static void crsfFrameVarioSensor(sbuf_t *dst)
+{
+ sbufWriteU8(dst, CRSF_FRAMETYPE_VARIO_SENSOR);
+ sbufWriteS16BE(dst, getEstimatedVarioCms());
}
-void crsfFrameGps(sbuf_t *dst)
+/*
+ * 0x08 Battery sensor
+ * Payload:
+ * uint16_t Voltage (100mV steps)
+ * uint16_t Current (100mA steps)
+ * uint24_t Fuel (drawn mAh)
+ * uint8_t Battery remaining (percent)
+*/
+static void crsfFrameBatterySensor(sbuf_t *dst)
{
- // use sbufWrite since CRC does not include frame length
- sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
- sbufWriteU8(dst, CRSF_FRAMETYPE_GPS);
- sbufWriteU32BigEndian(dst, gpsSol.llh.lat); // CRSF and betaflight use same units for degrees
- sbufWriteU32BigEndian(dst, gpsSol.llh.lon);
- sbufWriteU16BigEndian(dst, crsfGpsReuse(telemetryConfig()->crsf_gps_ground_speed_reuse,
- (gpsSol.groundSpeed * 36 + 50) / 100)); // gpsSol.groundSpeed is in cm/s
- sbufWriteU16BigEndian(dst, crsfGpsReuse(telemetryConfig()->crsf_gps_heading_reuse,
- gpsSol.groundCourse * 10)); // gpsSol.groundCourse is degrees * 10
- sbufWriteU16BigEndian(dst, crsfGpsAltitudeReuse(telemetryConfig()->crsf_gps_altitude_reuse,
- getEstimatedAltitudeCm()) + 1000);
- sbufWriteU8(dst, crsfGpsSatsReuse(telemetryConfig()->crsf_gps_sats_reuse, gpsSol.numSat));
+ sbufWriteU8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR);
+ sbufWriteU16BE(dst, getLegacyBatteryVoltage());
+ sbufWriteU16BE(dst, getLegacyBatteryCurrent());
+ sbufWriteU24BE(dst, getBatteryCapacityUsed());
+ sbufWriteU8(dst, calculateBatteryPercentageRemaining());
}
/*
-0x08 Battery sensor
-Payload:
-uint16_t Voltage ( mV * 100 )
-uint16_t Current ( mA * 100 )
-uint24_t Fuel ( drawn mAh )
-uint8_t Battery remaining ( percent )
+ * 0x09 Baro altitude sensor
+ * Payload:
+ * uint16_t Altitude (dm)
+ * int16_t Variometer
*/
-void crsfFrameBatterySensor(sbuf_t *dst)
+static void crsfFrameAltitudeSensor(sbuf_t *dst)
{
- // use sbufWrite since CRC does not include frame length
- sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
- sbufWriteU8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR);
- if (telemetryConfig()->report_cell_voltage) {
- sbufWriteU16BigEndian(dst, (getBatteryAverageCellVoltage() + 5) / 10); // vbat is in units of 0.01V
- } else {
- sbufWriteU16BigEndian(dst, getLegacyBatteryVoltage());
- }
- sbufWriteU16BigEndian(dst, getLegacyBatteryCurrent());
- const uint32_t mAhDrawn = getBatteryCapacityUsed();
- const uint8_t batteryRemainingPercentage = calculateBatteryPercentageRemaining();
- sbufWriteU8(dst, (mAhDrawn >> 16));
- sbufWriteU8(dst, (mAhDrawn >> 8));
- sbufWriteU8(dst, (uint8_t)mAhDrawn);
- sbufWriteU8(dst, batteryRemainingPercentage);
+ sbufWriteU8(dst, CRSF_FRAMETYPE_ALTITUDE_SENSOR);
+ sbufWriteU16BE(dst, getEstimatedAltitudeCm() / 10 + 10000);
+ sbufWriteS16BE(dst, getEstimatedVarioCms());
}
/*
-0x0B Heartbeat
-Payload:
-int16_t origin_add ( Origin Device address )
+ * 0x0B Heartbeat
+ * Payload:
+ * int16_t Origin Device address
*/
-void crsfFrameHeartbeat(sbuf_t *dst)
+static void crsfFrameHeartbeat(sbuf_t *dst)
{
- sbufWriteU8(dst, CRSF_FRAME_HEARTBEAT_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
sbufWriteU8(dst, CRSF_FRAMETYPE_HEARTBEAT);
- sbufWriteU16BigEndian(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
-}
-
-typedef enum {
- CRSF_ACTIVE_ANTENNA1 = 0,
- CRSF_ACTIVE_ANTENNA2 = 1
-} crsfActiveAntenna_e;
-
-typedef enum {
- CRSF_RF_MODE_4_HZ = 0,
- CRSF_RF_MODE_50_HZ = 1,
- CRSF_RF_MODE_150_HZ = 2
-} crsrRfMode_e;
-
-typedef enum {
- CRSF_RF_POWER_0_mW = 0,
- CRSF_RF_POWER_10_mW = 1,
- CRSF_RF_POWER_25_mW = 2,
- CRSF_RF_POWER_100_mW = 3,
- CRSF_RF_POWER_500_mW = 4,
- CRSF_RF_POWER_1000_mW = 5,
- CRSF_RF_POWER_2000_mW = 6,
- CRSF_RF_POWER_250_mW = 7,
- CRSF_RF_POWER_50_mW = 8
-} crsrRfPower_e;
+ sbufWriteU16BE(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
+}
/*
-0x1E Attitude
-Payload:
-int16_t Pitch angle ( rad / 10000 )
-int16_t Roll angle ( rad / 10000 )
-int16_t Yaw angle ( rad / 10000 )
-*/
+ * 0x1E Attitude
+ * Payload:
+ * int16_t Pitch angle (rad / 10000)
+ * int16_t Roll angle (rad / 10000)
+ * int16_t Yaw angle (rad / 10000)
+ */
-// convert andgle in decidegree to radians/10000 with reducing angle to +/-180 degree range
+// convert angle in decidegree to radians/10000 with +/-180 degree range
static int16_t decidegrees2Radians10000(int16_t angle_decidegree)
{
- while (angle_decidegree > 1800) {
+ while (angle_decidegree > 1800)
angle_decidegree -= 3600;
- }
- while (angle_decidegree < -1800) {
+ while (angle_decidegree < -1800)
angle_decidegree += 3600;
- }
- return (int16_t)(RAD * 1000.0f * angle_decidegree);
-}
-
-static int16_t crsfAttitudeReuse(uint8_t reuse, int attitude)
-{
- escSensorData_t *escData;
-
- switch (reuse) {
- case CRSF_ATT_REUSE_NONE:
- return decidegrees2Radians10000(attitude);
- case CRSF_ATT_REUSE_THROTTLE:
- return getGovernorOutput() * 10000;
- case CRSF_ATT_REUSE_ESC_TEMP:
- escData = getEscSensorData(ESC_SENSOR_COMBINED);
- return (escData) ? escData->temperature * 10 : 0;
- case CRSF_ATT_REUSE_ESC_PWM:
- escData = getEscSensorData(ESC_SENSOR_COMBINED);
- return (escData) ? escData->pwm : 0;
- case CRSF_ATT_REUSE_ESC_BEC_VOLTAGE:
- escData = getEscSensorData(ESC_SENSOR_COMBINED);
- return (escData) ? escData->bec_voltage : 0;
- case CRSF_ATT_REUSE_ESC_BEC_CURRENT:
- escData = getEscSensorData(ESC_SENSOR_COMBINED);
- return (escData) ? escData->bec_current : 0;
- case CRSF_ATT_REUSE_ESC_BEC_TEMP:
- escData = getEscSensorData(ESC_SENSOR_COMBINED);
- return (escData) ? escData->temperature2 * 10 : 0;
- case CRSF_ATT_REUSE_ESC_STATUS:
- escData = getEscSensorData(ESC_SENSOR_COMBINED);
- return (escData) ? (escData->status & 0xFFFF) : 0;
- case CRSF_ATT_REUSE_ESC_STATUS2:
- escData = getEscSensorData(ESC_SENSOR_COMBINED);
- return (escData) ? (escData->status >> 16) : 0;
- case CRSF_ATT_REUSE_MCU_TEMP:
- return getCoreTemperatureCelsius() * 100;
- case CRSF_ATT_REUSE_MCU_LOAD:
- return getAverageCPULoad() * 10;
- case CRSF_ATT_REUSE_SYS_LOAD:
- return getAverageSystemLoad() * 10;
- case CRSF_ATT_REUSE_RT_LOAD:
- return getMaxRealTimeLoad() * 10;
- case CRSF_ATT_REUSE_BEC_VOLTAGE:
- return getVoltageMeter(VOLTAGE_METER_ID_BEC);
- case CRSF_ATT_REUSE_BUS_VOLTAGE:
- return getVoltageMeter(VOLTAGE_METER_ID_BUS);
- case CRSF_ATT_REUSE_MCU_VOLTAGE:
- return getVoltageMeter(VOLTAGE_METER_ID_MCU);
- }
-
- return 0;
+ return RAD * 1000 * angle_decidegree;
}
// fill dst buffer with crsf-attitude telemetry frame
void crsfFrameAttitude(sbuf_t *dst)
{
- sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
sbufWriteU8(dst, CRSF_FRAMETYPE_ATTITUDE);
- sbufWriteU16BigEndian(dst, crsfAttitudeReuse(telemetryConfig()->crsf_att_pitch_reuse, attitude.values.pitch));
- sbufWriteU16BigEndian(dst, crsfAttitudeReuse(telemetryConfig()->crsf_att_roll_reuse, attitude.values.roll));
- sbufWriteU16BigEndian(dst, crsfAttitudeReuse(telemetryConfig()->crsf_att_yaw_reuse, attitude.values.yaw));
+ sbufWriteS16BE(dst, decidegrees2Radians10000(attitude.values.pitch));
+ sbufWriteS16BE(dst, decidegrees2Radians10000(attitude.values.roll));
+ sbufWriteS16BE(dst, decidegrees2Radians10000(attitude.values.yaw));
}
/*
-0x21 Flight mode text based
-Payload:
-char[] Flight mode ( Null terminated string )
-*/
-
+ * 0x21 Flight mode text based
+ * Payload:
+ * char[] Flight mode (Null terminated string)
+ */
static void crsfFlightModeInfo(char *buf)
{
const char *flightMode = "-";
@@ -567,7 +478,7 @@ static const char * govStateNames[] = {
"BAILOUT",
};
-static void crsfGovernorInfo(char *buf)
+void crsfGovernorInfo(char *buf)
{
// Modes that are only relevant when disarmed
if (!ARMING_FLAG(ARMED)) {
@@ -581,175 +492,357 @@ static void crsfGovernorInfo(char *buf)
}
}
-static void crsfHeadspeedInfo(char *buf)
+void crsfFrameFlightMode(sbuf_t *dst)
{
- int val = getHeadSpeed();
- tfp_sprintf(buf, "%d", val);
+ char buff[32] = INIT_ZERO;
+
+ crsfFlightModeInfo(buff);
+ //crsfGovernorInfo(buff);
+
+ sbufWriteU8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
+ sbufWriteStringWithZeroTerminator(dst, buff);
}
-static void crsfThrottleInfo(char *buf)
+/*
+ * 0x29 Device Info
+ * Payload:
+ * uint8_t Destination
+ * uint8_t Origin
+ * char[] Device Name (Null terminated string)
+ * uint32_t Null Bytes
+ * uint32_t Null Bytes
+ * uint32_t Null Bytes
+ * uint8_t 255 (Max MSP Parameter)
+ * uint8_t 0x01 (Parameter version 1)
+ */
+static void crsfFrameDeviceInfo(sbuf_t *dst)
{
- int val = lrintf(getGovernorOutput() * 100);
- tfp_sprintf(buf, "%d", val);
+ char buff[30];
+
+ tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, systemConfig()->boardIdentifier);
+
+ sbufWriteU8(dst, CRSF_FRAMETYPE_DEVICE_INFO);
+ sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
+ sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
+ sbufWriteStringWithZeroTerminator(dst, buff);
+ sbufWriteU32(dst, 0);
+ sbufWriteU32(dst, 0);
+ sbufWriteU32(dst, 0);
+ sbufWriteU8(dst, CRSF_DEVICEINFO_PARAMETER_COUNT);
+ sbufWriteU8(dst, CRSF_DEVICEINFO_VERSION);
}
-static void crsfMCUTempInfo(char *buf)
+
+/*
+ * 0x88 Custom telemetry
+ * Payload:
+ * uint16_t Sensor id
+ * Sensor data
+ * ...
+ */
+
+void crsfSensorEncodeNil(sbuf_t *buf, telemetrySensor_t *sensor)
{
- int val = getCoreTemperatureCelsius();
- tfp_sprintf(buf, "%d", val);
+ UNUSED(buf);
+ UNUSED(sensor);
}
-static void crsfMCULoadInfo(char *buf)
+void crsfSensorEncodeU8(sbuf_t *buf, telemetrySensor_t *sensor)
{
- int val = getAverageCPULoadPercent();
- tfp_sprintf(buf, "%d", val);
+ sbufWriteU8(buf, constrain(sensor->value, 0, 0xFF));
}
-static void crsfSysLoadInfo(char *buf)
+void crsfSensorEncodeS8(sbuf_t *buf, telemetrySensor_t *sensor)
{
- int val = getAverageSystemLoadPercent();
- tfp_sprintf(buf, "%d", val);
+ sbufWriteS8(buf, constrain(sensor->value, -0x80, 0x7F));
}
-static void crsfRTLoadInfo(char *buf)
+void crsfSensorEncodeU16(sbuf_t *buf, telemetrySensor_t *sensor)
{
- int val = getMaxRealTimeLoadPercent();
- tfp_sprintf(buf, "%d", val);
+ sbufWriteU16BE(buf, constrain(sensor->value, 0, 0xFFFF));
}
-static void crsfESCTempInfo(char *buf)
+void crsfSensorEncodeS16(sbuf_t *buf, telemetrySensor_t *sensor)
{
- escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
- if (escData) {
- int val = escData->temperature / 10;
- tfp_sprintf(buf, "%d", val);
- }
+ sbufWriteS16BE(buf, constrain(sensor->value, -0x8000, 0x7FFF));
}
-static void crsfVoltageMetereInfo(char *buf, voltageMeterId_e id)
+void crsfSensorEncodeU24(sbuf_t *buf, telemetrySensor_t *sensor)
{
- voltageMeter_t meter;
+ sbufWriteU24BE(buf, constrain(sensor->value, 0, 0xFFFFFF));
+}
- if (voltageMeterRead(id, &meter)) {
- int val = meter.voltage / 10;
- tfp_sprintf(buf, "%d.%02d", val / 100, val % 100);
- }
+void crsfSensorEncodeS24(sbuf_t *buf, telemetrySensor_t *sensor)
+{
+ sbufWriteS24BE(buf, constrain(sensor->value, -0x800000, 0x7FFFFF));
}
-static void crsfAdjFuncInfo(char *buf)
+void crsfSensorEncodeU32(sbuf_t *buf, telemetrySensor_t *sensor)
{
- if (getAdjustmentsRangeName()) {
- int fun = getAdjustmentsRangeFunc();
- int val = getAdjustmentsRangeValue();
- tfp_sprintf(buf, "%d:%d", fun, val);
+ sbufWriteU32BE(buf, sensor->value);
+}
+
+void crsfSensorEncodeS32(sbuf_t *buf, telemetrySensor_t *sensor)
+{
+ sbufWriteS32BE(buf, sensor->value);
+}
+
+void crsfSensorEncodeCellVolt(sbuf_t *buf, telemetrySensor_t *sensor)
+{
+ const int volt = constrain(sensor->value, 200, 455) - 200;
+ sbufWriteU8(buf, volt);
+}
+
+void crsfSensorEncodeCells(sbuf_t *buf, telemetrySensor_t *sensor)
+{
+ UNUSED(sensor);
+ const int cells = MIN(getBatteryCellCount(), 16);
+ sbufWriteU8(buf, cells);
+ for (int i = 0; i < cells; i++) {
+ int volt = constrain(getBatteryCellVoltage(i), 200, 455) - 200;
+ sbufWriteU8(buf, volt);
}
}
-static void crsfGovAdjFuncInfo(char *buf)
+void crsfSensorEncodeControl(sbuf_t *buf, telemetrySensor_t *sensor)
+{
+ UNUSED(sensor);
+ const int p = lrintf(mixerGetInput(MIXER_IN_STABILIZED_PITCH) * 1200);
+ const int r = lrintf(mixerGetInput(MIXER_IN_STABILIZED_ROLL) * 1200);
+ const int y = lrintf(mixerGetInput(MIXER_IN_STABILIZED_YAW) * 2400);
+ const int c = lrintf(mixerGetInput(MIXER_IN_STABILIZED_COLLECTIVE) * 1200);
+ sbufWriteU8(buf, ((p >> 8) & 0x0F) | ((r >> 4) & 0xF0));
+ sbufWriteU8(buf, (p & 0xFF));
+ sbufWriteU8(buf, (r & 0xFF));
+ sbufWriteU8(buf, ((y >> 8) & 0x0F) | ((c >> 4) & 0xF0));
+ sbufWriteU8(buf, (y & 0xFF));
+ sbufWriteU8(buf, (c & 0xFF));
+}
+
+void crsfSensorEncodeAttitude(sbuf_t *buf, telemetrySensor_t *sensor)
+{
+ UNUSED(sensor);
+ sbufWriteS16BE(buf, attitude.values.pitch);
+ sbufWriteS16BE(buf, attitude.values.roll);
+ sbufWriteS16BE(buf, attitude.values.yaw);
+}
+
+void crsfSensorEncodeAccel(sbuf_t *buf, telemetrySensor_t *sensor)
+{
+ UNUSED(sensor);
+ sbufWriteS16BE(buf, acc.accADC[0] * acc.dev.acc_1G_rec * 100);
+ sbufWriteS16BE(buf, acc.accADC[1] * acc.dev.acc_1G_rec * 100);
+ sbufWriteS16BE(buf, acc.accADC[2] * acc.dev.acc_1G_rec * 100);
+}
+
+void crsfSensorEncodeLatLong(sbuf_t *buf, telemetrySensor_t *sensor)
{
+ UNUSED(sensor);
+ sbufWriteS32BE(buf, gpsSol.llh.lat);
+ sbufWriteS32BE(buf, gpsSol.llh.lon);
+}
+
+void crsfSensorEncodeAdjFunc(sbuf_t *buf, telemetrySensor_t *sensor)
+{
+ UNUSED(sensor);
if (getAdjustmentsRangeName()) {
- int fun = getAdjustmentsRangeFunc();
- int val = getAdjustmentsRangeValue();
- tfp_sprintf(buf, "%d:%d", fun, val);
+ sbufWriteU16BE(buf, getAdjustmentsRangeFunc());
+ sbufWriteS32BE(buf, getAdjustmentsRangeValue());
}
else {
- crsfGovernorInfo(buf);
+ sbufWriteU16BE(buf, 0);
+ sbufWriteS32BE(buf, 0);
}
}
-void crsfFrameFlightMode(sbuf_t *dst)
+static void crsfFrameCustomTelemetryHeader(sbuf_t *dst)
{
- char buff[32] = { 0, };
+ sbufWriteU8(dst, CRSF_FRAMETYPE_CUSTOM_TELEM);
+ sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
+ sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
+ sbufWriteU8(dst, crsfTelemetryFrameId);
+}
- switch (telemetryConfig()->crsf_flight_mode_reuse) {
- case CRSF_FM_REUSE_GOV_STATE:
- crsfGovernorInfo(buff);
- break;
- case CRSF_FM_REUSE_HEADSPEED:
- crsfHeadspeedInfo(buff);
- break;
- case CRSF_FM_REUSE_THROTTLE:
- crsfThrottleInfo(buff);
- break;
- case CRSF_FM_REUSE_ESC_TEMP:
- crsfESCTempInfo(buff);
- break;
- case CRSF_FM_REUSE_MCU_TEMP:
- crsfMCUTempInfo(buff);
- break;
- case CRSF_FM_REUSE_MCU_LOAD:
- crsfMCULoadInfo(buff);
- break;
- case CRSF_FM_REUSE_SYS_LOAD:
- crsfSysLoadInfo(buff);
- break;
- case CRSF_FM_REUSE_RT_LOAD:
- crsfRTLoadInfo(buff);
- break;
- case CRSF_FM_REUSE_BEC_VOLTAGE:
- crsfVoltageMetereInfo(buff, VOLTAGE_METER_ID_BEC);
- break;
- case CRSF_FM_REUSE_BUS_VOLTAGE:
- crsfVoltageMetereInfo(buff, VOLTAGE_METER_ID_BUS);
- break;
- case CRSF_FM_REUSE_MCU_VOLTAGE:
- crsfVoltageMetereInfo(buff, VOLTAGE_METER_ID_MCU);
- break;
- case CRSF_FM_REUSE_ADJFUNC:
- crsfAdjFuncInfo(buff);
- break;
- case CRSF_FM_REUSE_GOV_ADJFUNC:
- crsfGovAdjFuncInfo(buff);
- break;
- default:
- crsfFlightModeInfo(buff);
- break;
+static void crsfFrameCustomTelemetrySensor(sbuf_t *dst, telemetrySensor_t * sensor)
+{
+ sbufWriteU16BE(dst, sensor->tcode);
+ sensor->encode(dst, sensor);
+}
+
+
+#define TLM_SENSOR(NAME, CODE, MINI, MAXI, ENCODER) \
+ { \
+ .telid = TELEM_##NAME, \
+ .tcode = (CODE), \
+ .min_interval = (MINI), \
+ .max_interval = (MAXI), \
+ .bucket = 0, \
+ .value = 0, \
+ .update = 0, \
+ .active = false, \
+ .encode = crsfSensorEncode##ENCODER, \
}
- uint8_t *lengthPtr = sbufPtr(dst);
- sbufWriteU8(dst, 0);
- sbufWriteU8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
- sbufWriteStringWithZeroTerminator(dst, buff);
- *lengthPtr = sbufPtr(dst) - lengthPtr;
-}
+#define LEGACY_FLIGHT_MODE (SENSOR_MODE)
+#define LEGACY_BATTERY (SENSOR_VOLTAGE | SENSOR_CURRENT | SENSOR_FUEL | SENSOR_CAP_USED)
+#define LEGACY_ATTITUDE (SENSOR_PITCH | SENSOR_ROLL | SENSOR_HEADING)
+#define LEGACY_ALTITUDE (SENSOR_ALTITUDE | SENSOR_VARIO)
+#define LEGACY_GPS (SENSOR_LAT_LONG | SENSOR_GROUND_SPEED)
-/*
-0x29 Device Info
-Payload:
-uint8_t Destination
-uint8_t Origin
-char[] Device Name ( Null terminated string )
-uint32_t Null Bytes
-uint32_t Null Bytes
-uint32_t Null Bytes
-uint8_t 255 (Max MSP Parameter)
-uint8_t 0x01 (Parameter version 1)
-*/
-void crsfFrameDeviceInfo(sbuf_t *dst)
+static telemetrySensor_t crsfNativeTelemetrySensors[] =
{
- char buff[30];
- tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, systemConfig()->boardIdentifier);
+ TLM_SENSOR(FLIGHT_MODE, LEGACY_FLIGHT_MODE, 100, 100, Nil),
+ TLM_SENSOR(BATTERY, LEGACY_BATTERY, 100, 100, Nil),
+ TLM_SENSOR(ATTITUDE, LEGACY_ATTITUDE, 100, 100, Nil),
+ TLM_SENSOR(ALTITUDE, LEGACY_ALTITUDE, 100, 100, Nil),
+ TLM_SENSOR(GPS, LEGACY_GPS, 100, 100, Nil),
+};
- uint8_t *lengthPtr = sbufPtr(dst);
- sbufWriteU8(dst, 0);
- sbufWriteU8(dst, CRSF_FRAMETYPE_DEVICE_INFO);
- sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
- sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
- sbufWriteStringWithZeroTerminator(dst, buff);
- for (unsigned int ii = 0; ii < 12; ii++) {
- sbufWriteU8(dst, 0x00);
+static telemetrySensor_t crsfCustomTelemetrySensors[] =
+{
+ TLM_SENSOR(NONE, 0x0000, 1000, 1000, Nil),
+ TLM_SENSOR(HEARTBEAT, 0x0001, 1000, 1000, U16),
+
+ TLM_SENSOR(BATTERY_VOLTAGE, 0x0011, 200, 3000, U16),
+ TLM_SENSOR(BATTERY_CURRENT, 0x0012, 200, 3000, U16),
+ TLM_SENSOR(BATTERY_CONSUMPTION, 0x0013, 200, 3000, U16),
+ TLM_SENSOR(BATTERY_CHARGE_LEVEL, 0x0014, 200, 3000, U8),
+
+ TLM_SENSOR(BATTERY_CELL_COUNT, 0x0020, 200, 3000, U8),
+ TLM_SENSOR(BATTERY_CELL_VOLTAGE, 0x0021, 200, 3000, CellVolt),
+ TLM_SENSOR(BATTERY_CELL_VOLTAGES, 0x002F, 200, 3000, Cells),
+
+ TLM_SENSOR(CONTROL, 0x0030, 100, 3000, Control),
+ TLM_SENSOR(PITCH_CONTROL, 0x0031, 200, 3000, S16),
+ TLM_SENSOR(ROLL_CONTROL, 0x0032, 200, 3000, S16),
+ TLM_SENSOR(YAW_CONTROL, 0x0033, 200, 3000, S16),
+ TLM_SENSOR(COLLECTIVE_CONTROL, 0x0034, 200, 3000, S16),
+ TLM_SENSOR(THROTTLE_CONTROL, 0x0035, 200, 3000, S8),
+
+ TLM_SENSOR(ESC1_VOLTAGE, 0x0041, 200, 3000, U16),
+ TLM_SENSOR(ESC1_CURRENT, 0x0042, 200, 3000, U16),
+ TLM_SENSOR(ESC1_CAPACITY, 0x0043, 200, 3000, U16),
+ TLM_SENSOR(ESC1_ERPM, 0x0044, 200, 3000, U16),
+ TLM_SENSOR(ESC1_POWER, 0x0045, 200, 3000, U16),
+ TLM_SENSOR(ESC1_THROTTLE, 0x0046, 200, 3000, U16),
+ TLM_SENSOR(ESC1_TEMP1, 0x0047, 200, 3000, U8),
+ TLM_SENSOR(ESC1_TEMP2, 0x0048, 200, 3000, U8),
+ TLM_SENSOR(ESC1_BEC_VOLTAGE, 0x0049, 200, 3000, U16),
+ TLM_SENSOR(ESC1_BEC_CURRENT, 0x004A, 200, 3000, U16),
+ TLM_SENSOR(ESC1_STATUS, 0x004E, 200, 3000, U32),
+ TLM_SENSOR(ESC1_MODEL, 0x004F, 200, 3000, U8),
+
+ TLM_SENSOR(ESC2_VOLTAGE, 0x0051, 200, 3000, U16),
+ TLM_SENSOR(ESC2_CURRENT, 0x0052, 200, 3000, U16),
+ TLM_SENSOR(ESC2_CAPACITY, 0x0053, 200, 3000, U16),
+ TLM_SENSOR(ESC2_ERPM, 0x0054, 200, 3000, U16),
+ TLM_SENSOR(ESC2_POWER, 0x0055, 200, 3000, U16),
+ TLM_SENSOR(ESC2_THROTTLE, 0x0056, 200, 3000, U16),
+ TLM_SENSOR(ESC2_TEMP1, 0x0057, 200, 3000, U8),
+ TLM_SENSOR(ESC2_TEMP2, 0x0058, 200, 3000, U8),
+ TLM_SENSOR(ESC2_BEC_VOLTAGE, 0x0059, 200, 3000, U16),
+ TLM_SENSOR(ESC2_BEC_CURRENT, 0x005A, 200, 3000, U16),
+ TLM_SENSOR(ESC2_STATUS, 0x005E, 200, 3000, U32),
+ TLM_SENSOR(ESC2_MODEL, 0x005F, 200, 3000, U8),
+
+ TLM_SENSOR(ESC_VOLTAGE, 0x0080, 200, 3000, U16),
+ TLM_SENSOR(BEC_VOLTAGE, 0x0081, 200, 3000, U16),
+ TLM_SENSOR(BUS_VOLTAGE, 0x0082, 200, 3000, U16),
+ TLM_SENSOR(MCU_VOLTAGE, 0x0083, 200, 3000, U16),
+
+ TLM_SENSOR(ESC_CURRENT, 0x0090, 200, 3000, U16),
+ TLM_SENSOR(BEC_CURRENT, 0x0091, 200, 3000, U16),
+ TLM_SENSOR(BUS_CURRENT, 0x0092, 200, 3000, U16),
+ TLM_SENSOR(MCU_CURRENT, 0x0093, 200, 3000, U16),
+
+ TLM_SENSOR(ESC_TEMP, 0x00A0, 500, 3000, U8),
+ TLM_SENSOR(BEC_TEMP, 0x00A1, 500, 3000, U8),
+ TLM_SENSOR(MCU_TEMP, 0x00A3, 500, 3000, U8),
+
+ TLM_SENSOR(HEADING, 0x00B1, 200, 3000, S16),
+ TLM_SENSOR(ALTITUDE, 0x00B2, 200, 3000, S24),
+ TLM_SENSOR(VARIOMETER, 0x00B3, 200, 3000, S16),
+
+ TLM_SENSOR(HEADSPEED, 0x00C0, 200, 3000, U16),
+ TLM_SENSOR(TAILSPEED, 0x00C1, 200, 3000, U16),
+
+ TLM_SENSOR(ATTITUDE, 0x0100, 100, 3000, Attitude),
+ TLM_SENSOR(ATTITUDE_PITCH, 0x0101, 200, 3000, S16),
+ TLM_SENSOR(ATTITUDE_ROLL, 0x0102, 200, 3000, S16),
+ TLM_SENSOR(ATTITUDE_YAW, 0x0103, 200, 3000, S16),
+
+ TLM_SENSOR(ACCEL, 0x0110, 100, 3000, Accel),
+ TLM_SENSOR(ACCEL_X, 0x0111, 200, 3000, S16),
+ TLM_SENSOR(ACCEL_Y, 0x0112, 200, 3000, S16),
+ TLM_SENSOR(ACCEL_Z, 0x0113, 200, 3000, S16),
+
+ TLM_SENSOR(GPS_SATS, 0x0121, 500, 3000, U8),
+ TLM_SENSOR(GPS_PDOP, 0x0122, 500, 3000, U8),
+ TLM_SENSOR(GPS_HDOP, 0x0123, 500, 3000, U8),
+ TLM_SENSOR(GPS_VDOP, 0x0124, 500, 3000, U8),
+ TLM_SENSOR(GPS_COORD, 0x0125, 200, 3000, LatLong),
+ TLM_SENSOR(GPS_ALTITUDE, 0x0126, 200, 3000, S16),
+ TLM_SENSOR(GPS_HEADING, 0x0127, 200, 3000, S16),
+ TLM_SENSOR(GPS_GROUNDSPEED, 0x0128, 200, 3000, U16),
+ TLM_SENSOR(GPS_HOME_DISTANCE, 0x0129, 200, 3000, U16),
+ TLM_SENSOR(GPS_HOME_DIRECTION, 0x012A, 200, 3000, S16),
+
+ TLM_SENSOR(CPU_LOAD, 0x0141, 500, 3000, U8),
+ TLM_SENSOR(SYS_LOAD, 0x0142, 500, 3000, U8),
+ TLM_SENSOR(RT_LOAD, 0x0143, 500, 3000, U8),
+
+ TLM_SENSOR(MODEL_ID, 0x0200, 200, 3000, U8),
+ TLM_SENSOR(FLIGHT_MODE, 0x0201, 200, 3000, U16),
+ TLM_SENSOR(ARMING_FLAGS, 0x0202, 200, 3000, U8),
+ TLM_SENSOR(ARMING_DISABLE_FLAGS, 0x0203, 200, 3000, U32),
+ TLM_SENSOR(RESCUE_STATE, 0x0204, 200, 3000, U8),
+ TLM_SENSOR(GOVERNOR_STATE, 0x0205, 200, 3000, U8),
+
+ TLM_SENSOR(PID_PROFILE, 0x0211, 200, 3000, U8),
+ TLM_SENSOR(RATES_PROFILE, 0x0212, 200, 3000, U8),
+ TLM_SENSOR(LED_PROFILE, 0x0213, 200, 3000, U8),
+
+ TLM_SENSOR(ADJFUNC, 0x0220, 200, 3000, AdjFunc),
+
+ TLM_SENSOR(DEBUG_0, 0xFE00, 100, 3000, S32),
+ TLM_SENSOR(DEBUG_1, 0xFE01, 100, 3000, S32),
+ TLM_SENSOR(DEBUG_2, 0xFE02, 100, 3000, S32),
+ TLM_SENSOR(DEBUG_3, 0xFE03, 100, 3000, S32),
+ TLM_SENSOR(DEBUG_4, 0xFE04, 100, 3000, S32),
+ TLM_SENSOR(DEBUG_5, 0xFE05, 100, 3000, S32),
+ TLM_SENSOR(DEBUG_6, 0xFE06, 100, 3000, S32),
+ TLM_SENSOR(DEBUG_7, 0xFE07, 100, 3000, S32),
+};
+
+telemetrySensor_t * crsfGetNativeSensor(sensor_id_e id)
+{
+ for (size_t i = 0; i < ARRAYLEN(crsfNativeTelemetrySensors); i++) {
+ telemetrySensor_t * sensor = &crsfNativeTelemetrySensors[i];
+ if (sensor->telid == id)
+ return sensor;
}
- sbufWriteU8(dst, CRSF_DEVICEINFO_PARAMETER_COUNT);
- sbufWriteU8(dst, CRSF_DEVICEINFO_VERSION);
- *lengthPtr = sbufPtr(dst) - lengthPtr;
+
+ return NULL;
+}
+
+telemetrySensor_t * crsfGetCustomSensor(sensor_id_e id)
+{
+ for (size_t i = 0; i < ARRAYLEN(crsfCustomTelemetrySensors); i++) {
+ telemetrySensor_t * sensor = &crsfCustomTelemetrySensors[i];
+ if (sensor->telid == id)
+ return sensor;
+ }
+
+ return NULL;
}
#if defined(USE_CRSF_V3)
-void crsfFrameSpeedNegotiationResponse(sbuf_t *dst, bool reply)
+
+static void crsfFrameSpeedNegotiationResponse(sbuf_t *dst, bool reply)
{
- uint8_t *lengthPtr = sbufPtr(dst);
- sbufWriteU8(dst, 0);
+ uint8_t *start = sbufPtr(dst);
+
sbufWriteU8(dst, CRSF_FRAMETYPE_COMMAND);
sbufWriteU8(dst, CRSF_ADDRESS_CRSF_RECEIVER);
sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
@@ -757,64 +850,76 @@ void crsfFrameSpeedNegotiationResponse(sbuf_t *dst, bool reply)
sbufWriteU8(dst, CRSF_COMMAND_SUBCMD_GENERAL_CRSF_SPEED_RESPONSE);
sbufWriteU8(dst, crsfSpeed.portID);
sbufWriteU8(dst, reply);
- crc8_poly_0xba_sbuf_append(dst, &lengthPtr[1]);
- *lengthPtr = sbufPtr(dst) - lengthPtr;
+
+ crc8_poly_0xba_sbuf_append(dst, start);
}
static void crsfProcessSpeedNegotiationCmd(uint8_t *frameStart)
{
uint32_t newBaudrate = frameStart[2] << 24 | frameStart[3] << 16 | frameStart[4] << 8 | frameStart[5];
- uint8_t ii = 0;
- for (ii = 0; ii < BAUD_COUNT; ++ii) {
- if (newBaudrate == baudRates[ii]) {
+ unsigned index = 0;
+ for (index = 0; index < BAUD_COUNT; index++) {
+ if (newBaudrate == baudRates[index])
break;
- }
}
crsfSpeed.portID = frameStart[1];
- crsfSpeed.index = ii;
+ crsfSpeed.index = index;
}
-void crsfScheduleSpeedNegotiationResponse(void)
+static void crsfScheduleSpeedNegotiationResponse(void)
{
crsfSpeed.hasPendingReply = true;
crsfSpeed.isNewSpeedValid = false;
}
+/* TASK_SPEED_NEGOTIATION @ 100Hz */
void speedNegotiationProcess(timeUs_t currentTimeUs)
{
if (crsfSpeed.hasPendingReply) {
- bool found = ((crsfSpeed.index < BAUD_COUNT) && crsfRxUseNegotiatedBaud()) ? true : false;
- sbuf_t crsfSpeedNegotiationBuf;
- sbuf_t *dst = &crsfSpeedNegotiationBuf;
- crsfInitializeFrame(dst);
+ bool found = (crsfSpeed.index < BAUD_COUNT) && crsfRxUseNegotiatedBaud();
+ sbuf_t *dst = crsfInitializeSbuf();
crsfFrameSpeedNegotiationResponse(dst, found);
- crsfRxSendTelemetryData(); // prevent overwriting previous data
- crsfFinalize(dst);
- crsfRxSendTelemetryData();
+ crsfTransmitSbuf(dst);
crsfSpeed.hasPendingReply = false;
crsfSpeed.isNewSpeedValid = found;
crsfSpeed.confirmationTime = currentTimeUs;
- } else if (crsfSpeed.isNewSpeedValid) {
+ }
+ else if (crsfSpeed.isNewSpeedValid) {
+ // delay 4ms before applying the new baudrate
if (cmpTimeUs(currentTimeUs, crsfSpeed.confirmationTime) >= 4000) {
- // delay 4ms before applying the new baudrate
+ crsfSpeed.confirmationTime = currentTimeUs;
crsfRxUpdateBaudrate(getCrsfDesiredSpeed());
crsfSpeed.isNewSpeedValid = false;
isCrsfV3Running = true;
}
- } else if (!featureIsEnabled(FEATURE_TELEMETRY) && crsfRxUseNegotiatedBaud()) {
+ }
+ else if (!featureIsEnabled(FEATURE_TELEMETRY) && crsfRxUseNegotiatedBaud()) {
// Send heartbeat if telemetry is disabled to allow RX to detect baud rate mismatches
- sbuf_t crsfPayloadBuf;
- sbuf_t *dst = &crsfPayloadBuf;
- crsfInitializeFrame(dst);
- crsfFrameHeartbeat(dst);
- crsfRxSendTelemetryData(); // prevent overwriting previous data
- crsfFinalize(dst);
- crsfRxSendTelemetryData();
+ if (cmpTimeUs(currentTimeUs, crsfSpeed.confirmationTime) >= CRSF_HEARTBEAT_PERIOD) {
+ crsfSpeed.confirmationTime = currentTimeUs;
+ sbuf_t *dst = crsfInitializeSbuf();
+ crsfFrameHeartbeat(dst);
+ crsfTransmitSbuf(dst);
+ }
+ }
+}
+
+void crsfProcessCommand(uint8_t *frameStart)
+{
+ uint8_t cmd = frameStart[0];
+ uint8_t sub = frameStart[1];
+
+ if (cmd == CRSF_COMMAND_SUBCMD_GENERAL && sub == CRSF_COMMAND_SUBCMD_GENERAL_CRSF_SPEED_PROPOSAL) {
+ crsfProcessSpeedNegotiationCmd(&frameStart[1]);
+ crsfScheduleSpeedNegotiationResponse();
}
}
+
#endif
+
#if defined(USE_CRSF_CMS_TELEMETRY)
+
#define CRSF_DISPLAYPORT_MAX_CHUNK_LENGTH 50
#define CRSF_DISPLAYPORT_BATCH_MAX 0x3F
#define CRSF_DISPLAYPORT_FIRST_CHUNK_MASK 0x80
@@ -862,7 +967,8 @@ static void cRleEncodeStream(sbuf_t *source, sbuf_t *dest, uint8_t maxDestLen)
} else {
break;
}
- } else if (destRemaining >= runLength) {
+ }
+ else if (destRemaining >= runLength) {
sbufWriteU8(dest, c);
sbufAdvance(source, runLength);
}
@@ -871,379 +977,397 @@ static void cRleEncodeStream(sbuf_t *source, sbuf_t *dest, uint8_t maxDestLen)
static void crsfFrameDisplayPortChunk(sbuf_t *dst, sbuf_t *src, uint8_t batchId, uint8_t idx)
{
- uint8_t *lengthPtr = sbufPtr(dst);
- sbufWriteU8(dst, 0);
sbufWriteU8(dst, CRSF_FRAMETYPE_DISPLAYPORT_CMD);
sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
sbufWriteU8(dst, CRSF_DISPLAYPORT_SUBCMD_UPDATE);
- uint8_t *metaPtr = sbufPtr(dst);
+
+ uint8_t *ptr = sbufPtr(dst);
sbufWriteU8(dst, batchId);
sbufWriteU8(dst, idx);
cRleEncodeStream(src, dst, CRSF_DISPLAYPORT_MAX_CHUNK_LENGTH);
- if (idx == 0) {
- *metaPtr |= CRSF_DISPLAYPORT_FIRST_CHUNK_MASK;
- }
- if (!sbufBytesRemaining(src)) {
- *metaPtr |= CRSF_DISPLAYPORT_LAST_CHUNK_MASK;
- }
- *lengthPtr = sbufPtr(dst) - lengthPtr;
+
+ if (idx == 0)
+ *ptr |= CRSF_DISPLAYPORT_FIRST_CHUNK_MASK;
+ if (sbufBytesRemaining(src) == 0)
+ *ptr |= CRSF_DISPLAYPORT_LAST_CHUNK_MASK;
}
static void crsfFrameDisplayPortClear(sbuf_t *dst)
{
- uint8_t *lengthPtr = sbufPtr(dst);
- sbufWriteU8(dst, CRSF_DISPLAY_PORT_COLS_MAX + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
sbufWriteU8(dst, CRSF_FRAMETYPE_DISPLAYPORT_CMD);
sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
sbufWriteU8(dst, CRSF_DISPLAYPORT_SUBCMD_CLEAR);
- *lengthPtr = sbufPtr(dst) - lengthPtr;
}
-#endif
-
-// schedule array to decide how often each type of frame is sent
-typedef enum {
- CRSF_FRAME_START_INDEX = 0,
- CRSF_FRAME_ATTITUDE_INDEX = CRSF_FRAME_START_INDEX,
- CRSF_FRAME_BATTERY_SENSOR_INDEX,
- CRSF_FRAME_FLIGHT_MODE_INDEX,
- CRSF_FRAME_GPS_INDEX,
- CRSF_FRAME_HEARTBEAT_INDEX,
- CRSF_SCHEDULE_COUNT_MAX
-} crsfFrameTypeIndex_e;
-
-static uint8_t crsfScheduleCount;
-static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
+static bool crsfSendDisplayPortData(void)
+{
+ static uint8_t displayPortBatchId = 0;
-#if defined(USE_MSP_OVER_TELEMETRY)
+ if (crsfDisplayPortScreen()->reset) {
+ crsfDisplayPortScreen()->reset = false;
+ sbuf_t *dst = crsfInitializeSbuf();
+ crsfFrameDisplayPortClear(dst);
+ crsfTransmitSbuf(dst);
+ return true;
+ }
-static bool mspReplyPending;
-static uint8_t mspRequestOriginID = 0; // origin ID of last msp-over-crsf request. Needed to send response to the origin.
+ if (crsfDisplayPortIsReady() && crsfDisplayPortScreen()->updated)
+ {
+ crsfDisplayPortScreen()->updated = false;
+ uint16_t screenSize = crsfDisplayPortScreen()->rows * crsfDisplayPortScreen()->cols;
+ uint8_t *srcStart = (uint8_t*)crsfDisplayPortScreen()->buffer;
+ uint8_t *srcEnd = (uint8_t*)(crsfDisplayPortScreen()->buffer + screenSize);
+ sbuf_t displayPortSbuf;
+ sbuf_t *src = sbufInit(&displayPortSbuf, srcStart, srcEnd);
+ displayPortBatchId = (displayPortBatchId + 1) % CRSF_DISPLAYPORT_BATCH_MAX;
+ for (uint8_t i = 0; sbufBytesRemaining(src); i++) {
+ sbuf_t *dst = crsfInitializeSbuf();
+ crsfFrameDisplayPortChunk(dst, src, displayPortBatchId, i);
+ crsfTransmitSbuf(dst);
+ }
+ return true;
+ }
-void crsfScheduleMspResponse(uint8_t requestOriginID)
-{
- mspReplyPending = true;
- mspRequestOriginID = requestOriginID;
+ return false;
}
-// sends MSP response chunk over CRSF. Must be of type mspResponseFnPtr
-static void crsfSendMspResponse(uint8_t *payload, const uint8_t payloadSize)
+void crsfProcessDisplayPortCmd(uint8_t *frameStart)
{
- sbuf_t crsfPayloadBuf;
- sbuf_t *dst = &crsfPayloadBuf;
+ const uint8_t cmd = frameStart[0];
- crsfInitializeFrame(dst);
- sbufWriteU8(dst, payloadSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); // size of CRSF frame (everything except sync and size itself)
- sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP); // CRSF type
- sbufWriteU8(dst, mspRequestOriginID); // response destination must be the same as request origin in order to response reach proper destination.
- sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); // origin is always this device
- sbufWriteData(dst, payload, payloadSize);
- crsfFinalize(dst);
+ switch (cmd) {
+ case CRSF_DISPLAYPORT_SUBCMD_OPEN:;
+ const uint8_t rows = frameStart[CRSF_DISPLAYPORT_OPEN_ROWS_OFFSET];
+ const uint8_t cols = frameStart[CRSF_DISPLAYPORT_OPEN_COLS_OFFSET];
+ crsfDisplayPortSetDimensions(rows, cols);
+ crsfDisplayPortMenuOpen();
+ break;
+ case CRSF_DISPLAYPORT_SUBCMD_CLOSE:
+ crsfDisplayPortMenuExit();
+ break;
+ case CRSF_DISPLAYPORT_SUBCMD_POLL:
+ crsfDisplayPortRefresh();
+ break;
+ default:
+ break;
+ }
}
-#endif
-static void processCrsf(void)
+#endif /* USE_CRSF_CMS_TELEMETRY */
+
+
+#if defined(USE_RX_EXPRESSLRS)
+
+static int crsfTransmitSbufBuf(sbuf_t *dst, uint8_t *frame)
{
- if (!crsfRxIsTelemetryBufEmpty()) {
- return; // do nothing if telemetry ouptut buffer is not empty yet.
- }
+ // frame size including CRC
+ const size_t frameLength = crsfSbufLen(dst) + 2;
- static uint8_t crsfScheduleIndex = 0;
+ // Set frame length into the placeholder
+ crsfFrame[1] = frameSize - 3;
- const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex];
+ // frame CRC
+ crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
- sbuf_t crsfPayloadBuf;
- sbuf_t *dst = &crsfPayloadBuf;
+ // Copy data to the frame
+ memcpy(frame, crsfFrame, frameSize);
- if (currentSchedule & BIT(CRSF_FRAME_ATTITUDE_INDEX)) {
- crsfInitializeFrame(dst);
- crsfFrameAttitude(dst);
- crsfFinalize(dst);
- }
- if (currentSchedule & BIT(CRSF_FRAME_BATTERY_SENSOR_INDEX)) {
- crsfInitializeFrame(dst);
- crsfFrameBatterySensor(dst);
- crsfFinalize(dst);
- }
+ return frameSize;
+}
- if (currentSchedule & BIT(CRSF_FRAME_FLIGHT_MODE_INDEX)) {
- crsfInitializeFrame(dst);
- crsfFrameFlightMode(dst);
- crsfFinalize(dst);
- }
-#ifdef USE_GPS
- if (currentSchedule & BIT(CRSF_FRAME_GPS_INDEX)) {
- crsfInitializeFrame(dst);
- crsfFrameGps(dst);
- crsfFinalize(dst);
- }
-#endif
+int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
+{
+ sbuf_t *dst = crsfInitializeSbuf();
-#if defined(USE_CRSF_V3)
- if (currentSchedule & BIT(CRSF_FRAME_HEARTBEAT_INDEX)) {
- crsfInitializeFrame(dst);
- crsfFrameHeartbeat(dst);
- crsfFinalize(dst);
- }
+ switch (frameType) {
+ default:
+ case CRSF_FRAMETYPE_ATTITUDE:
+ crsfFrameAttitude(dst);
+ break;
+ case CRSF_FRAMETYPE_VARIO_SENSOR:
+ crsfFrameVarioSensor(dst);
+ break;
+ case CRSF_FRAMETYPE_ALTITUDE_SENSOR:
+ crsfFrameAltitudeSensor(dst);
+ break;
+ case CRSF_FRAMETYPE_BATTERY_SENSOR:
+ crsfFrameBatterySensor(dst);
+ break;
+ case CRSF_FRAMETYPE_FLIGHT_MODE:
+ crsfFrameFlightMode(dst);
+ break;
+#if defined(USE_GPS)
+ case CRSF_FRAMETYPE_GPS:
+ crsfFrameGps(dst);
+ break;
#endif
+ case CRSF_FRAMETYPE_DEVICE_INFO:
+ crsfFrameDeviceInfo(dst);
+ break;
+ }
+
+ return crsfTransmitSbufBuf(dst, frame);
+}
- crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
+#if defined(USE_MSP_OVER_TELEMETRY)
+int getCrsfMspFrame(uint8_t *frame, uint8_t *payload, const uint8_t payloadSize)
+{
+ sbuf_t *dst = crsfInitializeSbuf();
+
+ sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP);
+ sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
+ sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
+ sbufWriteData(dst, payload, payloadSize);
+
+ return crsfTransmitSbufBuf(dst, frame);
}
+#endif /* USE_MSP_OVER_TELEMETRY */
+#endif /* USE_RX_EXPRESSLRS */
+
void crsfScheduleDeviceInfoResponse(void)
{
deviceInfoReplyPending = true;
}
-void initCrsfTelemetry(void)
+static bool crsfSendDeviceInfoData(void)
{
- // check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX)
- // and feature is enabled, if so, set CRSF telemetry enabled
- crsfTelemetryEnabled = crsfRxIsActive();
-
- if (!crsfTelemetryEnabled) {
- return;
- }
-
- deviceInfoReplyPending = false;
-#if defined(USE_MSP_OVER_TELEMETRY)
- mspReplyPending = false;
-#endif
-
- int index = 0;
- if (sensors(SENSOR_ACC) && telemetryIsSensorEnabled(SENSOR_PITCH | SENSOR_ROLL | SENSOR_HEADING)) {
- crsfSchedule[index++] = BIT(CRSF_FRAME_ATTITUDE_INDEX);
- }
- if ((isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE))
- || (isBatteryCurrentConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT | SENSOR_FUEL))) {
- crsfSchedule[index++] = BIT(CRSF_FRAME_BATTERY_SENSOR_INDEX);
- }
- if (telemetryIsSensorEnabled(SENSOR_MODE)) {
- crsfSchedule[index++] = BIT(CRSF_FRAME_FLIGHT_MODE_INDEX);
- }
-#ifdef USE_GPS
- if ((featureIsEnabled(FEATURE_GPS)
- && telemetryIsSensorEnabled(SENSOR_ALTITUDE | SENSOR_LAT_LONG | SENSOR_GROUND_SPEED | SENSOR_HEADING))
- || telemetryConfig()->crsf_gps_ground_speed_reuse
- || telemetryConfig()->crsf_gps_heading_reuse
- || telemetryConfig()->crsf_gps_altitude_reuse
- || telemetryConfig()->crsf_gps_sats_reuse) {
- crsfSchedule[index++] = BIT(CRSF_FRAME_GPS_INDEX);
+ if (deviceInfoReplyPending) {
+ deviceInfoReplyPending = false;
+ sbuf_t *dst = crsfInitializeSbuf();
+ crsfFrameDeviceInfo(dst);
+ crsfTransmitSbuf(dst);
+ return true;
}
-#endif
+ return false;
+}
+static bool crsfSendHeartBeat(void)
+{
#if defined(USE_CRSF_V3)
- while (index < (CRSF_CYCLETIME_US / CRSF_TELEMETRY_FRAME_INTERVAL_MAX_US) && index < CRSF_SCHEDULE_COUNT_MAX) {
- // schedule heartbeat to ensure that telemetry/heartbeat frames are sent at minimum 50Hz
- crsfSchedule[index++] = BIT(CRSF_FRAME_HEARTBEAT_INDEX);
+ if (crsfHeartBeatRateBucket >= 0) {
+ sbuf_t *dst = crsfInitializeSbuf();
+ crsfFrameHeartbeat(dst);
+ crsfTransmitSbuf(dst);
+ return true;
}
#endif
-
- crsfScheduleCount = (uint8_t)index;
-
-#if defined(USE_CRSF_CMS_TELEMETRY)
- crsfDisplayportRegister();
-#endif
+ return false;
}
-bool checkCrsfTelemetryState(void)
+static bool crsfSendTelemetry(void)
{
- return crsfTelemetryEnabled;
+ if (crsfTelemetryState == TELEMETRY_STATE_NATIVE)
+ {
+ telemetrySensor_t *sensor = telemetryScheduleNext();
+
+ if (sensor) {
+ sbuf_t *dst = crsfInitializeSbuf();
+ switch (sensor->telid) {
+ case TELEM_ATTITUDE:
+ crsfFrameAttitude(dst);
+ break;
+ case TELEM_VARIOMETER:
+ crsfFrameVarioSensor(dst);
+ break;
+ case TELEM_ALTITUDE:
+ crsfFrameAltitudeSensor(dst);
+ break;
+ case TELEM_BATTERY:
+ crsfFrameBatterySensor(dst);
+ break;
+ case TELEM_FLIGHT_MODE:
+ crsfFrameFlightMode(dst);
+ break;
+ #ifdef USE_GPS
+ case TELEM_GPS:
+ crsfFrameGps(dst);
+ break;
+ #endif
+ default:
+ crsfFrameHeartbeat(dst);
+ break;
+ }
+ crsfTransmitSbuf(dst);
+ telemetryScheduleCommit(sensor);
+ return true;
+ }
+ }
+
+ return false;
}
-#if defined(USE_CRSF_CMS_TELEMETRY)
-void crsfProcessDisplayPortCmd(uint8_t *frameStart)
+static bool crsfSendCustomTelemetry(void)
{
- uint8_t cmd = *frameStart;
- switch (cmd) {
- case CRSF_DISPLAYPORT_SUBCMD_OPEN: ;
- const uint8_t rows = *(frameStart + CRSF_DISPLAYPORT_OPEN_ROWS_OFFSET);
- const uint8_t cols = *(frameStart + CRSF_DISPLAYPORT_OPEN_COLS_OFFSET);
- crsfDisplayPortSetDimensions(rows, cols);
- crsfDisplayPortMenuOpen();
- break;
- case CRSF_DISPLAYPORT_SUBCMD_CLOSE:
- crsfDisplayPortMenuExit();
- break;
- case CRSF_DISPLAYPORT_SUBCMD_POLL:
- crsfDisplayPortRefresh();
- break;
- default:
- break;
+ if (crsfTelemetryState == TELEMETRY_STATE_CUSTOM)
+ {
+ size_t sensor_count = 0;
+ sbuf_t *dst = crsfInitializeSbuf();
+
+ crsfFrameCustomTelemetryHeader(dst);
+
+ while (sbufBytesRemaining(dst) > 6) {
+ telemetrySensor_t *sensor = telemetryScheduleNext();
+ if (sensor) {
+ uint8_t *ptr = sbufPtr(dst);
+ crsfFrameCustomTelemetrySensor(dst, sensor);
+ if (sbufBytesRemaining(dst) < 1) {
+ sbufReset(dst, ptr);
+ break;
+ }
+ telemetryScheduleCommit(sensor);
+ sensor_count++;
+ }
+ else {
+ break;
+ }
+ }
+
+ if (sensor_count) {
+ crsfTransmitSbuf(dst);
+ crsfTelemetryFrameId++;
+ return true;
+ }
}
+ return false;
}
-#endif
-
-#if defined(USE_CRSF_V3)
-void crsfProcessCommand(uint8_t *frameStart)
+static bool crsfPopulateCustomTelemetry(void)
{
- uint8_t cmd = *frameStart;
- uint8_t subCmd = frameStart[1];
- switch (cmd) {
- case CRSF_COMMAND_SUBCMD_GENERAL:
- switch (subCmd) {
- case CRSF_COMMAND_SUBCMD_GENERAL_CRSF_SPEED_PROPOSAL:
- crsfProcessSpeedNegotiationCmd(&frameStart[1]);
- crsfScheduleSpeedNegotiationResponse();
- break;
- default:
- break;
+ if (crsfTelemetryState == TELEMETRY_STATE_POPULATE)
+ {
+ static int slot = -10;
+
+ if (slot < 0) {
+ telemetrySensor_t * sensor = crsfGetCustomSensor(TELEM_NONE);
+ slot++;
+
+ if (sensor) {
+ sbuf_t *dst = crsfInitializeSbuf();
+ crsfFrameCustomTelemetryHeader(dst);
+ crsfFrameCustomTelemetrySensor(dst, sensor);
+ crsfTransmitSbuf(dst);
+ return true;
+ }
+ return false;
}
- break;
- default:
- break;
+
+ while (slot < TELEM_SENSOR_SLOT_COUNT) {
+ sensor_id_e id = telemetryConfig()->crsf_telemetry_sensors[slot];
+ slot++;
+
+ if (telemetrySensorActive(id)) {
+ telemetrySensor_t * sensor = crsfGetCustomSensor(id);
+ if (sensor) {
+ sbuf_t *dst = crsfInitializeSbuf();
+ crsfFrameCustomTelemetryHeader(dst);
+ crsfFrameCustomTelemetrySensor(dst, sensor);
+ crsfTransmitSbuf(dst);
+ crsfTelemetryFrameId++;
+ return true;
+ }
+ }
+ }
+
+ crsfTelemetryState = TELEMETRY_STATE_CUSTOM;
}
+
+ return false;
}
-#endif
-/*
- * Called periodically by the scheduler
- */
void handleCrsfTelemetry(timeUs_t currentTimeUs)
{
- static uint32_t crsfLastCycleTime;
-
- if (!crsfTelemetryEnabled) {
+ if (crsfTelemetryState == TELEMETRY_STATE_OFF)
return;
- }
#if defined(USE_CRSF_V3)
- if (crsfBaudNegotiationInProgress()) {
+ if (crsfBaudNegotiationInProgress())
return;
- }
#endif
- // Give the receiver a chance to send any outstanding telemetry data.
- // This needs to be done at high frequency, to enable the RX to send the telemetry frame
- // in between the RX frames.
- crsfRxSendTelemetryData();
+ crsfTelemetryRateUpdate(currentTimeUs);
- // Send ad-hoc response frames as soon as possible
+ if (crsfCanTransmitTelemetry())
+ {
+ bool __unused sent =
#if defined(USE_MSP_OVER_TELEMETRY)
- if (mspReplyPending) {
- mspReplyPending = handleCrsfMspFrameBuffer(&crsfSendMspResponse);
- crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request
- return;
- }
+ handleCrsfMspFrameBuffer(&crsfSendMspResponse) ||
#endif
-
- if (deviceInfoReplyPending) {
- sbuf_t crsfPayloadBuf;
- sbuf_t *dst = &crsfPayloadBuf;
- crsfInitializeFrame(dst);
- crsfFrameDeviceInfo(dst);
- crsfFinalize(dst);
- deviceInfoReplyPending = false;
- crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request
- return;
- }
-
#if defined(USE_CRSF_CMS_TELEMETRY)
- if (crsfDisplayPortScreen()->reset) {
- crsfDisplayPortScreen()->reset = false;
- sbuf_t crsfDisplayPortBuf;
- sbuf_t *dst = &crsfDisplayPortBuf;
- crsfInitializeFrame(dst);
- crsfFrameDisplayPortClear(dst);
- crsfFinalize(dst);
- crsfLastCycleTime = currentTimeUs;
- return;
- }
- static uint8_t displayPortBatchId = 0;
- if (crsfDisplayPortIsReady() && crsfDisplayPortScreen()->updated) {
- crsfDisplayPortScreen()->updated = false;
- uint16_t screenSize = crsfDisplayPortScreen()->rows * crsfDisplayPortScreen()->cols;
- uint8_t *srcStart = (uint8_t*)crsfDisplayPortScreen()->buffer;
- uint8_t *srcEnd = (uint8_t*)(crsfDisplayPortScreen()->buffer + screenSize);
- sbuf_t displayPortSbuf;
- sbuf_t *src = sbufInit(&displayPortSbuf, srcStart, srcEnd);
- sbuf_t crsfDisplayPortBuf;
- sbuf_t *dst = &crsfDisplayPortBuf;
- displayPortBatchId = (displayPortBatchId + 1) % CRSF_DISPLAYPORT_BATCH_MAX;
- uint8_t i = 0;
- while (sbufBytesRemaining(src)) {
- crsfInitializeFrame(dst);
- crsfFrameDisplayPortChunk(dst, src, displayPortBatchId, i);
- crsfFinalize(dst);
- crsfRxSendTelemetryData();
- i++;
- }
- crsfLastCycleTime = currentTimeUs;
- return;
- }
+ crsfSendDisplayPortData() ||
#endif
-
- // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
- // Spread out scheduled frames evenly so each frame is sent at the same frequency.
- if (currentTimeUs >= crsfLastCycleTime + (CRSF_CYCLETIME_US / crsfScheduleCount)) {
- crsfLastCycleTime = currentTimeUs;
- processCrsf();
+ crsfSendDeviceInfoData() ||
+ crsfSendTelemetry() ||
+ crsfSendCustomTelemetry() ||
+ crsfPopulateCustomTelemetry() ||
+ crsfSendHeartBeat();
}
}
-#if defined(UNIT_TEST) || defined(USE_RX_EXPRESSLRS)
-static int crsfFinalizeBuf(sbuf_t *dst, uint8_t *frame)
+static void INIT_CODE crsfInitNativeTelemetry(void)
{
- crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
- sbufSwitchToReader(dst, crsfFrame);
- const int frameSize = sbufBytesRemaining(dst);
- for (int ii = 0; sbufBytesRemaining(dst); ++ii) {
- frame[ii] = sbufReadU8(dst);
+ telemetryScheduleInit(crsfNativeTelemetrySensors, ARRAYLEN(crsfNativeTelemetrySensors));
+
+ for (size_t i = 0; i < ARRAYLEN(crsfNativeTelemetrySensors); i++) {
+ telemetrySensor_t * sensor = &crsfNativeTelemetrySensors[i];
+ if (telemetryIsSensorEnabled(sensor->tcode)) {
+ telemetryScheduleAdd(sensor);
+ }
}
- return frameSize;
}
-int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
+static void INIT_CODE crsfInitCustomTelemetry(void)
{
- sbuf_t crsfFrameBuf;
- sbuf_t *sbuf = &crsfFrameBuf;
-
- crsfInitializeFrame(sbuf);
- switch (frameType) {
- default:
- case CRSF_FRAMETYPE_ATTITUDE:
- crsfFrameAttitude(sbuf);
- break;
- case CRSF_FRAMETYPE_BATTERY_SENSOR:
- crsfFrameBatterySensor(sbuf);
- break;
- case CRSF_FRAMETYPE_FLIGHT_MODE:
- crsfFrameFlightMode(sbuf);
- break;
-#if defined(USE_GPS)
- case CRSF_FRAMETYPE_GPS:
- crsfFrameGps(sbuf);
- break;
-#endif
-#if defined(USE_MSP_OVER_TELEMETRY)
- case CRSF_FRAMETYPE_DEVICE_INFO:
- crsfFrameDeviceInfo(sbuf);
- break;
-#endif
+ telemetryScheduleInit(crsfCustomTelemetrySensors, ARRAYLEN(crsfCustomTelemetrySensors));
+
+ for (int i = 0; i < TELEM_SENSOR_SLOT_COUNT; i++) {
+ sensor_id_e id = telemetryConfig()->crsf_telemetry_sensors[i];
+ if (telemetrySensorActive(id)) {
+ telemetrySensor_t * sensor = crsfGetCustomSensor(id);
+ if (sensor) {
+ if (telemetryConfig()->crsf_telemetry_interval[i])
+ sensor->min_interval = telemetryConfig()->crsf_telemetry_interval[i];
+ if (sensor->max_interval > 1000)
+ sensor->max_interval += rand() % 100;
+ telemetryScheduleAdd(sensor);
+ }
+ }
}
- const int frameSize = crsfFinalizeBuf(sbuf, frame);
- return frameSize;
}
-#if defined(USE_MSP_OVER_TELEMETRY)
-int getCrsfMspFrame(uint8_t *frame, uint8_t *payload, const uint8_t payloadSize)
+void INIT_CODE initCrsfTelemetry(void)
{
- sbuf_t crsfFrameBuf;
- sbuf_t *sbuf = &crsfFrameBuf;
+ crsfTelemetryState = !crsfRxIsActive() ? TELEMETRY_STATE_OFF :
+ (telemetryConfig()->crsf_telemetry_mode == CRSF_TELEMETRY_MODE_NATIVE ?
+ TELEMETRY_STATE_NATIVE : TELEMETRY_STATE_POPULATE);
- crsfInitializeFrame(sbuf);
- sbufWriteU8(sbuf, payloadSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
- sbufWriteU8(sbuf, CRSF_FRAMETYPE_MSP_RESP);
- sbufWriteU8(sbuf, CRSF_ADDRESS_RADIO_TRANSMITTER);
- sbufWriteU8(sbuf, CRSF_ADDRESS_FLIGHT_CONTROLLER);
- sbufWriteData(sbuf, payload, payloadSize);
- const int frameSize = crsfFinalizeBuf(sbuf, frame);
- return frameSize;
-}
-#endif
+ if (crsfTelemetryState)
+ {
+ const float rate = telemetryConfig()->crsf_telemetry_link_rate;
+ const float ratio = telemetryConfig()->crsf_telemetry_link_ratio;
+
+ crsfTelemetryRateQuanta = rate / (ratio * 1000000);
+ crsfTelemetryRateBucket = 0;
+ crsfTelemetryFrameId = 0;
+
+#if defined(USE_CRSF_V3)
+ crsfHeartBeatRateBucket = 0;
#endif
+
+ if (crsfTelemetryState == TELEMETRY_STATE_NATIVE) {
+ crsfInitNativeTelemetry();
+ }
+ else {
+ crsfInitCustomTelemetry();
+ }
+
+#if defined(USE_CRSF_CMS_TELEMETRY)
+ crsfDisplayportRegister();
#endif
+ }
+}
+
+#endif /* USE_TELEMETRY_CRSF */
diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h
index 5710e3a440..ed514c342c 100644
--- a/src/main/telemetry/crsf.h
+++ b/src/main/telemetry/crsf.h
@@ -1,21 +1,18 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Rotorflight.
*
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software under the terms of the
- * GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option)
- * any later version.
+ * Rotorflight is free software. You can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * will be useful, but WITHOUT ANY WARRANTY; without even the implied
- * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * Rotorflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
- * along with this software.
- *
- * If not, see .
+ * along with this software. If not, see .
*/
#pragma once
@@ -28,91 +25,32 @@
#include "rx/crsf_protocol.h"
#include "telemetry/msp_shared.h"
-enum {
- CRSF_FM_REUSE_NONE = 0,
- CRSF_FM_REUSE_GOV_STATE,
- CRSF_FM_REUSE_HEADSPEED,
- CRSF_FM_REUSE_THROTTLE,
- CRSF_FM_REUSE_ESC_TEMP,
- CRSF_FM_REUSE_MCU_TEMP,
- CRSF_FM_REUSE_MCU_LOAD,
- CRSF_FM_REUSE_SYS_LOAD,
- CRSF_FM_REUSE_RT_LOAD,
- CRSF_FM_REUSE_BEC_VOLTAGE,
- CRSF_FM_REUSE_BUS_VOLTAGE,
- CRSF_FM_REUSE_MCU_VOLTAGE,
- CRSF_FM_REUSE_ADJFUNC,
- CRSF_FM_REUSE_GOV_ADJFUNC,
-};
-
-enum {
- CRSF_ATT_REUSE_NONE = 0,
- CRSF_ATT_REUSE_THROTTLE,
- CRSF_ATT_REUSE_ESC_TEMP,
- CRSF_ATT_REUSE_ESC_PWM,
- CRSF_ATT_REUSE_ESC_BEC_VOLTAGE,
- CRSF_ATT_REUSE_ESC_BEC_CURRENT,
- CRSF_ATT_REUSE_ESC_BEC_TEMP,
- CRSF_ATT_REUSE_ESC_STATUS,
- CRSF_ATT_REUSE_ESC_STATUS2,
- CRSF_ATT_REUSE_MCU_TEMP,
- CRSF_ATT_REUSE_MCU_LOAD,
- CRSF_ATT_REUSE_SYS_LOAD,
- CRSF_ATT_REUSE_RT_LOAD,
- CRSF_ATT_REUSE_BEC_VOLTAGE,
- CRSF_ATT_REUSE_BUS_VOLTAGE,
- CRSF_ATT_REUSE_MCU_VOLTAGE,
-};
-
-enum {
- CRSF_GPS_REUSE_NONE = 0,
- CRSF_GPS_REUSE_HEADSPEED,
- CRSF_GPS_REUSE_THROTTLE,
- CRSF_GPS_REUSE_ESC_TEMP,
- CRSF_GPS_REUSE_ESC_PWM,
- CRSF_GPS_REUSE_ESC_THROTTLE,
- CRSF_GPS_REUSE_ESC_BEC_VOLTAGE,
- CRSF_GPS_REUSE_ESC_BEC_CURRENT,
- CRSF_GPS_REUSE_ESC_BEC_TEMP,
- CRSF_GPS_REUSE_ESC_STATUS,
- CRSF_GPS_REUSE_ESC_STATUS2,
- CRSF_GPS_REUSE_MCU_TEMP,
- CRSF_GPS_REUSE_MCU_LOAD,
- CRSF_GPS_REUSE_SYS_LOAD,
- CRSF_GPS_REUSE_RT_LOAD,
- CRSF_GPS_REUSE_BEC_VOLTAGE,
- CRSF_GPS_REUSE_BUS_VOLTAGE,
- CRSF_GPS_REUSE_MCU_VOLTAGE,
-};
-
-enum {
- CRSF_GPS_SATS_REUSE_NONE = 0,
- CRSF_GPS_SATS_REUSE_ESC_TEMP,
- CRSF_GPS_SATS_REUSE_MCU_TEMP,
- CRSF_GPS_SATS_REUSE_PROFILE,
- CRSF_GPS_SATS_REUSE_RATE_PROFILE,
- CRSF_GPS_SATS_REUSE_LED_PROFILE,
- CRSF_GPS_SATS_REUSE_MODEL_ID,
-};
void initCrsfTelemetry(void);
+void handleCrsfTelemetry(timeUs_t currentTimeUs);
+
uint32_t getCrsfDesiredSpeed(void);
void setCrsfDefaultSpeed(void);
+
bool checkCrsfTelemetryState(void);
-void handleCrsfTelemetry(timeUs_t currentTimeUs);
+
void crsfScheduleDeviceInfoResponse(void);
void crsfScheduleMspResponse(uint8_t requestOriginID);
+
int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType);
void crsfProcessCommand(uint8_t *frameStart);
+
#if defined(USE_CRSF_CMS_TELEMETRY)
void crsfProcessDisplayPortCmd(uint8_t *frameStart);
#endif
+
#if defined(USE_MSP_OVER_TELEMETRY)
void initCrsfMspBuffer(void);
bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength);
bool handleCrsfMspFrameBuffer(mspResponseFnPtr responseFn);
int getCrsfMspFrame(uint8_t *frame, uint8_t *payload, const uint8_t payloadSize);
#endif
+
#if defined(USE_CRSF_V3)
void speedNegotiationProcess(uint32_t currentTime);
bool crsfBaudNegotiationInProgress(void);
diff --git a/src/main/telemetry/sensors.c b/src/main/telemetry/sensors.c
new file mode 100644
index 0000000000..280f85b2ff
--- /dev/null
+++ b/src/main/telemetry/sensors.c
@@ -0,0 +1,511 @@
+/*
+ * This file is part of Rotorflight.
+ *
+ * Rotorflight is free software. You can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Rotorflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software. If not, see .
+ */
+
+#include
+#include
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_TELEMETRY
+
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+#include "pg/pilot.h"
+#include "pg/telemetry.h"
+
+#include "build/debug.h"
+
+#include "common/crc.h"
+
+#include "sensors/battery.h"
+#include "sensors/voltage.h"
+#include "sensors/current.h"
+#include "sensors/esc_sensor.h"
+#include "sensors/adcinternal.h"
+#include "sensors/acceleration.h"
+
+#include "flight/position.h"
+#include "flight/governor.h"
+#include "flight/rescue.h"
+#include "flight/mixer.h"
+#include "flight/imu.h"
+
+#include "io/gps.h"
+#include "io/ledstrip.h"
+
+#include "fc/rc_modes.h"
+#include "fc/rc_adjustments.h"
+#include "fc/runtime_config.h"
+
+#include "scheduler/scheduler.h"
+
+#include "telemetry/sensors.h"
+
+
+/** Sensor functions **/
+
+static int getVoltage(voltageMeterId_e id)
+{
+ voltageMeter_t voltage;
+ return voltageMeterRead(id, &voltage) ? voltage.voltage / 10 : 0;
+}
+
+static int getCurrent(currentMeterId_e id)
+{
+ currentMeter_t current;
+ return currentMeterRead(id, ¤t) ? current.current / 10: 0;
+}
+
+static int getEscSensorValue(uint8_t motor, uint8_t id)
+{
+ escSensorData_t * data = getEscSensorData(motor);
+
+ if (data) {
+ switch (id) {
+ case 1:
+ return data->voltage / 10;
+ case 2:
+ return data->current / 10;
+ case 3:
+ return data->consumption;
+ case 4:
+ return data->erpm;
+ case 5:
+ return data->pwm;
+ case 6:
+ return data->throttle;
+ case 7:
+ return data->temperature / 10;
+ case 8:
+ return data->temperature2 / 10;
+ case 9:
+ return data->bec_voltage;
+ case 10:
+ return data->bec_current;
+ case 11:
+ return data->status;
+ case 12:
+ return 0; // data->model_id
+ }
+ }
+
+ return 0;
+}
+
+static uint32_t getTupleHash(uint32_t a, uint32_t b)
+{
+ uint32_t data[2] = { a, b };
+ return fnv_update(0x42424242, data, sizeof(data));
+}
+
+
+int telemetrySensorValue(sensor_id_e id)
+{
+ switch (id) {
+ case TELEM_NONE:
+ return 0;
+
+ case TELEM_HEARTBEAT:
+ return millis() % 60000;
+
+ case TELEM_BATTERY:
+ return 0;
+ case TELEM_BATTERY_VOLTAGE:
+ return getBatteryVoltage();
+ case TELEM_BATTERY_CURRENT:
+ return getBatteryCurrent();
+ case TELEM_BATTERY_CONSUMPTION:
+ return getBatteryCapacityUsed();
+ case TELEM_BATTERY_CHARGE_LEVEL:
+ return calculateBatteryPercentageRemaining();
+ case TELEM_BATTERY_CELL_COUNT:
+ return getBatteryCellCount();
+ case TELEM_BATTERY_CELL_VOLTAGE:
+ return getBatteryAverageCellVoltage();
+ case TELEM_BATTERY_CELL_VOLTAGES:
+ return 0;
+
+ case TELEM_CONTROL:
+ return millis();
+ case TELEM_PITCH_CONTROL:
+ return lrintf(mixerGetInput(MIXER_IN_STABILIZED_PITCH) * 120);
+ case TELEM_ROLL_CONTROL:
+ return lrintf(mixerGetInput(MIXER_IN_STABILIZED_ROLL) * 120);
+ case TELEM_YAW_CONTROL:
+ return lrintf(mixerGetInput(MIXER_IN_STABILIZED_YAW) * 240);
+ case TELEM_COLLECTIVE_CONTROL:
+ return lrintf(mixerGetInput(MIXER_IN_STABILIZED_COLLECTIVE) * 120);
+ case TELEM_THROTTLE_CONTROL:
+ return lrintf(mixerGetInput(MIXER_IN_STABILIZED_THROTTLE) * 100);
+
+ case TELEM_ESC1_DATA:
+ case TELEM_ESC1_VOLTAGE:
+ case TELEM_ESC1_CURRENT:
+ case TELEM_ESC1_CAPACITY:
+ case TELEM_ESC1_ERPM:
+ case TELEM_ESC1_POWER:
+ case TELEM_ESC1_THROTTLE:
+ case TELEM_ESC1_TEMP1:
+ case TELEM_ESC1_TEMP2:
+ case TELEM_ESC1_BEC_VOLTAGE:
+ case TELEM_ESC1_BEC_CURRENT:
+ case TELEM_ESC1_STATUS:
+ case TELEM_ESC1_MODEL:
+ return getEscSensorValue(0, id - TELEM_ESC1_DATA);
+
+ case TELEM_ESC2_DATA:
+ case TELEM_ESC2_VOLTAGE:
+ case TELEM_ESC2_CURRENT:
+ case TELEM_ESC2_CAPACITY:
+ case TELEM_ESC2_ERPM:
+ case TELEM_ESC2_POWER:
+ case TELEM_ESC2_THROTTLE:
+ case TELEM_ESC2_TEMP1:
+ case TELEM_ESC2_TEMP2:
+ case TELEM_ESC2_BEC_VOLTAGE:
+ case TELEM_ESC2_BEC_CURRENT:
+ case TELEM_ESC2_STATUS:
+ case TELEM_ESC2_MODEL:
+ return getEscSensorValue(1, id - TELEM_ESC2_DATA);
+
+ case TELEM_ESC_VOLTAGE:
+ return getVoltage(VOLTAGE_METER_ID_ESC_COMBINED);
+ case TELEM_BEC_VOLTAGE:
+ return getVoltage(VOLTAGE_METER_ID_BEC);
+ case TELEM_BUS_VOLTAGE:
+ return getVoltage(VOLTAGE_METER_ID_BUS);
+ case TELEM_MCU_VOLTAGE:
+ return getVoltage(VOLTAGE_METER_ID_MCU);
+
+ case TELEM_ESC_CURRENT:
+ return getCurrent(CURRENT_METER_ID_ESC_COMBINED);
+ case TELEM_BEC_CURRENT:
+ return getCurrent(CURRENT_METER_ID_BEC);
+ case TELEM_BUS_CURRENT:
+ return getCurrent(CURRENT_METER_ID_BUS);
+ case TELEM_MCU_CURRENT:
+ return getCurrent(CURRENT_METER_ID_MCU);
+
+ case TELEM_MCU_TEMP:
+ return getCoreTemperatureCelsius();
+
+ case TELEM_ESC_TEMP:
+ case TELEM_BEC_TEMP:
+ case TELEM_AIR_TEMP:
+ case TELEM_MOTOR_TEMP:
+ case TELEM_BATTERY_TEMP:
+ case TELEM_EXHAUST_TEMP:
+ return 0;
+
+ case TELEM_HEADING:
+ return attitude.values.yaw;
+ case TELEM_ALTITUDE:
+ return getEstimatedAltitudeCm();
+ case TELEM_VARIOMETER:
+ return getEstimatedVarioCms();
+
+ case TELEM_HEADSPEED:
+ return getHeadSpeed();
+ case TELEM_TAILSPEED:
+ return getTailSpeed();
+
+ case TELEM_MOTOR_RPM:
+ case TELEM_TRANS_RPM:
+ return 0;
+
+ case TELEM_ATTITUDE:
+ return millis();
+ case TELEM_ATTITUDE_PITCH:
+ return attitude.values.pitch / 10;
+ case TELEM_ATTITUDE_ROLL:
+ return attitude.values.roll / 10;
+ case TELEM_ATTITUDE_YAW:
+ return attitude.values.yaw / 10;
+
+ case TELEM_ACCEL:
+ return millis();
+ case TELEM_ACCEL_X:
+ return lrintf(acc.accADC[0] * acc.dev.acc_1G_rec * 10);
+ case TELEM_ACCEL_Y:
+ return lrintf(acc.accADC[1] * acc.dev.acc_1G_rec * 10);
+ case TELEM_ACCEL_Z:
+ return lrintf(acc.accADC[2] * acc.dev.acc_1G_rec * 10);
+
+ case TELEM_GPS:
+ return 0;
+ case TELEM_GPS_SATS:
+ return gpsSol.numSat;
+ case TELEM_GPS_PDOP:
+ return 0;
+ case TELEM_GPS_HDOP:
+ return gpsSol.hdop;
+ case TELEM_GPS_VDOP:
+ return 0;
+ case TELEM_GPS_COORD:
+ return getTupleHash(gpsSol.llh.lat, gpsSol.llh.lat);
+ case TELEM_GPS_ALTITUDE:
+ return gpsSol.llh.altCm;
+ case TELEM_GPS_HEADING:
+ return gpsSol.groundCourse;
+ case TELEM_GPS_GROUNDSPEED:
+ return gpsSol.groundSpeed;
+ case TELEM_GPS_HOME_DISTANCE:
+ return GPS_distanceToHome;
+ case TELEM_GPS_HOME_DIRECTION:
+ return GPS_directionToHome;
+ case TELEM_GPS_DATE_TIME:
+ return 0;
+
+ case TELEM_LOAD:
+ return 0;
+ case TELEM_CPU_LOAD:
+ return getAverageCPULoadPercent();
+ case TELEM_SYS_LOAD:
+ return getAverageSystemLoadPercent();
+ case TELEM_RT_LOAD:
+ return getMaxRealTimeLoadPercent();
+
+ case TELEM_MODEL_ID:
+ return pilotConfig()->modelId;
+ case TELEM_FLIGHT_MODE:
+ return flightModeFlags;
+ case TELEM_ARMING_FLAGS:
+ return armingFlags;
+ case TELEM_ARMING_DISABLE_FLAGS:
+ return getArmingDisableFlags();
+ case TELEM_RESCUE_STATE:
+ return getRescueState();
+ case TELEM_GOVERNOR_STATE:
+ return getGovernorState();
+ case TELEM_GOVERNOR_FLAGS:
+ return 0;
+
+ case TELEM_PID_PROFILE:
+ return getCurrentPidProfileIndex() + 1;
+ case TELEM_RATES_PROFILE:
+ return getCurrentControlRateProfileIndex() + 1;
+ case TELEM_LED_PROFILE:
+ return getLedProfile() + 1;
+ case TELEM_BATTERY_PROFILE:
+ return 0;
+
+ case TELEM_ADJFUNC:
+ return getAdjustmentsRangeName() ?
+ getTupleHash(getAdjustmentsRangeFunc(), getAdjustmentsRangeValue()) : 0;
+
+ case TELEM_DEBUG_0:
+ return debug[0];
+ case TELEM_DEBUG_1:
+ return debug[1];
+ case TELEM_DEBUG_2:
+ return debug[2];
+ case TELEM_DEBUG_3:
+ return debug[3];
+ case TELEM_DEBUG_4:
+ return debug[4];
+ case TELEM_DEBUG_5:
+ return debug[5];
+ case TELEM_DEBUG_6:
+ return debug[6];
+ case TELEM_DEBUG_7:
+ return debug[7];
+
+ default:
+ return 0;
+ }
+
+ return 0;
+}
+
+
+bool telemetrySensorActive(sensor_id_e id)
+{
+ switch (id) {
+ case TELEM_NONE:
+ return false;
+
+ case TELEM_HEARTBEAT:
+ return true;
+
+ case TELEM_BATTERY:
+ case TELEM_BATTERY_VOLTAGE:
+ case TELEM_BATTERY_CURRENT:
+ case TELEM_BATTERY_CONSUMPTION:
+ case TELEM_BATTERY_CHARGE_LEVEL:
+ case TELEM_BATTERY_CELL_COUNT:
+ case TELEM_BATTERY_CELL_VOLTAGE:
+ return true;
+
+ case TELEM_BATTERY_CELL_VOLTAGES:
+ return false;
+
+ case TELEM_CONTROL:
+ case TELEM_ROLL_CONTROL:
+ case TELEM_PITCH_CONTROL:
+ case TELEM_YAW_CONTROL:
+ case TELEM_COLLECTIVE_CONTROL:
+ case TELEM_THROTTLE_CONTROL:
+ return true;
+
+ case TELEM_ESC1_DATA:
+ case TELEM_ESC1_VOLTAGE:
+ case TELEM_ESC1_CURRENT:
+ case TELEM_ESC1_CAPACITY:
+ case TELEM_ESC1_ERPM:
+ case TELEM_ESC1_POWER:
+ case TELEM_ESC1_THROTTLE:
+ case TELEM_ESC1_TEMP1:
+ case TELEM_ESC1_TEMP2:
+ case TELEM_ESC1_BEC_VOLTAGE:
+ case TELEM_ESC1_BEC_CURRENT:
+ case TELEM_ESC1_STATUS:
+ case TELEM_ESC1_MODEL:
+ return true;
+
+ case TELEM_ESC2_DATA:
+ case TELEM_ESC2_VOLTAGE:
+ case TELEM_ESC2_CURRENT:
+ case TELEM_ESC2_CAPACITY:
+ case TELEM_ESC2_ERPM:
+ case TELEM_ESC2_POWER:
+ case TELEM_ESC2_THROTTLE:
+ case TELEM_ESC2_TEMP1:
+ case TELEM_ESC2_TEMP2:
+ case TELEM_ESC2_BEC_VOLTAGE:
+ case TELEM_ESC2_BEC_CURRENT:
+ case TELEM_ESC2_STATUS:
+ case TELEM_ESC2_MODEL:
+ return true;
+
+ case TELEM_ESC_VOLTAGE:
+ case TELEM_BEC_VOLTAGE:
+ case TELEM_BUS_VOLTAGE:
+ case TELEM_MCU_VOLTAGE:
+ return true;
+
+ case TELEM_ESC_CURRENT:
+ case TELEM_BEC_CURRENT:
+ case TELEM_BUS_CURRENT:
+ case TELEM_MCU_CURRENT:
+ return true;
+
+ case TELEM_MCU_TEMP:
+ return true;
+ case TELEM_ESC_TEMP:
+ case TELEM_BEC_TEMP:
+ case TELEM_AIR_TEMP:
+ case TELEM_MOTOR_TEMP:
+ case TELEM_BATTERY_TEMP:
+ case TELEM_EXHAUST_TEMP:
+ return false;
+
+ case TELEM_HEADING:
+ case TELEM_ALTITUDE:
+ case TELEM_VARIOMETER:
+ return true;
+
+ case TELEM_HEADSPEED:
+ case TELEM_TAILSPEED:
+ return true;
+
+ case TELEM_MOTOR_RPM:
+ case TELEM_TRANS_RPM:
+ return false;
+
+ case TELEM_ATTITUDE:
+ case TELEM_ATTITUDE_PITCH:
+ case TELEM_ATTITUDE_ROLL:
+ case TELEM_ATTITUDE_YAW:
+ return true;
+
+ case TELEM_ACCEL:
+ case TELEM_ACCEL_X:
+ case TELEM_ACCEL_Y:
+ case TELEM_ACCEL_Z:
+ return true;
+
+ case TELEM_GPS:
+ case TELEM_GPS_SATS:
+ case TELEM_GPS_HDOP:
+ case TELEM_GPS_COORD:
+ case TELEM_GPS_ALTITUDE:
+ case TELEM_GPS_HEADING:
+ case TELEM_GPS_GROUNDSPEED:
+ case TELEM_GPS_HOME_DISTANCE:
+ case TELEM_GPS_HOME_DIRECTION:
+ return true;
+
+ case TELEM_GPS_PDOP:
+ case TELEM_GPS_VDOP:
+ case TELEM_GPS_DATE_TIME:
+ return false;
+
+ case TELEM_LOAD:
+ case TELEM_CPU_LOAD:
+ case TELEM_SYS_LOAD:
+ case TELEM_RT_LOAD:
+ return true;
+
+ case TELEM_MODEL_ID:
+ case TELEM_FLIGHT_MODE:
+ case TELEM_ARMING_FLAGS:
+ case TELEM_ARMING_DISABLE_FLAGS:
+ case TELEM_RESCUE_STATE:
+ case TELEM_GOVERNOR_STATE:
+ case TELEM_GOVERNOR_FLAGS:
+ return true;
+
+ case TELEM_PID_PROFILE:
+ case TELEM_RATES_PROFILE:
+ return true;
+
+ case TELEM_BATTERY_PROFILE:
+ case TELEM_LED_PROFILE:
+ return false;
+
+ case TELEM_ADJFUNC:
+ return true;
+
+ case TELEM_DEBUG_0:
+ case TELEM_DEBUG_1:
+ case TELEM_DEBUG_2:
+ case TELEM_DEBUG_3:
+ case TELEM_DEBUG_4:
+ case TELEM_DEBUG_5:
+ case TELEM_DEBUG_6:
+ case TELEM_DEBUG_7:
+ return debugMode;
+
+ default:
+ return false;
+ }
+
+ return false;
+}
+
+
+/** Legacy sensors **/
+
+bool telemetryIsSensorEnabled(uint32_t sensor_bits)
+{
+ return (telemetryConfig()->enableSensors & sensor_bits);
+}
+
+#endif /* USE_TELEMETRY */
diff --git a/src/main/telemetry/sensors.h b/src/main/telemetry/sensors.h
new file mode 100644
index 0000000000..481e81abe3
--- /dev/null
+++ b/src/main/telemetry/sensors.h
@@ -0,0 +1,198 @@
+/*
+ * This file is part of Rotorflight.
+ *
+ * Rotorflight is free software. You can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Rotorflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software. If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+#include
+#include
+
+#include "pg/pg.h"
+#include "pg/telemetry.h"
+
+#include "common/streambuf.h"
+
+#include "flight/motors.h"
+#include "flight/servos.h"
+
+
+/** Custom telemetry sensor types **/
+
+typedef enum
+{
+ TELEM_NONE = 0,
+
+ TELEM_HEARTBEAT = 1,
+
+ TELEM_BATTERY = 2,
+ TELEM_BATTERY_VOLTAGE = 3,
+ TELEM_BATTERY_CURRENT = 4,
+ TELEM_BATTERY_CONSUMPTION = 5,
+ TELEM_BATTERY_CHARGE_LEVEL = 6,
+ TELEM_BATTERY_CELL_COUNT = 7,
+ TELEM_BATTERY_CELL_VOLTAGE = 8,
+ TELEM_BATTERY_CELL_VOLTAGES = 9,
+
+ TELEM_CONTROL = 10,
+ TELEM_PITCH_CONTROL = 11,
+ TELEM_ROLL_CONTROL = 12,
+ TELEM_YAW_CONTROL = 13,
+ TELEM_COLLECTIVE_CONTROL = 14,
+ TELEM_THROTTLE_CONTROL = 15,
+
+ TELEM_ESC1_DATA = 16,
+ TELEM_ESC1_VOLTAGE = 17,
+ TELEM_ESC1_CURRENT = 18,
+ TELEM_ESC1_CAPACITY = 19,
+ TELEM_ESC1_ERPM = 20,
+ TELEM_ESC1_POWER = 21,
+ TELEM_ESC1_THROTTLE = 22,
+ TELEM_ESC1_TEMP1 = 23,
+ TELEM_ESC1_TEMP2 = 24,
+ TELEM_ESC1_BEC_VOLTAGE = 25,
+ TELEM_ESC1_BEC_CURRENT = 26,
+ TELEM_ESC1_STATUS = 27,
+ TELEM_ESC1_MODEL = 28,
+
+ TELEM_ESC2_DATA = 29,
+ TELEM_ESC2_VOLTAGE = 30,
+ TELEM_ESC2_CURRENT = 31,
+ TELEM_ESC2_CAPACITY = 32,
+ TELEM_ESC2_ERPM = 33,
+ TELEM_ESC2_POWER = 34,
+ TELEM_ESC2_THROTTLE = 35,
+ TELEM_ESC2_TEMP1 = 36,
+ TELEM_ESC2_TEMP2 = 37,
+ TELEM_ESC2_BEC_VOLTAGE = 38,
+ TELEM_ESC2_BEC_CURRENT = 39,
+ TELEM_ESC2_STATUS = 40,
+ TELEM_ESC2_MODEL = 41,
+
+ TELEM_ESC_VOLTAGE = 42,
+ TELEM_BEC_VOLTAGE = 43,
+ TELEM_BUS_VOLTAGE = 44,
+ TELEM_MCU_VOLTAGE = 45,
+
+ TELEM_ESC_CURRENT = 46,
+ TELEM_BEC_CURRENT = 47,
+ TELEM_BUS_CURRENT = 48,
+ TELEM_MCU_CURRENT = 49,
+
+ TELEM_ESC_TEMP = 50,
+ TELEM_BEC_TEMP = 51,
+ TELEM_MCU_TEMP = 52,
+ TELEM_AIR_TEMP = 53,
+ TELEM_MOTOR_TEMP = 54,
+ TELEM_BATTERY_TEMP = 55,
+ TELEM_EXHAUST_TEMP = 56,
+
+ TELEM_HEADING = 57,
+ TELEM_ALTITUDE = 58,
+ TELEM_VARIOMETER = 59,
+
+ TELEM_HEADSPEED = 60,
+ TELEM_TAILSPEED = 61,
+ TELEM_MOTOR_RPM = 62,
+ TELEM_TRANS_RPM = 63,
+
+ TELEM_ATTITUDE = 64,
+ TELEM_ATTITUDE_PITCH = 65,
+ TELEM_ATTITUDE_ROLL = 66,
+ TELEM_ATTITUDE_YAW = 67,
+
+ TELEM_ACCEL = 68,
+ TELEM_ACCEL_X = 69,
+ TELEM_ACCEL_Y = 70,
+ TELEM_ACCEL_Z = 71,
+
+ TELEM_GPS = 72,
+ TELEM_GPS_SATS = 73,
+ TELEM_GPS_PDOP = 74,
+ TELEM_GPS_HDOP = 75,
+ TELEM_GPS_VDOP = 76,
+ TELEM_GPS_COORD = 77,
+ TELEM_GPS_ALTITUDE = 78,
+ TELEM_GPS_HEADING = 79,
+ TELEM_GPS_GROUNDSPEED = 80,
+ TELEM_GPS_HOME_DISTANCE = 81,
+ TELEM_GPS_HOME_DIRECTION = 82,
+ TELEM_GPS_DATE_TIME = 83,
+
+ TELEM_LOAD = 84,
+ TELEM_CPU_LOAD = 85,
+ TELEM_SYS_LOAD = 86,
+ TELEM_RT_LOAD = 87,
+
+ TELEM_MODEL_ID = 88,
+ TELEM_FLIGHT_MODE = 89,
+ TELEM_ARMING_FLAGS = 90,
+ TELEM_ARMING_DISABLE_FLAGS = 91,
+ TELEM_RESCUE_STATE = 92,
+ TELEM_GOVERNOR_STATE = 93,
+ TELEM_GOVERNOR_FLAGS = 94,
+
+ TELEM_PID_PROFILE = 95,
+ TELEM_RATES_PROFILE = 96,
+ TELEM_BATTERY_PROFILE = 97,
+ TELEM_LED_PROFILE = 98,
+
+ TELEM_ADJFUNC = 99,
+
+ TELEM_DEBUG_0 = 100,
+ TELEM_DEBUG_1 = 101,
+ TELEM_DEBUG_2 = 102,
+ TELEM_DEBUG_3 = 103,
+ TELEM_DEBUG_4 = 104,
+ TELEM_DEBUG_5 = 105,
+ TELEM_DEBUG_6 = 106,
+ TELEM_DEBUG_7 = 107,
+
+ TELEM_SENSOR_COUNT
+
+} sensor_id_e;
+
+
+typedef struct telemetrySensor_s telemetrySensor_t;
+
+typedef void (*telemetryEncode_f)(sbuf_t *buf, telemetrySensor_t *sensor);
+
+struct telemetrySensor_s {
+
+ uint16_t telid;
+ uint32_t tcode;
+
+ uint16_t min_interval;
+ uint16_t max_interval;
+
+ bool active;
+ bool update;
+ int bucket;
+ int value;
+
+ telemetryEncode_f encode;
+};
+
+
+int telemetrySensorValue(sensor_id_e id);
+bool telemetrySensorActive(sensor_id_e id);
+
+
+/** Legacy sensors **/
+
+bool telemetryIsSensorEnabled(uint32_t sensor_bits);
+
diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
index 5450a0354c..ce9e58ca9c 100644
--- a/src/main/telemetry/telemetry.c
+++ b/src/main/telemetry/telemetry.c
@@ -1,26 +1,24 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Rotorflight.
*
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software under the terms of the
- * GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option)
- * any later version.
+ * Rotorflight is free software. You can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * will be useful, but WITHOUT ANY WARRANTY; without even the implied
- * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * Rotorflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
- * along with this software.
- *
- * If not, see .
+ * along with this software. If not, see .
*/
#include
#include
#include
+#include
#include "platform.h"
@@ -62,51 +60,12 @@
#include "telemetry/msp_shared.h"
-void telemetryInit(void)
-{
-#ifdef USE_TELEMETRY_FRSKY_HUB
- initFrSkyHubTelemetry();
-#endif
-#ifdef USE_TELEMETRY_HOTT
- initHoTTTelemetry();
-#endif
-#ifdef USE_TELEMETRY_SMARTPORT
- initSmartPortTelemetry();
-#endif
-#ifdef USE_TELEMETRY_LTM
- initLtmTelemetry();
-#endif
-#ifdef USE_TELEMETRY_JETIEXBUS
- initJetiExBusTelemetry();
-#endif
-#ifdef USE_TELEMETRY_MAVLINK
- initMAVLinkTelemetry();
-#endif
-#ifdef USE_TELEMETRY_GHST
- initGhstTelemetry();
-#endif
-#ifdef USE_TELEMETRY_CRSF
- initCrsfTelemetry();
-#if defined(USE_MSP_OVER_TELEMETRY)
- initCrsfMspBuffer();
-#endif
-#endif
-#ifdef USE_TELEMETRY_SRXL
- initSrxlTelemetry();
-#endif
-#ifdef USE_TELEMETRY_IBUS
- initIbusTelemetry();
-#endif
-#if defined(USE_MSP_OVER_TELEMETRY)
- initSharedMsp();
-#endif
+serialPort_t *telemetrySharedPort = NULL;
- telemetryCheckState();
-}
bool telemetryDetermineEnabledState(portSharing_e portSharing)
{
- bool enabled = portSharing == PORTSHARING_NOT_SHARED;
+ bool enabled = (portSharing == PORTSHARING_NOT_SHARED);
if (portSharing == PORTSHARING_SHARED) {
if (isModeActivationConditionPresent(BOXTELEMETRY))
@@ -120,22 +79,23 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing)
bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig, const SerialRXType serialrxProvider)
{
- if (portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK &&
+ if ((portConfig->functionMask & FUNCTION_RX_SERIAL) &&
+ (portConfig->functionMask & TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK) &&
(serialrxProvider == SERIALRX_SPEKTRUM1024 ||
- serialrxProvider == SERIALRX_SPEKTRUM2048 ||
- serialrxProvider == SERIALRX_SBUS ||
- serialrxProvider == SERIALRX_SUMD ||
- serialrxProvider == SERIALRX_SUMH ||
- serialrxProvider == SERIALRX_XBUS_MODE_B ||
- serialrxProvider == SERIALRX_XBUS_MODE_B_RJ01 ||
- serialrxProvider == SERIALRX_IBUS)) {
+ serialrxProvider == SERIALRX_SPEKTRUM2048 ||
+ serialrxProvider == SERIALRX_SBUS ||
+ serialrxProvider == SERIALRX_SUMD ||
+ serialrxProvider == SERIALRX_SUMH ||
+ serialrxProvider == SERIALRX_XBUS_MODE_B ||
+ serialrxProvider == SERIALRX_XBUS_MODE_B_RJ01 ||
+ serialrxProvider == SERIALRX_IBUS)) {
return true;
}
#ifdef USE_TELEMETRY_IBUS
- if (portConfig->functionMask & FUNCTION_TELEMETRY_IBUS
- && portConfig->functionMask & FUNCTION_RX_SERIAL
- && serialrxProvider == SERIALRX_IBUS) {
+ if (portConfig->functionMask & FUNCTION_TELEMETRY_IBUS &&
+ portConfig->functionMask & FUNCTION_RX_SERIAL &&
+ serialrxProvider == SERIALRX_IBUS) {
// IBUS serial RX & telemetry
return true;
}
@@ -143,7 +103,40 @@ bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig, const Seri
return false;
}
-serialPort_t *telemetrySharedPort = NULL;
+
+void telemetryProcess(timeUs_t currentTime)
+{
+#ifdef USE_TELEMETRY_FRSKY_HUB
+ handleFrSkyHubTelemetry(currentTime);
+#endif
+#ifdef USE_TELEMETRY_HOTT
+ handleHoTTTelemetry(currentTime);
+#endif
+#ifdef USE_TELEMETRY_SMARTPORT
+ handleSmartPortTelemetry();
+#endif
+#ifdef USE_TELEMETRY_LTM
+ handleLtmTelemetry();
+#endif
+#ifdef USE_TELEMETRY_JETIEXBUS
+ handleJetiExBusTelemetry();
+#endif
+#ifdef USE_TELEMETRY_MAVLINK
+ handleMAVLinkTelemetry();
+#endif
+#ifdef USE_TELEMETRY_CRSF
+ handleCrsfTelemetry(currentTime);
+#endif
+#ifdef USE_TELEMETRY_GHST
+ handleGhstTelemetry(currentTime);
+#endif
+#ifdef USE_TELEMETRY_SRXL
+ handleSrxlTelemetry(currentTime);
+#endif
+#ifdef USE_TELEMETRY_IBUS
+ handleIbusTelemetry();
+#endif
+}
void telemetryCheckState(void)
{
@@ -179,46 +172,111 @@ void telemetryCheckState(void)
#endif
}
-void telemetryProcess(uint32_t currentTime)
+void INIT_CODE telemetryInit(void)
{
#ifdef USE_TELEMETRY_FRSKY_HUB
- handleFrSkyHubTelemetry(currentTime);
-#else
- UNUSED(currentTime);
+ initFrSkyHubTelemetry();
#endif
#ifdef USE_TELEMETRY_HOTT
- handleHoTTTelemetry(currentTime);
-#else
- UNUSED(currentTime);
+ initHoTTTelemetry();
#endif
#ifdef USE_TELEMETRY_SMARTPORT
- handleSmartPortTelemetry();
+ initSmartPortTelemetry();
#endif
#ifdef USE_TELEMETRY_LTM
- handleLtmTelemetry();
+ initLtmTelemetry();
#endif
#ifdef USE_TELEMETRY_JETIEXBUS
- handleJetiExBusTelemetry();
+ initJetiExBusTelemetry();
#endif
#ifdef USE_TELEMETRY_MAVLINK
- handleMAVLinkTelemetry();
-#endif
-#ifdef USE_TELEMETRY_CRSF
- handleCrsfTelemetry(currentTime);
+ initMAVLinkTelemetry();
#endif
#ifdef USE_TELEMETRY_GHST
- handleGhstTelemetry(currentTime);
+ initGhstTelemetry();
+#endif
+#ifdef USE_TELEMETRY_CRSF
+ initCrsfTelemetry();
#endif
#ifdef USE_TELEMETRY_SRXL
- handleSrxlTelemetry(currentTime);
+ initSrxlTelemetry();
#endif
#ifdef USE_TELEMETRY_IBUS
- handleIbusTelemetry();
+ initIbusTelemetry();
#endif
+#if defined(USE_MSP_OVER_TELEMETRY)
+ initSharedMsp();
+#endif
+
+ telemetryCheckState();
}
-bool telemetryIsSensorEnabled(sensor_e sensor)
+
+/** Telemetry scheduling framework **/
+
+static telemetryScheduler_t sch = INIT_ZERO;
+
+
+void INIT_CODE telemetryScheduleAdd(telemetrySensor_t * sensor)
{
- return telemetryConfig()->enableSensors & sensor;
+ if (sensor) {
+ sensor->bucket = 0;
+ sensor->value = 0;
+ sensor->update = true;
+ sensor->active = true;
+ }
}
-#endif
+
+void telemetryScheduleUpdate(timeUs_t currentTime)
+{
+ timeDelta_t delta = cmpTimeUs(currentTime, sch.update_time);
+
+ for (int i = 0; i < sch.sensor_count; i++) {
+ telemetrySensor_t * sensor = &sch.sensors[i];
+ if (sensor->active) {
+ const int value = telemetrySensorValue(sensor->telid);
+ sensor->update |= (value != sensor->value);
+ sensor->value = value;
+
+ const int interval = (sensor->update) ? sensor->min_interval : sensor->max_interval;
+ sensor->bucket += delta * 1000 / interval;
+ sensor->bucket = constrain(sensor->bucket, -1000000, 250000);
+ }
+ }
+
+ sch.update_time = currentTime;
+}
+
+telemetrySensor_t * telemetryScheduleNext(void)
+{
+ int index = sch.start_index;
+
+ for (int i = 0; i < sch.sensor_count; i++) {
+ index = (index + 1) % sch.sensor_count;
+ telemetrySensor_t * sensor = &sch.sensors[index];
+ if (sensor->active && sensor->bucket >= 0) {
+ sch.start_index = index;
+ return sensor;
+ }
+ }
+
+ return NULL;
+}
+
+void telemetryScheduleCommit(telemetrySensor_t * sensor)
+{
+ if (sensor) {
+ sensor->bucket -= 1000000;
+ sensor->update = false;
+ }
+}
+
+void INIT_CODE telemetryScheduleInit(telemetrySensor_t * sensors, size_t count)
+{
+ sch.update_time = 0;
+ sch.start_index = 0;
+ sch.sensor_count = count;
+ sch.sensors = sensors;
+}
+
+#endif /* USE_TELEMETRY */
diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h
index 0b2f3386cf..d87bfab3d9 100644
--- a/src/main/telemetry/telemetry.h
+++ b/src/main/telemetry/telemetry.h
@@ -1,52 +1,55 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Rotorflight.
*
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software under the terms of the
- * GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option)
- * any later version.
+ * Rotorflight is free software. You can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * will be useful, but WITHOUT ANY WARRANTY; without even the implied
- * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * Rotorflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
- * along with this software.
- *
- * If not, see .
- */
-
-/*
- * telemetry.h
- *
- * Created on: 6 Apr 2014
- * Author: Hydra
+ * along with this software. If not, see .
*/
#pragma once
#include "common/unit.h"
-#include "io/serial.h"
-
#include "pg/pg.h"
#include "pg/telemetry.h"
+#include "io/serial.h"
+
#include "rx/rx.h"
-#include "telemetry/ibus_shared.h"
+#include "telemetry/sensors.h"
+
+
+typedef struct {
+ timeUs_t update_time;
+ uint16_t start_index;
+ uint16_t sensor_count;
+ telemetrySensor_t * sensors;
+} telemetryScheduler_t;
extern serialPort_t *telemetrySharedPort;
-void telemetryInit(void);
+bool telemetryDetermineEnabledState(portSharing_e portSharing);
bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig, const SerialRXType serialrxProvider);
+void telemetryProcess(timeUs_t currentTime);
void telemetryCheckState(void);
-void telemetryProcess(uint32_t currentTime);
+void telemetryInit(void);
-bool telemetryDetermineEnabledState(portSharing_e portSharing);
+telemetrySensor_t * telemetryScheduleNext(void);
+
+void telemetryScheduleAdd(telemetrySensor_t * sensor);
+void telemetryScheduleUpdate(timeUs_t currentTime);
+void telemetryScheduleCommit(telemetrySensor_t * sensor);
+void telemetryScheduleInit(telemetrySensor_t * sensors, size_t count);
-bool telemetryIsSensorEnabled(sensor_e sensor);