From 7e973c2635d682000e5568fea0b81ca7817a0338 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Tue, 4 Jun 2024 11:51:44 +0100 Subject: [PATCH] Refactor CRSF telemetry This commit introduces Custom CRSF Telemetry. It is using a "custom" CRSF frame (0x88) for passing the telemetry data to the Tx. On the Tx side, a LUA script is parsing these frames and injecting the sensors into the native sensor framework. The FC has over hundred telemetry sensors to choose from. Up to 40 can be enabled at the same time. Depending on the link bandwidth (up to 20kbit/s with ELRS), sensor update rates up to 50Hz are achievable. This framework is also fully extensible in the future. For adding new sensors, changes in the firmware and the LUA script is needed. The Receiver or the Transmitter are not involved in the process. --- make/source.mk | 1 + src/main/cli/settings.c | 41 +- src/main/cli/settings.h | 5 +- src/main/fc/tasks.c | 4 +- src/main/msp/msp.c | 14 + src/main/pg/telemetry.c | 22 +- src/main/pg/telemetry.h | 75 +- src/main/rx/crsf.c | 31 +- src/main/rx/crsf.h | 4 +- src/main/rx/crsf_protocol.h | 5 + src/main/rx/rx.c | 2 + src/main/sensors/battery.c | 6 + src/main/sensors/battery.h | 1 + src/main/target/STM32_UNIFIED/target.h | 2 + src/main/telemetry/crsf.c | 1692 +++++++++++++----------- src/main/telemetry/crsf.h | 96 +- src/main/telemetry/sensors.c | 511 +++++++ src/main/telemetry/sensors.h | 198 +++ src/main/telemetry/telemetry.c | 228 ++-- src/main/telemetry/telemetry.h | 55 +- 20 files changed, 1908 insertions(+), 1085 deletions(-) create mode 100644 src/main/telemetry/sensors.c create mode 100644 src/main/telemetry/sensors.h 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);