From 942226b1097316928d3f994643f512b67db37761 Mon Sep 17 00:00:00 2001 From: Error414 Date: Thu, 27 Nov 2025 22:45:12 +0100 Subject: [PATCH 1/5] * Refactor CRSF and SmartPort telemetry * Telemetry scheduler for CRSF and SmartPort --- docs/Settings.md | 50 +- src/main/CMakeLists.txt | 4 + src/main/common/crc.c | 14 + src/main/common/crc.h | 7 +- src/main/fc/cli.c | 74 +++ src/main/fc/fc_core.c | 7 + src/main/fc/fc_init.c | 2 + src/main/fc/fc_msp.c | 34 ++ src/main/fc/runtime_config.c | 3 +- src/main/fc/runtime_config.h | 7 +- src/main/fc/settings.yaml | 36 +- src/main/io/osd.c | 2 + src/main/io/osd_dji_hd.c | 2 + src/main/msp/msp_protocol_v2_inav.h | 5 + src/main/rx/crsf.c | 5 +- src/main/rx/crsf.h | 2 + src/main/target/common.h | 7 + src/main/telemetry/crsf.c | 776 +++++++++++++++++++------- src/main/telemetry/crsf.h | 1 + src/main/telemetry/sensors.c | 442 +++++++++++++++ src/main/telemetry/sensors.h | 159 ++++++ src/main/telemetry/smartport.c | 595 ++++++-------------- src/main/telemetry/smartport.h | 8 +- src/main/telemetry/smartport_legacy.c | 399 +++++++++++++ src/main/telemetry/smartport_legacy.h | 44 ++ src/main/telemetry/telemetry.c | 111 +++- src/main/telemetry/telemetry.h | 48 +- 27 files changed, 2196 insertions(+), 648 deletions(-) create mode 100644 src/main/telemetry/sensors.c create mode 100644 src/main/telemetry/sensors.h create mode 100644 src/main/telemetry/smartport_legacy.c create mode 100644 src/main/telemetry/smartport_legacy.h diff --git a/docs/Settings.md b/docs/Settings.md index b44f1171df0..f40165cbbc9 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -582,6 +582,36 @@ Blackbox logging rate numerator. Use num/denom settings to decide if a frame sho --- +### crsf_telemetry_link_rate + +CRSF telemetry link rate + +| Default | Min | Max | +| --- | --- | --- | +| 250 | 0 | 50000 | + +--- + +### crsf_telemetry_link_ratio + +CRSF telemetry link ratio + +| Default | Min | Max | +| --- | --- | --- | +| 8 | 0 | 50000 | + +--- + +### crsf_telemetry_mode + +Use extended custom or native telemetry sensors for CRSF + +| Default | Min | Max | +| --- | --- | --- | +| NATIVE | | | + +--- + ### cruise_power Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit @@ -1192,16 +1222,6 @@ S.Port telemetry: Send pitch and roll degrees*10 instead of raw accelerometer da --- -### frsky_use_legacy_gps_mode_sensor_ids - -S.Port telemetry: If `ON`, send the legacy telemetry IDs for modes (Tmp1) and GNSS (Tmp2). These are old IDs, deprecated, and will be removed in INAV 10.0. Tools and scripts using these IDs should be updated to use the new IDs of **470** for Modes and **480** for GNSS. Default: 'OFF' - -| Default | Min | Max | -| --- | --- | --- | -| OFF | OFF | ON | - ---- - ### fw_autotune_max_rate_deflection The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`. @@ -6202,6 +6222,16 @@ _// TODO_ --- +### smartport_telemetry_mode + +Use legacy or standard telemetry sensors for SmartPort + +| Default | Min | Max | +| --- | --- | --- | +| LEGACY | | | + +--- + ### smith_predictor_delay Expected delay of the gyro signal. In milliseconds diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 101c7223372..c6d7ec82b96 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -626,6 +626,10 @@ main_sources(COMMON_SRC telemetry/sim.h telemetry/telemetry.c telemetry/telemetry.h + telemetry/sensors.c + telemetry/sensors.h + telemetry/smartport_legacy.c + telemetry/smartport_legacy.h ) add_subdirectory(target) diff --git a/src/main/common/crc.c b/src/main/common/crc.c index d11d37ae9ed..31ddab1c4a5 100644 --- a/src/main/common/crc.c +++ b/src/main/common/crc.c @@ -142,4 +142,18 @@ uint8_t crc8_sum_update(uint8_t crc, const void *data, uint32_t length) crc += *p; } return crc; +} + +// Fowler–Noll–Vo hash function; see https://en.wikipedia.org/wiki/Fowler–Noll–Vo_hash_function +uint32_t fnv_update(uint32_t hash, const void *data, uint32_t length) +{ + const uint8_t *ptr = data; + const uint8_t *pend = ptr + length; + + while (ptr != pend) { + hash *= FNV_PRIME; + hash ^= *ptr++; + } + + return hash; } \ No newline at end of file diff --git a/src/main/common/crc.h b/src/main/common/crc.h index 9e02cc88d59..1425e07510f 100644 --- a/src/main/common/crc.h +++ b/src/main/common/crc.h @@ -19,6 +19,8 @@ #include +#define FNV_PRIME 16777619 + struct sbuf_s; uint16_t crc16_ccitt(uint16_t crc, unsigned char a); @@ -35,4 +37,7 @@ void crc8_xor_sbuf_append(struct sbuf_s *dst, uint8_t *start); uint8_t crc8(uint8_t crc, uint8_t a); uint8_t crc8_update(uint8_t crc, const void *data, uint32_t length); -uint8_t crc8_sum_update(uint8_t crc, const void *data, uint32_t length); \ No newline at end of file +uint8_t crc8_sum_update(uint8_t crc, const void *data, uint32_t length); + +uint32_t fnv_update(uint32_t hash, const void *data, uint32_t length); + diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 6c60f08c6ed..544f294af45 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -918,6 +918,72 @@ static void cliSerial(char *cmdline) memcpy(currentConfig, &portConfig, sizeof(portConfig)); } +#ifdef USE_CUSTOM_TELEMETRY +static void printTelemetrySensors(uint8_t dumpMask, const telemetryConfig_t *telemetryConfig, const telemetryConfig_t *telemetryConfigDefault) +{ + const char *format = "ts %d %d"; + bool equalsDefault = false; + for (size_t i = 0; i < TELEM_SENSOR_SLOT_COUNT; ++i) { + if(telemetryConfigDefault) { + equalsDefault = telemetryConfig->telemetry_sensors[i] == telemetryConfigDefault->telemetry_sensors[i]; + cliDefaultPrintLinef(dumpMask, equalsDefault, format, + i, + telemetryConfigDefault->telemetry_sensors[i] + ); + } + + cliDumpPrintLinef(dumpMask, equalsDefault, format, + i, + telemetryConfig->telemetry_sensors[i] + ); + } +} + +static void cliTelemetrySensors(char *cmdline) +{ + char * saveptrMain; + char * saveptrParams; + int args[2], check = 0; + uint8_t len = strlen(cmdline); + + if (len == 0) { + printTelemetrySensors(DUMP_MASTER, telemetryConfig(), NULL); + } else { + //split by ", first are params second is text + char *ptrMain = strtok_r(cmdline, "\"", &saveptrMain); + enum { + INDEX = 0, + VALUE = 1, + ARGS_COUNT + }; + + char *ptrParams = strtok_r(ptrMain, " ", &saveptrParams); + while (ptrParams != NULL && check < ARGS_COUNT) { + args[check++] = fastA2I(ptrParams); + ptrParams = strtok_r(NULL, " ", &saveptrParams); + } + + if (check != ARGS_COUNT) { + cliShowParseError(); + return; + } + + if(args[INDEX] < 0 || args[INDEX] >= TELEM_SENSOR_SLOT_COUNT) { + cliShowArgumentRangeError("index", 0, TELEM_SENSOR_SLOT_COUNT - 1); + return; + } + + if(args[VALUE] < 0 || args[VALUE] >= TELEM_SENSOR_COUNT) { + cliShowArgumentRangeError("value", 0, TELEM_SENSOR_COUNT - 1); + return; + } + telemetryConfigMutable()->telemetry_sensors[args[INDEX]] = args[VALUE]; + + printTelemetrySensors(DUMP_MASTER, telemetryConfig(), NULL); + } +} +#endif + #ifdef USE_SERIAL_PASSTHROUGH portOptions_t constructPortOptions(char *options) { @@ -4480,6 +4546,11 @@ static void printConfig(const char *cmdline, bool doDiff) cliPrintHashLine("features"); printFeature(dumpMask, &featureConfig_Copy, featureConfig()); +#ifdef USE_CUSTOM_TELEMETRY + cliPrintHashLine("telemetry sensors"); + printTelemetrySensors(dumpMask, &telemetryConfig_Copy, telemetryConfig()); +#endif + #if defined(BEEPER) || defined(USE_DSHOT) cliPrintHashLine("beeper"); printBeeper(dumpMask, &beeperConfig_Copy, beeperConfig()); @@ -4900,6 +4971,9 @@ const clicmd_t cmdTable[] = { #endif CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave), CLI_COMMAND_DEF("serial", "configure serial ports", NULL, cliSerial), +#ifdef USE_CUSTOM_TELEMETRY + CLI_COMMAND_DEF("telemetry_sensors", "configure telemetry sensors", NULL, cliTelemetrySensors), +#endif #ifdef USE_SERIAL_PASSTHROUGH CLI_COMMAND_DEF("serialpassthrough", "passthrough serial data to port", " [baud] [mode] [options]: passthrough to serial", cliSerialPassthrough), #endif diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index ce874c8aedc..3e78d540924 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -203,6 +203,13 @@ static void updateArmingStatus(void) if (ARMING_FLAG(ARMED)) { LED0_ON; } else { + + // Check if the power on arming grace time has elapsed + if ((isArmingDisabledReason() & ARMING_DISABLED_BOOT_GRACE_TIME) && (millis() >= 3000)) { + // If so, unset the grace time arming disable flag + DISABLE_ARMING_FLAG(ARMING_DISABLED_BOOT_GRACE_TIME); + } + /* CHECK: Run-time calibration */ static bool calibratingFinishedBeep = false; if (areSensorsCalibrating()) { diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index a8ca6c0c199..fb28183c98a 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -611,6 +611,8 @@ void init(void) } #endif + ENABLE_ARMING_FLAG(ARMING_DISABLED_BOOT_GRACE_TIME); + #ifdef USE_BLACKBOX //Do not allow blackbox to be run faster that 1kHz. It can cause UAV to drop dead when digital ESC protocol is used diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 936c708030d..7248ec2e0e1 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3608,6 +3608,29 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } break; +#endif +#ifdef USE_CUSTOM_TELEMETRY + case MSP2_INAV_SET_TELEMETRY_SENSORS: + if (dataSize != TELEM_SENSOR_SLOT_COUNT * 2) { + return MSP_RESULT_ERROR; + } + + for (unsigned int i = 0; i < ARRAYLEN(telemetryConfig()->telemetry_sensors); i++){ + telemetryConfigMutable()->telemetry_sensors[i] = sbufReadU16(src); + } + break; + + case MSP2_INAV_SET_TELEMETRY_SENSOR: + if(!sbufReadU16Safe(&tmp_u16, src)) { + return MSP_RESULT_ERROR; + } + + if (tmp_u16 >= TELEM_SENSOR_SLOT_COUNT) { + return MSP_RESULT_ERROR; + } + + telemetryConfigMutable()->telemetry_sensors[tmp_u16] = MIN(TELEM_SENSOR_COUNT, sbufReadU16(src)); + break; #endif case MSP2_BETAFLIGHT_BIND: if (rxConfig()->receiverType == RX_TYPE_SERIAL) { @@ -4110,6 +4133,17 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu } break; #endif + case MSP2_INAV_TELEMETRY_SENSORS: +#ifdef USE_CUSTOM_TELEMETRY + sbufWriteU16(dst, TELEM_SENSOR_SLOT_COUNT); + for (unsigned int i = 0; i < ARRAYLEN(telemetryConfig()->telemetry_sensors); ++i) { + sbufWriteU16(dst, telemetryConfig()->telemetry_sensors[i]); + } +#else + sbufWriteU16(dst, 0); +#endif + *ret = MSP_RESULT_ACK; + break; #ifdef USE_SAFE_HOME case MSP2_INAV_SAFEHOME: *ret = mspFcSafeHomeOutCommand(dst, src); diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 892df87b3ac..012f4539654 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -58,7 +58,8 @@ const armingFlag_e armDisableReasonsChecklist[] = { ARMING_DISABLED_SERVO_AUTOTRIM, ARMING_DISABLED_OOM, ARMING_DISABLED_NO_PREARM, - ARMING_DISABLED_DSHOT_BEEPER + ARMING_DISABLED_DSHOT_BEEPER, + ARMING_DISABLED_BOOT_GRACE_TIME }; armingFlag_e isArmingDisabledReason(void) diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 452256a6b64..7b352d52769 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -48,6 +48,7 @@ typedef enum { ARMING_DISABLED_NO_PREARM = (1 << 28), ARMING_DISABLED_DSHOT_BEEPER = (1 << 29), ARMING_DISABLED_LANDING_DETECTED = (1 << 30), + ARMING_DISABLED_BOOT_GRACE_TIME = (1 << 31), ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_GEOZONE | ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE | @@ -57,7 +58,7 @@ typedef enum { ARMING_DISABLED_CMS_MENU | ARMING_DISABLED_OSD_MENU | ARMING_DISABLED_ROLLPITCH_NOT_CENTERED | ARMING_DISABLED_SERVO_AUTOTRIM | ARMING_DISABLED_OOM | ARMING_DISABLED_INVALID_SETTING | ARMING_DISABLED_PWM_OUTPUT_ERROR | ARMING_DISABLED_NO_PREARM | ARMING_DISABLED_DSHOT_BEEPER | - ARMING_DISABLED_LANDING_DETECTED), + ARMING_DISABLED_LANDING_DETECTED | ARMING_DISABLED_BOOT_GRACE_TIME), } armingFlag_e; // Arming blockers that can be overriden by emergency arming. @@ -71,7 +72,9 @@ typedef enum { | ARMING_DISABLED_COMPASS_NOT_CALIBRATED \ | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED \ | ARMING_DISABLED_ARM_SWITCH \ - | ARMING_DISABLED_HARDWARE_FAILURE) + | ARMING_DISABLED_HARDWARE_FAILURE \ + | ARMING_DISABLED_BOOT_GRACE_TIME \ + ) extern uint32_t armingFlags; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c871e9caeac..a56c6470b45 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -103,6 +103,12 @@ tables: - name: bat_voltage_source values: ["RAW", "SAG_COMP"] enum: batVoltageSource_e + - name: crsf_telemetry_modes + values: [ "OFF", "NATIVE", "CUSTOM" ] + enum: crsfTelemetryMode_e + - name: smartport_telemetry_modes + values: [ "LEGACY", "STANDARD" ] + enum: smartportTelemetryMode_e - name: smartport_fuel_unit values: ["PERCENT", "MAH", "MWH"] enum: smartportFuelUnit_e @@ -3117,10 +3123,32 @@ groups: description: "S.Port telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data" default_value: OFF type: bool - - name: frsky_use_legacy_gps_mode_sensor_ids - description: "S.Port telemetry: If `ON`, send the legacy telemetry IDs for modes (Tmp1) and GNSS (Tmp2). These are old IDs, deprecated, and will be removed in INAV 10.0. Tools and scripts using these IDs should be updated to use the new IDs of **470** for Modes and **480** for GNSS. Default: 'OFF'" - default_value: OFF - type: bool + - name: crsf_telemetry_mode + description: "Use extended custom or native telemetry sensors for CRSF" + default_value: NATIVE + condition: USE_CUSTOM_TELEMETRY + table: crsf_telemetry_modes + type: uint8_t + - name: smartport_telemetry_mode + description: "Use legacy or standard telemetry sensors for SmartPort" + default_value: LEGACY + condition: USE_CUSTOM_TELEMETRY + table: smartport_telemetry_modes + type: uint8_t + - name: crsf_telemetry_link_rate + description: "CRSF telemetry link rate" + condition: USE_TELEMETRY_CRSF + default_value: 250 + type: uint16_t + min: 0 + max: 50000 + - name: crsf_telemetry_link_ratio + description: "CRSF telemetry link ratio" + default_value: 8 + condition: USE_TELEMETRY_CRSF + type: uint16_t + min: 0 + max: 50000 - name: report_cell_voltage description: "S.Port and IBUS telemetry: Send the average cell voltage if set to ON" default_value: OFF diff --git a/src/main/io/osd.c b/src/main/io/osd.c index c7a40e982cc..247bdbaa314 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -966,6 +966,8 @@ static const char * osdArmingDisabledReasonMessage(void) // Cases without message case ARMING_DISABLED_LANDING_DETECTED: FALLTHROUGH; + case ARMING_DISABLED_BOOT_GRACE_TIME: + FALLTHROUGH; case ARMING_DISABLED_CMS_MENU: FALLTHROUGH; case ARMING_DISABLED_OSD_MENU: diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index a8adf663d57..5d019c45c96 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -520,6 +520,8 @@ static char * osdArmingDisabledReasonMessage(void) // Cases without message case ARMING_DISABLED_GEOZONE: return OSD_MESSAGE_STR("NO FLY ZONE"); + case ARMING_DISABLED_BOOT_GRACE_TIME: + FALLTHROUGH; case ARMING_DISABLED_LANDING_DETECTED: FALLTHROUGH; case ARMING_DISABLED_CMS_MENU: diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 696d426cd78..02b3c89bdea 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -125,3 +125,8 @@ #define MSP2_INAV_SET_GVAR 0x2214 #define MSP2_INAV_FULL_LOCAL_POSE 0x2220 + + +#define MSP2_INAV_TELEMETRY_SENSORS 0x222A +#define MSP2_INAV_SET_TELEMETRY_SENSORS 0x222B +#define MSP2_INAV_SET_TELEMETRY_SENSOR 0x222C \ No newline at end of file diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 6ac184b9c66..526b98c40e8 100755 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -32,7 +32,6 @@ #include "drivers/time.h" #include "drivers/serial.h" -#include "drivers/serial_uart.h" #include "io/serial.h" #include "io/osd.h" @@ -179,6 +178,10 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *rxCallbackData) } break; } + + case CRSF_FRAMETYPE_DEVICE_PING: + crsfScheduleDeviceInfoResponse(); + break; #endif default: break; diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 8d84d52b8e5..0ed1696b8d4 100755 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -88,6 +88,7 @@ typedef enum { CRSF_FRAMETYPE_VARIO_SENSOR = 0x07, CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, CRSF_FRAMETYPE_BAROMETER_ALTITUDE = 0x09, + CRSF_FRAMETYPE_HEARTBEAT = 0x0B, CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, CRSF_FRAMETYPE_ATTITUDE = 0x1E, @@ -104,6 +105,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 { diff --git a/src/main/target/common.h b/src/main/target/common.h index 45eb12ac4bc..85f95c8fae0 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -153,6 +153,11 @@ #define USE_TELEMETRY_SMARTPORT #define USE_TELEMETRY_CRSF #define USE_TELEMETRY_JETIEXBUS + +#if defined(USE_TELEMETRY_SMARTPORT) || defined(USE_TELEMETRY_CRSF) +#define USE_CUSTOM_TELEMETRY +#endif + // These are rather exotic serial protocols #define USE_RX_MSP #define USE_MSP_RC_OVERRIDE @@ -222,6 +227,8 @@ #define SKIP_CLI_COMMAND_HELP #undef USE_SERIALRX_SPEKTRUM #undef USE_TELEMETRY_SRXL + #undef USE_CUSTOM_TELEMETRY + #undef USE_ADSB #endif #define USE_EZ_TUNE diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index d86822ada7a..a85497ef16c 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -24,7 +24,6 @@ #if defined(USE_TELEMETRY) && defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF) #include "build/atomic.h" -#include "build/build_config.h" #include "build/version.h" #include "common/axis.h" @@ -36,34 +35,30 @@ #include "config/feature.h" -#include "drivers/serial.h" -#include "drivers/time.h" #include "drivers/nvic.h" #include "fc/config.h" -#include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/imu.h" +#include "flight/mixer.h" #include "io/gps.h" -#include "io/serial.h" #include "navigation/navigation.h" #include "rx/crsf.h" -#include "rx/rx.h" #include "sensors/battery.h" -#include "sensors/sensors.h" +#include "sensors/esc_sensor.h" +#include "sensors/temperature.h" #include "telemetry/crsf.h" #include "telemetry/telemetry.h" #include "telemetry/msp_shared.h" -#define CRSF_CYCLETIME_US 100000 // 100ms, 10 Hz #define CRSF_DEVICEINFO_VERSION 0x01 // According to TBS: "CRSF over serial should always use a sync byte at the beginning of each frame. // To get better performance it's recommended to use the sync byte 0xC8 to get better performance" @@ -75,11 +70,24 @@ #define CRSF_MSP_LENGTH_OFFSET 1 static uint8_t crsfCrc; -static bool crsfTelemetryEnabled; static bool deviceInfoReplyPending; +static sbuf_t crsfSbuf; static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX]; +///////////////////////////////////////////////////// +#define CRSF_CUSTOM_TELEMETRY_MIN_SPACE 32 + +static uint8_t crsfTelemetryState; +static float crsfTelemetryRateBucket; +static float crsfTelemetryRateQuanta; + +static uint8_t crsfCustomTelemetryFrameId; +static timeUs_t crsfTelemetryUpdateTime; +///////////////////////////////////////////////////// + #if defined(USE_MSP_OVER_TELEMETRY) +///////////////////////////////////////////////////////// +///////////////// MSP ///////////////////////////// typedef struct mspBuffer_s { uint8_t bytes[CRSF_MSP_BUFFER_SIZE]; int len; @@ -87,11 +95,6 @@ typedef struct mspBuffer_s { static mspBuffer_t mspRxBuffer; -void initCrsfMspBuffer(void) -{ - mspRxBuffer.len = 0; -} - bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength) { if (mspRxBuffer.len + CRSF_MSP_LENGTH_OFFSET + frameLength > CRSF_MSP_BUFFER_SIZE) { @@ -138,15 +141,20 @@ bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn) } return replyPending; } +///////////////////////////////////////////////////////// +///////////////////////////////////////////////////////// #endif -static void crsfInitializeFrame(sbuf_t *dst) +///////////////////////////////////////////////////////// +static inline size_t crsfLinkFrameSlots(size_t bytes) { - crsfCrc = 0; - dst->ptr = crsfFrame; - dst->end = ARRAYEND(crsfFrame); + // Telemetry data is send in 5 byte slots, with 1 slot overhead + return (bytes + 9) / 5; +} - sbufWriteU8(dst, CRSF_TELEMETRY_SYNC_BYTE); +static inline void crsfTelemetryRateConsume(size_t slots) +{ + crsfTelemetryRateBucket -= slots; } static void crsfSerialize8(sbuf_t *dst, uint8_t v) @@ -155,14 +163,24 @@ static void crsfSerialize8(sbuf_t *dst, uint8_t v) crsfCrc = crc8_dvb_s2(crsfCrc, v); } -static void crsfSerialize16(sbuf_t *dst, uint16_t v) +static void crsfSerialize16BE(sbuf_t *dst, uint16_t v) { // Use BigEndian format crsfSerialize8(dst, (v >> 8)); crsfSerialize8(dst, (uint8_t)v); } -static void crsfSerialize32(sbuf_t *dst, uint32_t v) +#ifdef USE_CUSTOM_TELEMETRY +static void crsfSerialize24BE(sbuf_t *dst, uint32_t v) +{ + // Use BigEndian format + crsfSerialize8(dst, (v >> 16)); + crsfSerialize8(dst, (v >> 8)); + crsfSerialize8(dst, (uint8_t)v); +} +#endif + +static void crsfSerialize32BE(sbuf_t *dst, uint32_t v) { // Use BigEndian format crsfSerialize8(dst, (v >> 24)); @@ -181,22 +199,283 @@ static void crsfSerializeData(sbuf_t *dst, const uint8_t *data, int len) static void crsfFinalize(sbuf_t *dst) { sbufWriteU8(dst, crsfCrc); + + // Consume telemetry rate + crsfTelemetryRateConsume(crsfLinkFrameSlots(dst->ptr - crsfFrame)); + sbufSwitchToReader(dst, crsfFrame); // write the telemetry frame to the receiver. crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst)); } -static int crsfFinalizeBuf(sbuf_t *dst, uint8_t *frame) +#ifdef USE_CUSTOM_TELEMETRY +static void crsfFrameCustomTelemetrySensor(sbuf_t *dst, telemetrySensor_t * sensor) { - sbufWriteU8(dst, crsfCrc); - sbufSwitchToReader(dst, crsfFrame); - const int frameSize = sbufBytesRemaining(dst); - for (int ii = 0; sbufBytesRemaining(dst); ++ii) { - frame[ii] = sbufReadU8(dst); + crsfSerialize16BE(dst, sensor->app_id); + sensor->encode(sensor, dst); +} + +static void crsfFillCustomSensorHeader(sbuf_t *dst) +{ + sbufWriteU8(dst, 0); // placeholder for [SIZE] + crsfSerialize8(dst, CRSF_FRAMETYPE_CUSTOM_TELEM); + crsfSerialize8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER); + crsfSerialize8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); + crsfSerialize8(dst, crsfCustomTelemetryFrameId); +} +#endif + +//create start of CRSF header without length and command +static void crsfInitializeFrame(sbuf_t *dst) +{ + crsfCrc = 0; + sbufWriteU8(dst, CRSF_TELEMETRY_SYNC_BYTE); +} + +//sensor encoder NIL +void crsfSensorEncodeNil(__unused telemetrySensor_t *sensor, __unused sbuf_t *buf) +{ +} + +#ifdef USE_CUSTOM_TELEMETRY +void crsfSensorEncodeU8(telemetrySensor_t *sensor, sbuf_t *buf) +{ + crsfSerialize8(buf, constrain((uint8_t)sensor->value, 0, 0xFF)); +} + +void crsfSensorEncodeU16(telemetrySensor_t *sensor, sbuf_t *buf) +{ + crsfSerialize16BE(buf, constrain((uint16_t)sensor->value, 0, 0xFFFF)); +} + +void crsfSensorEncodeU24(telemetrySensor_t *sensor, sbuf_t *buf) +{ + crsfSerialize24BE(buf, constrain((uint32_t)sensor->value, 0, 0xFFFFFF)); +} + +void crsfSensorEncodeU32(telemetrySensor_t *sensor, sbuf_t *buf) +{ + crsfSerialize32BE(buf, sensor->value); +} + +void crsfSensorEncodeCellVolt(telemetrySensor_t *sensor, sbuf_t *buf) +{ + const int volt = constrain(sensor->value, 200, 455) - 200; + crsfSerialize8(buf, volt); +} + +void crsfSensorEncodeCells(__unused telemetrySensor_t *sensor, sbuf_t *buf) +{ + const int cells = MIN(getBatteryCellCount(), 16); + crsfSerialize8(buf, cells); + for (int i = 0; i < cells; i++) { + int volt = constrain(getBatteryAverageCellVoltage(), 200, 455) - 200; + crsfSerialize8(buf, volt); + } +} + +void crsfSensorEncodeAttitude(__unused telemetrySensor_t *sensor, sbuf_t *buf) +{ + crsfSerialize16BE(buf, attitude.values.pitch); + crsfSerialize16BE(buf, attitude.values.roll); + crsfSerialize16BE(buf, attitude.values.yaw); +} + + +void crsfSensorEncodeLatLong(telemetrySensor_t *sensor, sbuf_t *buf) +{ + UNUSED(sensor); + crsfSerialize32BE(buf, gpsSol.llh.lat); + crsfSerialize32BE(buf, gpsSol.llh.lon); +} + +void crsfSensorEncodeEscRpm(telemetrySensor_t *sensor, sbuf_t *buf) +{ + UNUSED(sensor); + uint8_t motorCount = MAX(getMotorCount(), 1); //must send at least one motor, to avoid CRSF frame shifting + motorCount = MIN(getMotorCount(), CRSF_PAYLOAD_SIZE_MAX / 3); // 3 bytes per RPM value + motorCount = MIN(motorCount, MAX_SUPPORTED_MOTORS); // ensure we don't exceed available ESC telemetry data + + for (uint8_t i = 0; i < motorCount; i++) { + const escSensorData_t *escState = getEscTelemetry(i); + crsfSerialize24BE(buf, escState->rpm & 0xFFFFFF); } - return frameSize; } +void crsfSensorEncodeEscTemp(telemetrySensor_t *sensor, sbuf_t *buf) +{ + UNUSED(sensor); + uint8_t motorCount = MAX(getMotorCount(), 1); //must send at least one motor, to avoid CRSF frame shifting + motorCount = MIN(getMotorCount(), CRSF_PAYLOAD_SIZE_MAX / 3); // 3 bytes per RPM value + motorCount = MIN(motorCount, MAX_SUPPORTED_MOTORS); // ensure we don't exceed available ESC telemetry data + + for (uint8_t i = 0; i < motorCount; i++) { + const escSensorData_t *escState = getEscTelemetry(i); + crsfSerialize16BE(buf, escState->temperature & 0xFFFF); + } +} + +void crsfSensorEncodeEscTemperature(telemetrySensor_t *sensor, sbuf_t *buf) +{ + UNUSED(sensor); + + uint8_t motorCount = MAX(getMotorCount(), 1); //must send at least one motor, to avoid CRSF frame shifting + motorCount = MIN(getMotorCount(), CRSF_PAYLOAD_SIZE_MAX / 3); // 3 bytes per RPM value + motorCount = MIN(motorCount, MAX_SUPPORTED_MOTORS); // ensure we don't exceed available ESC telemetry data + + for (uint8_t i = 0; i < motorCount; i++) { + const escSensorData_t *escState = getEscTelemetry(i); + uint32_t rpm = (escState) ? (escState->temperature * 10) & 0xFFFFFF : TEMPERATURE_INVALID_VALUE; + crsfSerialize24BE(buf, rpm); + } + +} + +#endif // USE_CUSTOM_TELEMETRY + +/////////////////////////////////////////////////////////////////////////////////////////// + + +///////////////////////////////////////////////////////// +///////////////////////////////////////////////////////// + +static bool crsfCanTransmitTelemetry(void) +{ + return (crsfTelemetryRateBucket >= 0) && !(isArmingDisabledReason() & ARMING_DISABLED_BOOT_GRACE_TIME); +} + +static void crsfTelemetryRateUpdate(timeUs_t currentTimeUs) +{ + timeDelta_t delta = cmpTimeUs(currentTimeUs, crsfTelemetryUpdateTime); + + crsfTelemetryRateBucket += crsfTelemetryRateQuanta * delta; + crsfTelemetryRateBucket = constrainf(crsfTelemetryRateBucket, -25, 1); + + crsfTelemetryUpdateTime = currentTimeUs; + + telemetryScheduleUpdate(currentTimeUs); +} + +//initialize buffer for sending telemetry +static sbuf_t * crsfInitializeSbuf(void) +{ + sbuf_t * dst = &crsfSbuf; + + dst->ptr = crsfFrame; + dst->end = crsfFrame + CRSF_FRAME_SIZE_MAX; + + return dst; +} + +#define TLM_SENSOR(NAME, APPID, FAST, SLOW, DENOM, ENCODER) \ +{ \ + .sensor_id = TELEM_##NAME, \ + .app_id = (APPID), \ + .fast_interval = (FAST), \ + .slow_interval = (SLOW), \ + .fast_weight = 0, \ + .slow_weight = 0, \ + .ratio_num = 1, \ + .ratio_den = (DENOM), \ + .value = 0, \ + .bucket = 0, \ + .update = 0, \ + .active = false, \ + .encode = (telemetryEncode_f)crsfSensorEncode##ENCODER, \ +} + +/////////////////////////////////////////////////////////////////////// +////// LEGACY sensors, don't add new sensors here ///////////////////// +static telemetrySensor_t crsfNativeTelemetrySensors[] = +{ + TLM_SENSOR(FLIGHT_MODE, 0, 100, 100, 0, Nil), + TLM_SENSOR(BATTERY, 0, 100, 100, 0, Nil), + TLM_SENSOR(ATTITUDE, 0, 100, 100, 0, Nil), +#ifdef USE_BARO + TLM_SENSOR(ALTITUDE, 0, 100, 100, 0, Nil), +#endif +#if defined(USE_BARO) || defined(USE_GPS) + TLM_SENSOR(VARIOMETER, 0, 100, 100, 0, Nil), +#endif +#ifdef USE_GPS + TLM_SENSOR(GPS, 0, 100, 100, 0, Nil), +#endif +}; +/////////////////////////////////////////////////////////////////////// +#ifdef USE_CUSTOM_TELEMETRY +static telemetrySensor_t crsfCustomTelemetrySensors[] = +{ + TLM_SENSOR(NONE, 0x1000, 1000, 1000, 0, Nil), + TLM_SENSOR(HEARTBEAT, 0x1001, 1000, 1000, 0, U16), + + TLM_SENSOR(BATTERY_VOLTAGE, 0x1011, 200, 3000, 0, U16), + TLM_SENSOR(BATTERY_CURRENT, 0x1012, 200, 3000, 0, U16), + TLM_SENSOR(BATTERY_CONSUMPTION, 0x1013, 200, 3000, 0, U16), + TLM_SENSOR(BATTERY_CHARGE_LEVEL, 0x1014, 200, 3000, 0, U8), + TLM_SENSOR(BATTERY_CELL_COUNT, 0x1020, 200, 3000, 0, U8), + TLM_SENSOR(BATTERY_CELL_VOLTAGE, 0x1021, 200, 3000, 0, CellVolt), + TLM_SENSOR(BATTERY_CELL_VOLTAGES, 0x102F, 200, 3000, 0, Cells), + +#ifdef USE_BARO + TLM_SENSOR(ALTITUDE, 0x10B2, 200, 3000, 0, U24), + TLM_SENSOR(VARIOMETER, 0x10B3, 200, 3000, 0, U16), +#endif + TLM_SENSOR(HEADING, 0x10B1, 200, 3000, 0, U16), + + TLM_SENSOR(ATTITUDE, 0x1100, 100, 3000, 0, Attitude), + TLM_SENSOR(ATTITUDE_PITCH, 0x1101, 200, 3000, 10, U16), + TLM_SENSOR(ATTITUDE_ROLL, 0x1102, 200, 3000, 10, U16), + TLM_SENSOR(ATTITUDE_YAW, 0x1103, 200, 3000, 10, U16), + + TLM_SENSOR(ACCEL_X, 0x1111, 200, 3000, 100, U16), + TLM_SENSOR(ACCEL_Y, 0x1112, 200, 3000, 100, U16), + TLM_SENSOR(ACCEL_Z, 0x1113, 200, 3000, 100, U16), + +#ifdef USE_GPS + TLM_SENSOR(GPS_SATS, 0x1121, 500, 3000, 0, U8), + TLM_SENSOR(GPS_HDOP, 0x1123, 500, 3000, 0, U8), + TLM_SENSOR(GPS_COORD, 0x1125, 200, 3000, 0, LatLong), + TLM_SENSOR(GPS_ALTITUDE, 0x1126, 200, 3000, 0, U16), + TLM_SENSOR(GPS_HEADING, 0x1127, 200, 3000, 0, U16), + TLM_SENSOR(GPS_GROUNDSPEED, 0x1128, 200, 3000, 0, U16), + TLM_SENSOR(GPS_HOME_DISTANCE, 0x1129, 200, 3000, 0, U16), + TLM_SENSOR(GPS_HOME_DIRECTION, 0x112A, 200, 3000, 0, U16), + TLM_SENSOR(GPS_AZIMUTH, 0x112B, 200, 3000, 0, U16), +#endif + +#ifdef USE_ESC_SENSOR + TLM_SENSOR(ESC_RPM, 0x1131, 200, 3000, 0, EscRpm), + TLM_SENSOR(ESC1_RPM, 0x1132, 200, 3000, 0, U16), + TLM_SENSOR(ESC2_RPM, 0x1133, 200, 3000, 0, U16), + TLM_SENSOR(ESC3_RPM, 0x1134, 200, 3000, 0, U16), + TLM_SENSOR(ESC4_RPM, 0x1135, 200, 3000, 0, U16), + + TLM_SENSOR(ESC_TEMPERATURE, 0x1136, 200, 3000, 0, EscTemp), + TLM_SENSOR(ESC1_TEMPERATURE, 0x1137, 200, 3000, 0, U8), + TLM_SENSOR(ESC2_TEMPERATURE, 0x1138, 200, 3000, 0, U8), + TLM_SENSOR(ESC3_TEMPERATURE, 0x1139, 200, 3000, 0, U8), + TLM_SENSOR(ESC4_TEMPERATURE, 0x113A, 200, 3000, 0, U8), +#endif + + TLM_SENSOR(CPU_LOAD, 0x1150, 500, 3000, 10, U8), + TLM_SENSOR(FLIGHT_MODE, 0x1251, 200, 3000, 0, U16), + TLM_SENSOR(ARMING_FLAGS, 0x1252, 200, 3000, 0, U8), +}; + +telemetrySensor_t * crsfGetCustomSensor(sensor_id_e id) +{ + for (size_t i = 0; i < ARRAYLEN(crsfCustomTelemetrySensors); i++) { + telemetrySensor_t * sensor = &crsfCustomTelemetrySensors[i]; + if (sensor->sensor_id == id) + return sensor; + } + + return NULL; +} +#endif // USE_CUSTOM_TELEMETRY + +/////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////// /* CRSF frame has the structure: @@ -221,12 +500,12 @@ static void crsfFrameGps(sbuf_t *dst) // use sbufWrite since CRC does not include frame length sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); crsfSerialize8(dst, CRSF_FRAMETYPE_GPS); - crsfSerialize32(dst, gpsSol.llh.lat); // CRSF and betaflight use same units for degrees - crsfSerialize32(dst, gpsSol.llh.lon); - crsfSerialize16(dst, (gpsSol.groundSpeed * 36 + 50) / 100); // gpsSol.groundSpeed is in cm/s - crsfSerialize16(dst, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse)); // gpsSol.groundCourse is 0.1 degrees, need 0.01 deg + crsfSerialize32BE(dst, gpsSol.llh.lat); // CRSF and betaflight use same units for degrees + crsfSerialize32BE(dst, gpsSol.llh.lon); + crsfSerialize16BE(dst, (gpsSol.groundSpeed * 36 + 50) / 100); // gpsSol.groundSpeed is in cm/s + crsfSerialize16BE(dst, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse)); // gpsSol.groundCourse is 0.1 degrees, need 0.01 deg const uint16_t altitude = (getEstimatedActualPosition(Z) / 100) + 1000; - crsfSerialize16(dst, altitude); + crsfSerialize16BE(dst, altitude); crsfSerialize8(dst, gpsSol.numSat); } @@ -240,7 +519,7 @@ static void crsfFrameVarioSensor(sbuf_t *dst) // use sbufWrite since CRC does not include frame length sbufWriteU8(dst, CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); crsfSerialize8(dst, CRSF_FRAMETYPE_VARIO_SENSOR); - crsfSerialize16(dst, lrintf(getEstimatedActualVelocity(Z))); + crsfSerialize16BE(dst, lrintf(getEstimatedActualVelocity(Z))); } /* @@ -257,11 +536,11 @@ static void crsfFrameBatterySensor(sbuf_t *dst) sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); crsfSerialize8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR); if (telemetryConfig()->report_cell_voltage) { - crsfSerialize16(dst, getBatteryAverageCellVoltage() / 10); + crsfSerialize16BE(dst, getBatteryAverageCellVoltage() / 10); } else { - crsfSerialize16(dst, getBatteryVoltage() / 10); // vbat is in units of 0.01V + crsfSerialize16BE(dst, getBatteryVoltage() / 10); // vbat is in units of 0.01V } - crsfSerialize16(dst, getAmperage() / 10); + crsfSerialize16BE(dst, getAmperage() / 10); const uint8_t batteryRemainingPercentage = calculateBatteryPercentage(); crsfSerialize8(dst, (getMAhDrawn() >> 16)); crsfSerialize8(dst, (getMAhDrawn() >> 8)); @@ -293,30 +572,8 @@ static void crsfBarometerAltitude(sbuf_t *dst) } sbufWriteU8(dst, CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); crsfSerialize8(dst, CRSF_FRAMETYPE_BAROMETER_ALTITUDE); - crsfSerialize16(dst, altitude_packed); -} - -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 -} crsrRfPower_e; + crsfSerialize16BE(dst, altitude_packed); +} /* 0x1E Attitude @@ -342,9 +599,9 @@ static void crsfFrameAttitude(sbuf_t *dst) { sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); crsfSerialize8(dst, CRSF_FRAMETYPE_ATTITUDE); - crsfSerialize16(dst, decidegrees2Radians10000(attitude.values.pitch)); - crsfSerialize16(dst, decidegrees2Radians10000(attitude.values.roll)); - crsfSerialize16(dst, decidegrees2Radians10000(attitude.values.yaw)); + crsfSerialize16BE(dst, decidegrees2Radians10000(attitude.values.pitch)); + crsfSerialize16BE(dst, decidegrees2Radians10000(attitude.values.roll)); + crsfSerialize16BE(dst, decidegrees2Radians10000(attitude.values.yaw)); } /* @@ -454,22 +711,20 @@ static void crsfFrameDeviceInfo(sbuf_t *dst) *lengthPtr = sbufPtr(dst) - lengthPtr; } -#define BV(x) (1 << (x)) // bit value +/* + * 0x0B Heartbeat + * Payload: + * int16_t Origin Device address +*/ +static void crsfFrameHeartbeat(sbuf_t *dst) +{ + sbufWriteU8(dst, CRSF_FRAMETYPE_HEARTBEAT); + sbufWriteU16(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); +} -// 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_VARIO_SENSOR_INDEX, - CRSF_FRAME_BAROMETER_ALTITUDE_INDEX, - CRSF_SCHEDULE_COUNT_MAX -} crsfFrameTypeIndex_e; - -static uint8_t crsfScheduleCount; -static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; +#define BV(x) (1 << (x)) // bit value +/////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////// #if defined(USE_MSP_OVER_TELEMETRY) @@ -486,11 +741,10 @@ void crsfScheduleMspResponse(uint8_t requestOriginID) void crsfSendMspResponse(uint8_t *payload, const uint8_t payloadSize) { - sbuf_t crsfPayloadBuf; - sbuf_t *dst = &crsfPayloadBuf; + sbuf_t *dst = crsfInitializeSbuf(); - crsfInitializeFrame(dst); - sbufWriteU8(dst, payloadSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); + crsfInitializeFrame(dst); // write sync byte [CRSF_TELEMETRY_SYNC_BYTE] + sbufWriteU8(dst, payloadSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); // [LENGTH] crsfSerialize8(dst, CRSF_FRAMETYPE_MSP_RESP); crsfSerialize8(dst, mspRequestOriginID); crsfSerialize8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); @@ -499,96 +753,180 @@ void crsfSendMspResponse(uint8_t *payload, const uint8_t payloadSize) } #endif -static void processCrsf(void) +static bool crsfSendNativeTelemetry(void) { - if (!crsfRxIsTelemetryBufEmpty()) { - return; // do nothing if telemetry ouptut buffer is not empty yet. + if (crsfTelemetryState != CRSFR_TELEMETRY_STATE_NATIVE) + { + return false; } - static uint8_t crsfScheduleIndex = 0; - const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex]; - - sbuf_t crsfPayloadBuf; - sbuf_t *dst = &crsfPayloadBuf; + telemetrySensor_t *sensor = telemetryScheduleNext(); - if (currentSchedule & BV(CRSF_FRAME_ATTITUDE_INDEX)) { - crsfInitializeFrame(dst); - crsfFrameAttitude(dst); - crsfFinalize(dst); - } - if (currentSchedule & BV(CRSF_FRAME_BATTERY_SENSOR_INDEX)) { - crsfInitializeFrame(dst); - crsfFrameBatterySensor(dst); - crsfFinalize(dst); - } - if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE_INDEX)) { - crsfInitializeFrame(dst); - crsfFrameFlightMode(dst); - crsfFinalize(dst); - } + if (sensor) { + sbuf_t *dst = crsfInitializeSbuf(); + switch (sensor->sensor_id) { + case TELEM_ATTITUDE: + crsfInitializeFrame(dst); // write sync byte [CRSF_TELEMETRY_SYNC_BYTE] + crsfFrameAttitude(dst); // create whole frame without SYNC and CRC + crsfFinalize(dst); + break; +#if defined(USE_BARO) || defined(USE_GPS) + case TELEM_VARIOMETER: + crsfInitializeFrame(dst); // write sync byte [CRSF_TELEMETRY_SYNC_BYTE] + crsfFrameVarioSensor(dst); // create whole frame without SYNC and CRC + crsfFinalize(dst); + break; + case TELEM_ALTITUDE: + crsfInitializeFrame(dst); // write sync byte [CRSF_TELEMETRY_SYNC_BYTE] + crsfBarometerAltitude(dst); // create whole frame without SYNC and CRC + crsfFinalize(dst); + break; +#endif + case TELEM_BATTERY: + crsfInitializeFrame(dst); // write sync byte [CRSF_TELEMETRY_SYNC_BYTE] + crsfFrameBatterySensor(dst); // create whole frame without SYNC and CRC + crsfFinalize(dst); + break; + case TELEM_FLIGHT_MODE: + crsfInitializeFrame(dst); // write sync byte [CRSF_TELEMETRY_SYNC_BYTE] + crsfFrameFlightMode(dst); // create whole frame without SYNC and CRC + crsfFinalize(dst); + break; #ifdef USE_GPS - if (currentSchedule & BV(CRSF_FRAME_GPS_INDEX)) { - crsfInitializeFrame(dst); - crsfFrameGps(dst); - crsfFinalize(dst); - } + case TELEM_GPS: + crsfInitializeFrame(dst); + crsfFrameGps(dst); + crsfFinalize(dst); + break; #endif -#if defined(USE_BARO) || defined(USE_GPS) - if (currentSchedule & BV(CRSF_FRAME_VARIO_SENSOR_INDEX)) { - crsfInitializeFrame(dst); - crsfFrameVarioSensor(dst); - crsfFinalize(dst); - } - if (currentSchedule & BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX)) { - crsfInitializeFrame(dst); - crsfBarometerAltitude(dst); - crsfFinalize(dst); + default: + crsfInitializeFrame(dst); // write sync byte [CRSF_TELEMETRY_SYNC_BYTE] + crsfFrameHeartbeat(dst); // create whole frame without SYNC and CRC + crsfFinalize(dst); + break; + } + telemetryScheduleCommit(sensor); + return true; } -#endif - crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount; + + return false; } -void crsfScheduleDeviceInfoResponse(void) +#ifdef USE_CUSTOM_TELEMETRY +static bool crsfSendCustomTelemetry(void) { - deviceInfoReplyPending = true; + if (crsfTelemetryState == CRSFR_TELEMETRY_STATE_CUSTOM) + { + size_t sensor_count = 0; + sbuf_t *dst = crsfInitializeSbuf(); // prepare buffer + + crsfInitializeFrame(dst); // write sync byte [CRSF_TELEMETRY_SYNC_BYTE] + uint8_t *lengthPtr = sbufPtr(dst); // [LENGTH] take position of length, because we don't know length + crsfFillCustomSensorHeader(dst); // fill rest of header [CRSF_FRAMETYPE_CUSTOM_TELEM, CRSF_ADDRESS_RADIO_TRANSMITTER, CRSF_ADDRESS_FLIGHT_CONTROLLER, frameId] + + while (sbufBytesRemaining(dst) > CRSF_CUSTOM_TELEMETRY_MIN_SPACE) { + telemetrySensor_t *sensor = telemetryScheduleNext(); + if (sensor) { + crsfFrameCustomTelemetrySensor(dst, sensor); + if (sbufBytesRemaining(dst) < 1) { + break; + } + telemetryScheduleCommit(sensor); + sensor_count++; + } + else { + break; + } + } + + if (sensor_count) { + *lengthPtr = sbufPtr(dst) - lengthPtr; // write length,[LENGTH] + // here should frame looks like: + // [CRSF_TELEMETRY_SYNC_BYTE] [LENGTH] [CRSF_FRAMETYPE_CUSTOM_TELEM, CRSF_ADDRESS_RADIO_TRANSMITTER, CRSF_ADDRESS_FLIGHT_CONTROLLER, frameId] [DATA] + // CRC is filled by crsfFinalize function + crsfFinalize(dst); + crsfCustomTelemetryFrameId++; + return true; + } + } + + return false; } +#endif -void initCrsfTelemetry(void) +#ifdef USE_CUSTOM_TELEMETRY +static bool crsfPopulateCustomTelemetry(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 (crsfTelemetryState == CRSFR_TELEMETRY_STATE_POPULATE) + { + static int slot = -10; + + if (slot < 0) { + telemetrySensor_t * sensor = crsfGetCustomSensor(TELEM_NONE); + slot++; + + if (sensor) { + sbuf_t *dst = crsfInitializeSbuf(); + crsfInitializeFrame(dst); + uint8_t *lengthPtr = sbufPtr(dst); + crsfFillCustomSensorHeader(dst); + crsfFrameCustomTelemetrySensor(dst, sensor); + *lengthPtr = sbufPtr(dst) - lengthPtr; + crsfFinalize(dst); + return true; + } + return false; + } - deviceInfoReplyPending = false; -#if defined(USE_MSP_OVER_TELEMETRY) - mspReplyPending = false; -#endif + // send list of active sensors to LUA telemetry + while (slot < TELEM_SENSOR_SLOT_COUNT) { + sensor_id_e id = telemetryConfig()->telemetry_sensors[slot]; + slot++; + + if (telemetrySensorActive(id)) { + telemetrySensor_t * sensor = crsfGetCustomSensor(id); + if (sensor) { + sbuf_t *dst = crsfInitializeSbuf(); + crsfInitializeFrame(dst); + uint8_t *lengthPtr = sbufPtr(dst); + crsfFillCustomSensorHeader(dst); + crsfFrameCustomTelemetrySensor(dst, sensor); + *lengthPtr = sbufPtr(dst) - lengthPtr; + crsfFinalize(dst); + crsfCustomTelemetryFrameId++; + return true; + } + } + } - int index = 0; - crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX); - crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX); - crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX); -#ifdef USE_GPS - if (feature(FEATURE_GPS)) { - crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX); - } -#endif -#if defined(USE_BARO) || defined(USE_GPS) - if (sensors(SENSOR_BARO) || (STATE(FIXED_WING_LEGACY) && feature(FEATURE_GPS))) { - crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX); + crsfTelemetryState = CRSFR_TELEMETRY_STATE_CUSTOM; } + + return false; +} #endif -#ifdef USE_BARO - if (sensors(SENSOR_BARO)) { - crsfSchedule[index++] = BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX); + +static bool crsfSendDeviceInfoData(void) +{ + if (deviceInfoReplyPending) { + deviceInfoReplyPending = false; + sbuf_t *dst = crsfInitializeSbuf(); + crsfInitializeFrame(dst); // write sync byte [CRSF_TELEMETRY_SYNC_BYTE] + crsfFrameDeviceInfo(dst); // create whole frame without SYNC and CRC + crsfFinalize(dst); + return true; } -#endif - crsfScheduleCount = (uint8_t)index; + return false; +} + +void crsfScheduleDeviceInfoResponse(void) +{ + deviceInfoReplyPending = true; } bool checkCrsfTelemetryState(void) { - return crsfTelemetryEnabled; + return crsfTelemetryState; } /* @@ -596,71 +934,101 @@ bool checkCrsfTelemetryState(void) */ void handleCrsfTelemetry(timeUs_t currentTimeUs) { - static uint32_t crsfLastCycleTime; - - if (!crsfTelemetryEnabled) { + if (!crsfTelemetryState) { return; } - // 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(); - // Send ad-hoc response frames as soon as possible + crsfTelemetryRateUpdate(currentTimeUs); + + if (crsfCanTransmitTelemetry()) { + // 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(); + + // Send ad-hoc response frames as soon as possible #if defined(USE_MSP_OVER_TELEMETRY) - if (mspReplyPending) { - mspReplyPending = handleCrsfMspFrameBuffer(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse); - crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request - return; - } + if (mspReplyPending) { + mspReplyPending = handleCrsfMspFrameBuffer(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse); + return; + } #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 (!crsfRxIsTelemetryBufEmpty()) { + return; // do nothing if telemetry ouptut buffer is not empty yet. + } + + bool __unused sent = + crsfSendDeviceInfoData() || + crsfSendNativeTelemetry() +#ifdef USE_CUSTOM_TELEMETRY + || + crsfSendCustomTelemetry() || + crsfPopulateCustomTelemetry() +#endif + ; + } +} +#ifdef USE_CUSTOM_TELEMETRY +static void initCrsfCustomSensors(void) +{ + telemetryScheduleInit(crsfCustomTelemetrySensors, ARRAYLEN(crsfCustomTelemetrySensors)); + + for(size_t i = 0; i < ARRAYLEN(crsfCustomTelemetrySensors); i++) { + if(telemetrySensorActive(crsfCustomTelemetrySensors[i].sensor_id) && telemetrySensorAllowed(crsfCustomTelemetrySensors[i].sensor_id)) { + telemetryScheduleAdd(&crsfCustomTelemetrySensors[i]); + } + } +} +#endif + +static void initCrsfNativeSensors(void) +{ + telemetryScheduleInit(crsfNativeTelemetrySensors, ARRAYLEN(crsfNativeTelemetrySensors)); - // 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(); + for(size_t i = 0; i < ARRAYLEN(crsfNativeTelemetrySensors); i++) { + if(telemetrySensorAllowed(crsfNativeTelemetrySensors[i].sensor_id)) { + telemetryScheduleAdd(&crsfNativeTelemetrySensors[i]); + } } } -int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType) -{ - 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; + +void initCrsfTelemetry(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 + +#ifdef USE_CUSTOM_TELEMETRY + crsfTelemetryState = !crsfRxIsActive() ? CRSFR_TELEMETRY_STATE_OFF : (telemetryConfig()->crsf_telemetry_mode == CRSFR_TELEMETRY_STATE_NATIVE ? CRSFR_TELEMETRY_STATE_NATIVE : CRSFR_TELEMETRY_STATE_POPULATE); +#else + crsfTelemetryState = !crsfRxIsActive() ? CRSFR_TELEMETRY_STATE_OFF : CRSFR_TELEMETRY_STATE_NATIVE; +#endif + + if(crsfTelemetryState) { + deviceInfoReplyPending = false; + + const float rate = telemetryConfig()->crsf_telemetry_link_rate; + const float ratio = telemetryConfig()->crsf_telemetry_link_ratio; + + crsfTelemetryRateQuanta = rate / (ratio * 1000000); + crsfTelemetryRateBucket = 0; + crsfCustomTelemetryFrameId = 0; + +#if defined(USE_MSP_OVER_TELEMETRY) + mspReplyPending = false; +#endif +#ifdef USE_CUSTOM_TELEMETRY + if (crsfTelemetryState == CRSFR_TELEMETRY_STATE_NATIVE) { + initCrsfNativeSensors(); + } else { + initCrsfCustomSensors(); + } +#else + initCrsfNativeSensors(); #endif - case CRSF_FRAMETYPE_VARIO_SENSOR: - crsfFrameVarioSensor(sbuf); - break; } - const int frameSize = crsfFinalizeBuf(sbuf, frame); - return frameSize; } + #endif diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index bfe9b9e4f3c..e53bc1be627 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -19,6 +19,7 @@ #include "common/time.h" #include "rx/crsf.h" +#include "telemetry/sensors.h" #define CRSF_MSP_RX_BUF_SIZE 128 #define CRSF_MSP_TX_BUF_SIZE 128 diff --git a/src/main/telemetry/sensors.c b/src/main/telemetry/sensors.c new file mode 100644 index 00000000000..ae8ddecde80 --- /dev/null +++ b/src/main/telemetry/sensors.c @@ -0,0 +1,442 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include "platform.h" + +#if defined(USE_TELEMETRY) + +#include "sensors.h" +#include "telemetry.h" + +#include "sensors/battery.h" +#include "sensors//acceleration.h" +#include "sensors/esc_sensor.h" +#include "sensors/temperature.h" +#include "sensors/pitotmeter.h" + +#include "drivers/time.h" + +#include "navigation/navigation.h" + +#include "flight/imu.h" +#include "flight/mixer.h" + +#include "common/crc.h" +#include "fc/config.h" + +#include "scheduler/scheduler.h" + +#include "fc/runtime_config.h" + +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; + + ///////////////////////////////////////// + ////// BATERY ////////////////////////// + case TELEM_BATTERY: + return millis(); + case TELEM_BATTERY_VOLTAGE: + if (isBatteryVoltageConfigured()) { + return getBatteryVoltage(); + } + return 0; + case TELEM_BATTERY_CURRENT: + if (isAmperageConfigured()) { + return getAmperage(); + } + return 0; + case TELEM_BATTERY_CONSUMPTION: + return getMAhDrawn(); + case TELEM_BATTERY_CHARGE_LEVEL: + return calculateBatteryPercentage(); + case TELEM_BATTERY_CELL_COUNT: + if (isBatteryVoltageConfigured()) { + return getBatteryCellCount(); + } + return 0; + case TELEM_BATTERY_CELL_VOLTAGE: + return getBatteryAverageCellVoltage(); + case TELEM_BATTERY_CELL_VOLTAGES: + return 0; + case TELEM_BATTERY_FUEL: + return millis(); + + ///////////////////////////////////////// + ////// MOVING ////////////////////////// + case TELEM_HEADING: + return DECIDEGREES_TO_DEGREES(attitude.values.yaw); + case TELEM_ALTITUDE: + return (int)(getEstimatedActualPosition(Z)); // cm + case TELEM_VARIOMETER: + return (int)getEstimatedActualVelocity(Z); + case TELEM_ATTITUDE: + return millis(); + case TELEM_ATTITUDE_PITCH: + return attitude.values.pitch; + case TELEM_ATTITUDE_ROLL: + return attitude.values.roll; + case TELEM_ATTITUDE_YAW: + return attitude.values.yaw; + + case TELEM_ACCEL_X: + return (int)(acc.accADCf[X] * 1000); + case TELEM_ACCEL_Y: + return (int)(acc.accADCf[Y] * 1000); + case TELEM_ACCEL_Z: + return (int)(acc.accADCf[Z] * 1000); + + ///////////////////////////////////////// + ////// GPS ////////////////////////// +#ifdef USE_GPS + case TELEM_GPS: + return (int)millis(); + case TELEM_GPS_SATS: + return gpsSol.numSat; + case TELEM_GPS_HDOP: + return gpsSol.hdop; + case TELEM_GPS_COORD: + return (int)getTupleHash(gpsSol.llh.lat, gpsSol.llh.lon); + case TELEM_GPS_ALTITUDE: + return gpsSol.llh.alt; + case TELEM_GPS_HEADING: + return gpsSol.groundCourse; + case TELEM_GPS_GROUNDSPEED: + return gpsSol.groundSpeed; + case TELEM_GPS_HOME_DISTANCE: + return (int)GPS_distanceToHome; + case TELEM_GPS_HOME_DIRECTION: + return GPS_directionToHome; + case TELEM_GPS_AZIMUTH: + return ((GPS_directionToHome < 0 ? GPS_directionToHome + 360 : GPS_directionToHome) + 180) % 360; +#endif + +#ifdef USE_ESC_SENSOR + ///////////////////////////////////////// + ////// ESC ////////////////////////// + case TELEM_ESC_RPM: + return (int)millis(); + case TELEM_ESC1_RPM: + return getMotorCount() > 0 ? (int)getEscTelemetry(0)->rpm : 0; + case TELEM_ESC2_RPM: + return getMotorCount() > 1 ? (int)getEscTelemetry(1)->rpm : 0; + case TELEM_ESC3_RPM: + return getMotorCount() > 2 ? (int)getEscTelemetry(2)->rpm : 0; + case TELEM_ESC4_RPM: + return getMotorCount() > 3 ? (int)getEscTelemetry(3)->rpm : 0; + + + case TELEM_ESC_TEMPERATURE: + return (int)millis(); + case TELEM_ESC1_TEMPERATURE: + return getMotorCount() > 0 ? (int)(getEscTelemetry(0)->temperature * 10) : 0; + case TELEM_ESC2_TEMPERATURE: + return getMotorCount() > 1 ? (int)(getEscTelemetry(1)->temperature * 10) : 0; + case TELEM_ESC3_TEMPERATURE: + return getMotorCount() > 2 ? (int)(getEscTelemetry(2)->temperature * 10) : 0; + case TELEM_ESC4_TEMPERATURE: + return getMotorCount() > 3 ? (int)(getEscTelemetry(3)->temperature * 10) : 0; +#endif + + + ///////////////////////////////////////// + ////// SYSTEM ////////////////////////// + case TELEM_FLIGHT_MODE: + return (int)flightModeFlags; + case TELEM_ARMING_FLAGS: + return (int)armingFlags; + case TELEM_CPU_LOAD: + return averageSystemLoadPercent * 10; + + //////////////////////////////////////// + ////// LEGACY SMARTPORT //////////////// + case TELEM_LEGACY_VFAS: + FALLTHROUGH; + case TELEM_LEGACY_CURRENT: + FALLTHROUGH; + case TELEM_LEGACY_ALTITUDE: + FALLTHROUGH; + case TELEM_LEGACY_FUEL: + FALLTHROUGH; + case TELEM_LEGACY_VARIO: + FALLTHROUGH; + case TELEM_LEGACY_HEADING: + FALLTHROUGH; + case TELEM_LEGACY_PITCH: + FALLTHROUGH; + case TELEM_LEGACY_ROLL: + FALLTHROUGH; + case TELEM_LEGACY_ACCX: + FALLTHROUGH; + case TELEM_LEGACY_ACCY: + FALLTHROUGH; + case TELEM_LEGACY_ACCZ: + FALLTHROUGH; + case TELEM_LEGACY_MODES: + FALLTHROUGH; + case TELEM_LEGACY_GNSS: + FALLTHROUGH; + case TELEM_LEGACY_SPEED: + FALLTHROUGH; + case TELEM_LEGACY_LAT: + FALLTHROUGH; + case TELEM_LEGACY_LON: + FALLTHROUGH; + case TELEM_LEGACY_HOME_DIST: + FALLTHROUGH; + case TELEM_LEGACY_GPS_ALT: + FALLTHROUGH; + case TELEM_LEGACY_FPV: + FALLTHROUGH; + case TELEM_LEGACY_AZIMUTH: + FALLTHROUGH; + case TELEM_LEGACY_A4: + FALLTHROUGH; + case TELEM_LEGACY_ASPD: + return millis(); + default: + return 0; + } +} + +///////////////////////////////////////////////////////////////////////////////////////////// +#ifdef USE_GPS +bool shouldSendGpsData(void) { + // We send GPS data if the GPS is configured and we have a fix + // or the craft has never been armed yet. This way if GPS stops working + // while in flight, the user will easily notice because the sensor will stop + // updating. + return feature(FEATURE_GPS) && (STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + || !ARMING_FLAG(WAS_EVER_ARMED)); +} +#endif + +bool shouldSendDataForMotorIndex(uint8_t motorIndex) +{ +#ifdef USE_ESC_SENSOR + return STATE(ESC_SENSOR_ENABLED) && getMotorCount() > motorIndex; +#else + UNUSED(motorIndex); + return false; +#endif +} + +///////////////////////////////////////////////////////////////////////////////////////////// +#ifdef USE_CUSTOM_TELEMETRY +bool telemetrySensorActive(sensor_id_e sid) +{ + // system sensors are always active + if(sid == TELEM_NONE || sid == TELEM_HEARTBEAT) { + return true; + } + + for (int i = 0; i < TELEM_SENSOR_SLOT_COUNT; ++i) { + if (telemetryConfig()->telemetry_sensors[i] == sid) { + return true; + } + } + + return false; +} +#endif + +bool telemetrySensorAllowed(sensor_id_e id) +{ + switch (id) { + case TELEM_NONE: + return true; + + case TELEM_HEARTBEAT: + return true; + + ///////////////////////////////////////// + ////// BATERY ////////////////////////// + case TELEM_BATTERY: + return true; + case TELEM_BATTERY_VOLTAGE: + return isBatteryVoltageConfigured(); + case TELEM_BATTERY_CURRENT: + return isAmperageConfigured(); + case TELEM_BATTERY_CONSUMPTION: + return isAmperageConfigured(); + case TELEM_BATTERY_CHARGE_LEVEL: + FALLTHROUGH; + case TELEM_BATTERY_CELL_COUNT: + FALLTHROUGH; + case TELEM_BATTERY_CELL_VOLTAGE: + FALLTHROUGH; + case TELEM_BATTERY_CELL_VOLTAGES: + return isBatteryVoltageConfigured(); + case TELEM_BATTERY_FUEL: + return true; + + ///////////////////////////////////////// + ////// MOVING ////////////////////////// + case TELEM_HEADING: + return true; + case TELEM_ALTITUDE: + return sensors(SENSOR_BARO); + case TELEM_ATTITUDE: + return true; + case TELEM_ATTITUDE_PITCH: + return true; + case TELEM_ATTITUDE_ROLL: + return true; + case TELEM_ATTITUDE_YAW: + return true; + case TELEM_VARIOMETER: + return sensors(SENSOR_BARO); + + case TELEM_ACCEL_X: + return true; + case TELEM_ACCEL_Y: + return true; + case TELEM_ACCEL_Z: + return true; + + ///////////////////////////////////////// + ////// GPS ////////////////////////// +#ifdef USE_GPS + case TELEM_GPS: + FALLTHROUGH; + case TELEM_GPS_SATS: + FALLTHROUGH; + case TELEM_GPS_HDOP: + FALLTHROUGH; + case TELEM_GPS_COORD: + FALLTHROUGH; + case TELEM_GPS_ALTITUDE: + FALLTHROUGH; + case TELEM_GPS_HEADING: + FALLTHROUGH; + case TELEM_GPS_GROUNDSPEED: + FALLTHROUGH; + case TELEM_GPS_HOME_DISTANCE: + FALLTHROUGH; + case TELEM_GPS_HOME_DIRECTION: + FALLTHROUGH; + case TELEM_GPS_AZIMUTH: + return shouldSendGpsData(); +#endif + + ///////////////////////////////////////// + ////// ESC ////////////////////////// + + case TELEM_ESC_RPM: + FALLTHROUGH; + case TELEM_ESC1_RPM: + return shouldSendDataForMotorIndex(0); + case TELEM_ESC2_RPM: + return shouldSendDataForMotorIndex(1); + case TELEM_ESC3_RPM: + return shouldSendDataForMotorIndex(2); + case TELEM_ESC4_RPM: + return shouldSendDataForMotorIndex(3);; + + case TELEM_TEMPERATURE: + return true; + case TELEM_ESC_TEMPERATURE: + FALLTHROUGH; + case TELEM_ESC1_TEMPERATURE: + return shouldSendDataForMotorIndex(0); + case TELEM_ESC2_TEMPERATURE: + return shouldSendDataForMotorIndex(1); + case TELEM_ESC3_TEMPERATURE: + return shouldSendDataForMotorIndex(2); + case TELEM_ESC4_TEMPERATURE: + return shouldSendDataForMotorIndex(3); + + ///////////////////////////////////////// + ////// SYSTEM ////////////////////////// + case TELEM_CPU_LOAD: + return true; + + case TELEM_FLIGHT_MODE: + return true; + case TELEM_ARMING_FLAGS: + return true; + + //////////////////////////////////////// + ////// LEGACY SMARTPORT //////////////// + case TELEM_LEGACY_VFAS: + return isBatteryVoltageConfigured(); + case TELEM_LEGACY_CURRENT: + return isAmperageConfigured(); + case TELEM_LEGACY_ALTITUDE: + return sensors(SENSOR_BARO); + case TELEM_LEGACY_FUEL: + return true; + case TELEM_LEGACY_VARIO: + return sensors(SENSOR_BARO); + case TELEM_LEGACY_HEADING: + return true; + case TELEM_LEGACY_PITCH: + FALLTHROUGH; + case TELEM_LEGACY_ROLL: + return telemetryConfig()->frsky_pitch_roll; + case TELEM_LEGACY_ACCX: + FALLTHROUGH; + case TELEM_LEGACY_ACCY: + FALLTHROUGH; + case TELEM_LEGACY_ACCZ: + return sensors(SENSOR_ACC) && !telemetryConfig()->frsky_pitch_roll; + case TELEM_LEGACY_MODES: + return true; + +#ifdef USE_GPS + case TELEM_LEGACY_GNSS: + FALLTHROUGH; + case TELEM_LEGACY_SPEED: + FALLTHROUGH; + case TELEM_LEGACY_LAT: + FALLTHROUGH; + case TELEM_LEGACY_LON: + FALLTHROUGH; + case TELEM_LEGACY_HOME_DIST: + FALLTHROUGH; + case TELEM_LEGACY_GPS_ALT: + FALLTHROUGH; + case TELEM_LEGACY_FPV: + FALLTHROUGH; + case TELEM_LEGACY_AZIMUTH: + return shouldSendGpsData(); + case TELEM_LEGACY_A4: + return isBatteryVoltageConfigured(); +#endif +#ifdef USE_PITOT + case TELEM_LEGACY_ASPD: + return sensors(SENSOR_PITOT) && pitotIsHealthy(); +#endif + default: + return false; + } +} + +//////////////////////////////////////////////////////////// +#endif // USE_TELEMETRY \ No newline at end of file diff --git a/src/main/telemetry/sensors.h b/src/main/telemetry/sensors.h new file mode 100644 index 00000000000..391704b880b --- /dev/null +++ b/src/main/telemetry/sensors.h @@ -0,0 +1,159 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ +#pragma once + +#include +#include + +#include "common/utils.h" + +typedef enum +{ + ///////////////////////////////////////// + ////// SPECIAL ////////////////////////// + TELEM_NONE = 0, + TELEM_HEARTBEAT = 1, + + ///////////////////////////////////////// + ////// BATERY ////////////////////////// + TELEM_BATTERY = 2, // start from settings + 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_BATTERY_FUEL = 10, + + ///////////////////////////////////////// + ////// MOVING ////////////////////////// + TELEM_HEADING = 11, + TELEM_ALTITUDE = 12, + TELEM_VARIOMETER = 13, + + TELEM_ATTITUDE = 14, + TELEM_ATTITUDE_PITCH = 15, + TELEM_ATTITUDE_ROLL = 16, + TELEM_ATTITUDE_YAW = 17, + + TELEM_ACCEL_X = 18, + TELEM_ACCEL_Y = 19, + TELEM_ACCEL_Z = 20, + + ///////////////////////////////////////// + ////// GPS ////////////////////////// + TELEM_GPS = 21, + TELEM_GPS_SATS = 22, + TELEM_GPS_HDOP = 23, + TELEM_GPS_COORD = 24, + TELEM_GPS_ALTITUDE = 25, + TELEM_GPS_HEADING = 26, + TELEM_GPS_GROUNDSPEED = 27, + TELEM_GPS_HOME_DISTANCE = 28, + TELEM_GPS_HOME_DIRECTION = 29, + TELEM_GPS_AZIMUTH = 30, + + ///////////////////////////////////////// + ////// ESC ////////////////////////// + TELEM_ESC_RPM = 31, + TELEM_ESC_TEMPERATURE = 32, + + TELEM_ESC1_RPM = 33, + TELEM_ESC1_TEMPERATURE = 34, + + TELEM_ESC2_RPM = 35, + TELEM_ESC2_TEMPERATURE = 36, + + TELEM_ESC3_RPM = 37, + TELEM_ESC3_TEMPERATURE = 38, + + TELEM_ESC4_RPM = 39, + TELEM_ESC4_TEMPERATURE = 40, + + TELEM_TEMPERATURE = 41, + + ///////////////////////////////////////// + ////// SYSTEM ////////////////////////// + TELEM_CPU_LOAD = 42, + TELEM_FLIGHT_MODE = 43, + TELEM_ARMING_FLAGS = 44, + + ///////////////////////////////////////// + ////// LEGACY SMARTPORT //////////////// + TELEM_LEGACY_VFAS = 45, + TELEM_LEGACY_CURRENT = 46, + TELEM_LEGACY_ALTITUDE = 47, + TELEM_LEGACY_FUEL = 48, + TELEM_LEGACY_VARIO = 49, + TELEM_LEGACY_HEADING = 50, + TELEM_LEGACY_PITCH = 51, + TELEM_LEGACY_ROLL = 52, + TELEM_LEGACY_ACCX = 53, + TELEM_LEGACY_ACCY = 54, + TELEM_LEGACY_ACCZ = 55, + TELEM_LEGACY_MODES = 56, + + TELEM_LEGACY_GNSS = 57, + TELEM_LEGACY_SPEED = 58, + TELEM_LEGACY_LAT = 59, + TELEM_LEGACY_LON = 60, + TELEM_LEGACY_HOME_DIST = 61, + TELEM_LEGACY_GPS_ALT = 62, + TELEM_LEGACY_FPV = 63, + TELEM_LEGACY_AZIMUTH = 64, + + TELEM_LEGACY_A4 = 65, + TELEM_LEGACY_ASPD = 66, + + TELEM_SENSOR_COUNT + +} sensor_id_e; + +typedef struct telemetrySensor_s telemetrySensor_t; + +typedef void (*telemetryEncode_f)(telemetrySensor_t *sensor, void *ptr); + +struct telemetrySensor_s { + + uint16_t index; + + uint16_t sensor_id; + uint32_t app_id; + + uint16_t fast_weight; + uint16_t slow_weight; + uint16_t fast_interval; + uint16_t slow_interval; + + int ratio_num; + int ratio_den; + + int value; + + bool active; + bool update; + + int bucket; + + telemetryEncode_f encode; +}; + +int telemetrySensorValue(sensor_id_e id); +bool telemetrySensorActive(sensor_id_e id); +bool telemetrySensorAllowed(sensor_id_e id); \ No newline at end of file diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 355e550b67e..323ef590a3a 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -12,121 +12,25 @@ #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT) -#include "common/axis.h" -#include "common/color.h" #include "common/maths.h" #include "common/utils.h" -#include "config/feature.h" -#include "config/parameter_group.h" #include "config/parameter_group_ids.h" -#include "drivers/accgyro/accgyro.h" -#include "drivers/compass/compass.h" -#include "drivers/sensor.h" #include "drivers/time.h" -#include "fc/config.h" -#include "fc/control_profile.h" -#include "fc/rc_controls.h" -#include "fc/rc_modes.h" -#include "fc/runtime_config.h" - -#include "flight/failsafe.h" -#include "flight/imu.h" -#include "flight/mixer.h" -#include "flight/pid.h" +#include "sensors/battery.h" -#include "io/beeper.h" #include "io/gps.h" #include "io/serial.h" -#include "navigation/navigation.h" - #include "rx/frsky_crc.h" -#include "sensors/boardalignment.h" -#include "sensors/sensors.h" -#include "sensors/battery.h" -#include "sensors/acceleration.h" -#include "sensors/barometer.h" -#include "sensors/compass.h" -#include "sensors/gyro.h" -#include "sensors/pitotmeter.h" - -#include "rx/rx.h" - #include "telemetry/telemetry.h" #include "telemetry/smartport.h" +#include "telemetry/smartport_legacy.h" #include "telemetry/msp_shared.h" -// these data identifiers are obtained from https://github.com/opentx/opentx/blob/2.3/radio/src/telemetry/frsky.h -enum -{ - FSSP_DATAID_SPEED = 0x0830, - FSSP_DATAID_VFAS = 0x0210, - FSSP_DATAID_CURRENT = 0x0200, - FSSP_DATAID_RPM = 0x050F, - FSSP_DATAID_ALTITUDE = 0x0100, - FSSP_DATAID_FUEL = 0x0600, - FSSP_DATAID_ADC1 = 0xF102, - FSSP_DATAID_ADC2 = 0xF103, - FSSP_DATAID_LATLONG = 0x0800, - FSSP_DATAID_CAP_USED = 0x0600, - FSSP_DATAID_VARIO = 0x0110, - FSSP_DATAID_CELLS = 0x0300, - FSSP_DATAID_CELLS_LAST = 0x030F, - FSSP_DATAID_HEADING = 0x0840, - FSSP_DATAID_FPV = 0x0450, - FSSP_DATAID_PITCH = 0x0430, - FSSP_DATAID_ROLL = 0x0440, - FSSP_DATAID_ACCX = 0x0700, - FSSP_DATAID_ACCY = 0x0710, - FSSP_DATAID_ACCZ = 0x0720, - FSSP_DATAID_LEGACY_MODES = 0x0400, // Deprecated. Should be removed in INAV 10.0 - FSSP_DATAID_LEGACY_GNSS = 0x0410, // Deprecated. Should be removed in INAV 10.0 - FSSP_DATAID_HOME_DIST = 0x0420, - FSSP_DATAID_GPS_ALT = 0x0820, - FSSP_DATAID_ASPD = 0x0A00, - FSSP_DATAID_A3 = 0x0900, - FSSP_DATAID_A4 = 0x0910, - FSSP_DATAID_AZIMUTH = 0x0460, - FSSP_DATAID_MODES = 0x0470, - FSSP_DATAID_GNSS = 0x0480, -}; - -const uint16_t frSkyDataIdTable[] = { - FSSP_DATAID_SPEED, - FSSP_DATAID_VFAS, - FSSP_DATAID_CURRENT, - //FSSP_DATAID_RPM, - FSSP_DATAID_ALTITUDE, - FSSP_DATAID_FUEL, - //FSSP_DATAID_ADC1, - //FSSP_DATAID_ADC2, - FSSP_DATAID_LATLONG, - FSSP_DATAID_LATLONG, // twice - //FSSP_DATAID_CAP_USED, - FSSP_DATAID_VARIO, - //FSSP_DATAID_CELLS, - //FSSP_DATAID_CELLS_LAST, - FSSP_DATAID_HEADING, - FSSP_DATAID_FPV, - FSSP_DATAID_PITCH, - FSSP_DATAID_ROLL, - FSSP_DATAID_ACCX, - FSSP_DATAID_ACCY, - FSSP_DATAID_ACCZ, - FSSP_DATAID_MODES, - FSSP_DATAID_GNSS, - FSSP_DATAID_HOME_DIST, - FSSP_DATAID_GPS_ALT, - FSSP_DATAID_ASPD, - // FSSP_DATAID_A3, - FSSP_DATAID_A4, - FSSP_DATAID_AZIMUTH, - 0 -}; #define __USE_C99_MATH // for roundf() #define SMARTPORT_BAUD 57600 @@ -146,13 +50,6 @@ enum }; static uint8_t telemetryState = TELEMETRY_STATE_UNINITIALIZED; -static uint8_t smartPortIdCnt = 0; - -typedef struct smartPortFrame_s { - uint8_t sensorId; - smartPortPayload_t payload; - uint8_t crc; -} __attribute__((packed)) smartPortFrame_t; #define SMARTPORT_MSP_PAYLOAD_SIZE (sizeof(smartPortPayload_t) - sizeof(uint8_t)) @@ -162,92 +59,6 @@ static smartPortWriteFrameFn *smartPortWriteFrame; static bool smartPortMspReplyPending = false; #endif -static uint32_t frskyGetFlightMode(void) -{ - uint32_t tmpi = 0; - - // ones column (G) - if (!isArmingDisabled()) - tmpi += 1; - else - tmpi += 2; - if (ARMING_FLAG(ARMED)) - tmpi += 4; - - // tens column (F) - if (FLIGHT_MODE(ANGLE_MODE)) - tmpi += 10; - if (FLIGHT_MODE(HORIZON_MODE)) - tmpi += 20; - if (FLIGHT_MODE(MANUAL_MODE)) - tmpi += 40; - - // hundreds column (E) - if (FLIGHT_MODE(HEADING_MODE)) - tmpi += 100; - if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) - tmpi += 200; - if (FLIGHT_MODE(NAV_POSHOLD_MODE) && !STATE(AIRPLANE)) - tmpi += 400; - - // thousands column (D) - if (FLIGHT_MODE(NAV_RTH_MODE) && !isWaypointMissionRTHActive()) - tmpi += 1000; - if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) // intentionally out of order and 'else-ifs' to prevent column overflow - tmpi += 8000; - else if (FLIGHT_MODE(NAV_WP_MODE)) - tmpi += 2000; - else if (FLIGHT_MODE(HEADFREE_MODE)) - tmpi += 4000; - - // ten thousands column (C) - if (FLIGHT_MODE(FLAPERON)) - tmpi += 10000; - if (FLIGHT_MODE(FAILSAFE_MODE)) - tmpi += 40000; - else if (FLIGHT_MODE(AUTO_TUNE)) // intentionally reverse order and 'else-if' to prevent 16-bit overflow - tmpi += 20000; - - // hundred thousands column (B) - if (FLIGHT_MODE(NAV_FW_AUTOLAND)) - tmpi += 100000; - if (FLIGHT_MODE(TURTLE_MODE)) - tmpi += 200000; - else if (FLIGHT_MODE(NAV_POSHOLD_MODE) && STATE(AIRPLANE)) - tmpi += 800000; - if (FLIGHT_MODE(NAV_SEND_TO)) - tmpi += 400000; - - // million column (A) - if (FLIGHT_MODE(NAV_RTH_MODE) && isWaypointMissionRTHActive()) - tmpi += 1000000; - if (FLIGHT_MODE(ANGLEHOLD_MODE)) - tmpi += 2000000; - - return tmpi; -} - -static uint16_t frskyGetGPSState(void) -{ - uint16_t tmpi = 0; - - // ones and tens columns (# of satellites 0 - 99) - tmpi += constrain(gpsSol.numSat, 0, 99); - - // hundreds column (satellite accuracy HDOP: 0 = worst [HDOP > 5.5], 9 = best [HDOP <= 1.0]) - tmpi += (9 - constrain((gpsSol.hdop - 51) / 50, 0, 9)) * 100; - - // thousands column (GPS fix status) - if (STATE(GPS_FIX)) - tmpi += 1000; - if (STATE(GPS_FIX_HOME)) - tmpi += 2000; - if (ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXHOMERESET) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE)) - tmpi += 4000; - - return tmpi; -} - smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool useChecksum) { static uint8_t rxBuffer[sizeof(smartPortPayload_t)]; @@ -311,7 +122,137 @@ smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPor return NULL; } +/////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////// +#ifdef USE_CUSTOM_TELEMETRY +static void smartPortSensorEncodeINT(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) { + payload->data = sensor->value; +} + +static void smartPortSensorEncodeAttitude(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + const uint32_t roll = (uint32_t)(telemetrySensorValue(TELEM_ATTITUDE_ROLL) * 10) & 0xFFFF; + const uint32_t pitch = (uint32_t)(telemetrySensorValue(TELEM_ATTITUDE_PITCH) * 10) & 0xFFFF; + + payload->data = roll | pitch << 16; +} + +static void smartPortSensorEncodeFuel(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + if (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_PERCENT) { + payload->data = calculateBatteryPercentage(); + } else if (isAmperageConfigured()) { + payload->data = telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_MAH ? getMAhDrawn() : getMWhDrawn(); + } +} + +static void smartPortSensorEncodeKnots(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + const uint32_t mknots = gpsSol.groundSpeed * 19438UL / 1000; + + payload->data = mknots; +} + +static void smartPortSensorEncodeLat(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + uint32_t lat = (uint32_t)(abs(gpsSol.llh.lat) * 6) / 100; //danger zone, may overflow for latitudes > 3579139.2 degrees, so convert int from abs function to uint32_t + + if (gpsSol.llh.lat < 0) { + lat |= BIT(30); + } + + payload->data = lat; +} + +static void smartPortSensorEncodeLon(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + uint32_t lon = (uint32_t)(abs(gpsSol.llh.lon) * 6) / 100; //danger zone, may overflow for longitudes > 3579139.2 degrees , so convert int from abs function to uint32_t + + if (gpsSol.llh.lon < 0) { + lon |= BIT(30); + } + + payload->data = lon | BIT(31); +} + +static void smartPortSensorEncodeHeading(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + const uint32_t cdeg = gpsSol.groundCourse * 10; + + payload->data = cdeg; +} + +#endif +/////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////// +#ifdef USE_CUSTOM_TELEMETRY +#define TLM_SENSOR(NAME, APPID, FAST, SLOW, WF, WS, DENOM, ENC) \ +{ \ + .sensor_id = TELEM_##NAME, \ + .app_id = (APPID), \ + .fast_interval = (FAST), \ + .slow_interval = (SLOW), \ + .fast_weight = (WF), \ + .slow_weight = (WS), \ + .ratio_num = 1, \ + .ratio_den = (DENOM), \ + .value = 0, \ + .bucket = 0, \ + .update = 0, \ + .active = false, \ + .encode = (telemetryEncode_f)smartPortSensorEncode##ENC, \ +} + +static telemetrySensor_t smartportTelemetrySensors[] = +{ + TLM_SENSOR(HEARTBEAT, 0x5100, 1000, 1000, 0, 0, 0, INT), + + TLM_SENSOR(BATTERY_VOLTAGE, 0x0210, 100, 3000, 1, 10, 0, INT), + TLM_SENSOR(BATTERY_CURRENT, 0x0200, 100, 3000, 1, 10, 10, INT), + TLM_SENSOR(BATTERY_CONSUMPTION, 0x5250, 100, 3000, 1, 10, 0, INT), + TLM_SENSOR(BATTERY_FUEL, 0x0600, 100, 3000, 1, 10, 0, Fuel), + TLM_SENSOR(BATTERY_CHARGE_LEVEL, 0x0600, 100, 3000, 1, 10, 0, INT), + TLM_SENSOR(BATTERY_CELL_COUNT, 0x5260, 200, 3000, 1, 10, 0, INT), + TLM_SENSOR(BATTERY_CELL_VOLTAGE, 0x0910, 200, 3000, 1, 10, 0, INT), + + TLM_SENSOR(HEADING, 0x5210, 100, 3000, 1, 10, 0, INT), +#ifdef USE_BARO + TLM_SENSOR(ALTITUDE, 0x0100, 100, 3000, 1, 10, 0, INT), + TLM_SENSOR(VARIOMETER, 0x0110, 100, 3000, 1, 10, 0, INT), +#endif + TLM_SENSOR(ATTITUDE, 0x0730, 100, 3000, 1, 10, 0, Attitude), + TLM_SENSOR(ACCEL_X, 0x0700, 100, 3000, 1, 10, 0, INT), + TLM_SENSOR(ACCEL_Y, 0x0710, 100, 3000, 1, 10, 0, INT), + TLM_SENSOR(ACCEL_Z, 0x0720, 100, 3000, 1, 10, 0, INT), +#ifdef USE_GPS + TLM_SENSOR(GPS_SATS, 0x0860, 500, 3000, 1, 10, 0, INT), + TLM_SENSOR(GPS_COORD, 0x0800, 100, 3000, 1, 10, 0, Lat), + TLM_SENSOR(GPS_COORD, 0x0800, 100, 3000, 1, 10, 0, Lon), + TLM_SENSOR(GPS_ALTITUDE, 0x0820, 100, 3000, 1, 10, 0, INT), + TLM_SENSOR(GPS_HEADING, 0x0840, 100, 3000, 1, 10, 0, Heading), + TLM_SENSOR(GPS_GROUNDSPEED, 0x0830, 100, 3000, 1, 10, 0, Knots), + TLM_SENSOR(GPS_HOME_DISTANCE, 0x5269, 100, 3000, 1, 10, 0, INT), + TLM_SENSOR(GPS_HOME_DIRECTION, 0x5268, 100, 3000, 1, 10, 0, INT), + TLM_SENSOR(GPS_AZIMUTH, 0x526A, 100, 3000, 1, 10, 0, INT), + TLM_SENSOR(GPS_HDOP, 0x526B, 100, 3000, 1, 10, 0, INT), +#endif + TLM_SENSOR(CPU_LOAD, 0x51D0, 200, 3000, 1, 10, 10, INT), + TLM_SENSOR(FLIGHT_MODE, 0x5121, 100, 3000, 1, 1, 0, INT), + TLM_SENSOR(ARMING_FLAGS, 0x5122, 100, 3000, 1, 1, 0, INT), + +#ifdef USE_ESC_SENSOR + TLM_SENSOR(ESC1_RPM, 0x5130, 100, 3000, 1, 10, 0, INT), + TLM_SENSOR(ESC2_RPM, 0x5131, 100, 3000, 1, 10, 0, INT), + TLM_SENSOR(ESC3_RPM, 0x5132, 100, 3000, 1, 10, 0, INT), + TLM_SENSOR(ESC4_RPM, 0x5133, 100, 3000, 1, 10, 0, INT), + TLM_SENSOR(ESC1_TEMPERATURE, 0x5140, 200, 3000, 1, 10, 0, INT), + TLM_SENSOR(ESC2_TEMPERATURE, 0x5141, 200, 3000, 1, 10, 0, INT), + TLM_SENSOR(ESC3_TEMPERATURE, 0x5142, 200, 3000, 1, 10, 0, INT), + TLM_SENSOR(ESC4_TEMPERATURE, 0x5143, 200, 3000, 1, 10, 0, INT), +#endif +}; +#endif void smartPortSendByte(uint8_t c, uint16_t *checksum, serialPort_t *port) { // smart port escape sequence @@ -327,11 +268,6 @@ void smartPortSendByte(uint8_t c, uint16_t *checksum, serialPort_t *port) } } -bool smartPortPayloadContainsMSP(const smartPortPayload_t *payload) -{ - return payload && (payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT); -} - void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *port, uint16_t checksum) { uint8_t *data = (uint8_t *)payload; @@ -347,14 +283,23 @@ static void smartPortWriteFrameInternal(const smartPortPayload_t *payload) smartPortWriteFrameSerial(payload, smartPortSerialPort, 0); } -static void smartPortSendPackage(uint16_t id, uint32_t val) +static void initSmartPortSensors(void) { - smartPortPayload_t payload; - payload.frameId = FSSP_DATA_FRAME; - payload.valueId = id; - payload.data = val; +#ifdef USE_CUSTOM_TELEMETRY + if(telemetryConfig()->smartport_telemetry_mode == SMARTPORT_TELEMETRY_STATE_LEGACY) { + initSmartPortSensorsLegacy(); + } else { + telemetryScheduleInit(smartportTelemetrySensors, ARRAYLEN(smartportTelemetrySensors)); - smartPortWriteFrame(&payload); + for(size_t i = 0; i < ARRAYLEN(smartportTelemetrySensors); i++) { + if(telemetrySensorActive(smartportTelemetrySensors[i].sensor_id) && telemetrySensorAllowed(smartportTelemetrySensors[i].sensor_id)) { + telemetryScheduleAdd(&smartportTelemetrySensors[i]); + } + } + } +#else + initSmartPortSensorsLegacy(); +#endif } bool initSmartPortTelemetry(void) @@ -363,9 +308,8 @@ bool initSmartPortTelemetry(void) portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT); if (portConfig) { smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT); - smartPortWriteFrame = smartPortWriteFrameInternal; - + initSmartPortSensors(); telemetryState = TELEMETRY_STATE_INITIALIZED_SERIAL; } @@ -379,9 +323,8 @@ bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameEx { if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) { smartPortWriteFrame = smartPortWriteFrameExternal; - + initSmartPortSensors(); telemetryState = TELEMETRY_STATE_INITIALIZED_EXTERNAL; - return true; } @@ -424,20 +367,13 @@ static void smartPortSendMspResponse(uint8_t *data, const uint8_t dataSize) { smartPortWriteFrame(&payload); } -#endif -static bool smartPortShouldSendGPSData(void) +bool smartPortPayloadContainsMSP(const smartPortPayload_t *payload) { - // We send GPS data if the GPS is configured and we have a fix - // or the craft has never been armed yet. This way if GPS stops working - // while in flight, the user will easily notice because the sensor will stop - // updating. - return feature(FEATURE_GPS) && (STATE(GPS_FIX) -#ifdef USE_GPS_FIX_ESTIMATION - || STATE(GPS_ESTIMATED_FIX) -#endif - || !ARMING_FLAG(WAS_EVER_ARMED)); + return payload && (payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT); } +#endif + void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout) { @@ -475,189 +411,18 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear return; } #endif - - // we can send back any data we want, our table keeps track of the order and frequency of each data type we send - uint16_t id = frSkyDataIdTable[smartPortIdCnt]; - if (id == 0) { // end of table reached, loop back - smartPortIdCnt = 0; - id = frSkyDataIdTable[smartPortIdCnt]; - } - smartPortIdCnt++; - - switch (id) { - case FSSP_DATAID_VFAS: - if (isBatteryVoltageConfigured()) { - uint16_t vfasVoltage = telemetryConfig()->report_cell_voltage ? getBatteryAverageCellVoltage() : getBatteryVoltage(); - smartPortSendPackage(id, vfasVoltage); - *clearToSend = false; - } - break; - case FSSP_DATAID_CURRENT: - if (isAmperageConfigured()) { - smartPortSendPackage(id, getAmperage() / 10); // given in 10mA steps, unknown requested unit - *clearToSend = false; - } - break; - //case FSSP_DATAID_RPM: - case FSSP_DATAID_ALTITUDE: - if (sensors(SENSOR_BARO)) { - smartPortSendPackage(id, getEstimatedActualPosition(Z)); // unknown given unit, requested 100 = 1 meter - *clearToSend = false; - } - break; - case FSSP_DATAID_FUEL: - if (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_PERCENT) { - smartPortSendPackage(id, calculateBatteryPercentage()); // Show remaining battery % if smartport_fuel_percent=ON - *clearToSend = false; - } else if (isAmperageConfigured()) { - smartPortSendPackage(id, (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_MAH ? getMAhDrawn() : getMWhDrawn())); - *clearToSend = false; - } - break; - //case FSSP_DATAID_ADC1: - //case FSSP_DATAID_ADC2: - //case FSSP_DATAID_CAP_USED: - case FSSP_DATAID_VARIO: - if (sensors(SENSOR_BARO)) { - smartPortSendPackage(id, lrintf(getEstimatedActualVelocity(Z))); // unknown given unit but requested in 100 = 1m/s - *clearToSend = false; - } - break; - case FSSP_DATAID_HEADING: - smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg - *clearToSend = false; - break; - case FSSP_DATAID_PITCH: - if (telemetryConfig()->frsky_pitch_roll) { - smartPortSendPackage(id, attitude.values.pitch); // given in 10*deg - *clearToSend = false; - } - break; - case FSSP_DATAID_ROLL: - if (telemetryConfig()->frsky_pitch_roll) { - smartPortSendPackage(id, attitude.values.roll); // given in 10*deg - *clearToSend = false; - } - break; - case FSSP_DATAID_ACCX: - if (!telemetryConfig()->frsky_pitch_roll) { - smartPortSendPackage(id, lrintf(100 * acc.accADCf[X])); - *clearToSend = false; - } - break; - case FSSP_DATAID_ACCY: - if (!telemetryConfig()->frsky_pitch_roll) { - smartPortSendPackage(id, lrintf(100 * acc.accADCf[Y])); - *clearToSend = false; - } - break; - case FSSP_DATAID_ACCZ: - if (!telemetryConfig()->frsky_pitch_roll) { - smartPortSendPackage(id, lrintf(100 * acc.accADCf[Z])); - *clearToSend = false; - } - break; - case FSSP_DATAID_MODES: - { - if (telemetryConfig()->frsky_use_legacy_gps_mode_sensor_ids) - id = FSSP_DATAID_LEGACY_MODES; - - smartPortSendPackage(id, frskyGetFlightMode()); - *clearToSend = false; - break; - } -#ifdef USE_GPS - case FSSP_DATAID_GNSS: - { - if (telemetryConfig()->frsky_use_legacy_gps_mode_sensor_ids) - id = FSSP_DATAID_LEGACY_GNSS; - - if (smartPortShouldSendGPSData()) { - smartPortSendPackage(id, frskyGetGPSState()); - *clearToSend = false; - } - break; - } - case FSSP_DATAID_SPEED: - if (smartPortShouldSendGPSData()) { - //convert to knots: 1cm/s = 0.0194384449 knots - //Speed should be sent in knots/1000 (GPS speed is in cm/s) - uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100; - smartPortSendPackage(id, tmpui); - *clearToSend = false; - } - break; - case FSSP_DATAID_LATLONG: - if (smartPortShouldSendGPSData()) { - uint32_t tmpui = 0; - // the same ID is sent twice, one for longitude, one for latitude - // the MSB of the sent uint32_t helps FrSky keep track - // the even/odd bit of our counter helps us keep track - if (smartPortIdCnt & 1) { - tmpui = abs(gpsSol.llh.lon); // now we have unsigned value and one bit to spare - tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast - if (gpsSol.llh.lon < 0) tmpui |= 0x40000000; - } - else { - tmpui = abs(gpsSol.llh.lat); // now we have unsigned value and one bit to spare - tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast - if (gpsSol.llh.lat < 0) tmpui |= 0x40000000; - } - smartPortSendPackage(id, tmpui); - *clearToSend = false; - } - break; - case FSSP_DATAID_HOME_DIST: - if (smartPortShouldSendGPSData()) { - smartPortSendPackage(id, GPS_distanceToHome); - *clearToSend = false; - } - break; - case FSSP_DATAID_GPS_ALT: - if (smartPortShouldSendGPSData()) { - smartPortSendPackage(id, gpsSol.llh.alt); // cm - *clearToSend = false; - } - break; - case FSSP_DATAID_FPV: - if (smartPortShouldSendGPSData()) { - smartPortSendPackage(id, gpsSol.groundCourse); // given in 10*deg - *clearToSend = false; - } - break; - case FSSP_DATAID_AZIMUTH: - if (smartPortShouldSendGPSData()) { - int16_t h = GPS_directionToHome; - if (h < 0) { - h += 360; - } - if(h >= 180) - h = h - 180; - else - h = h + 180; - smartPortSendPackage(id, h *10); // given in 10*deg - *clearToSend = false; - } - break; -#endif - case FSSP_DATAID_A4: - if (isBatteryVoltageConfigured()) { - smartPortSendPackage(id, getBatteryAverageCellVoltage()); - *clearToSend = false; - } - break; - case FSSP_DATAID_ASPD: -#ifdef USE_PITOT - if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { - smartPortSendPackage(id, getAirspeedEstimate() * 0.194384449f); // cm/s to knots*1 - *clearToSend = false; - } -#endif - break; - default: - break; - // if nothing is sent, hasRequest isn't cleared, we already incremented the counter, just loop back to the start + telemetrySensor_t *sensor = telemetryScheduleNext(); + if (sensor) { + smartPortPayload_t frame; + frame.frameId = FSSP_DATA_FRAME; + frame.valueId = sensor->app_id; + sensor->encode(sensor, &frame); + + telemetryScheduleCommit(sensor); + smartPortWriteFrame(&frame); + *clearToSend = false; } + } } @@ -666,18 +431,30 @@ static bool serialCheckQueueEmpty(void) return (serialRxBytesWaiting(smartPortSerialPort) == 0); } -void handleSmartPortTelemetry(void) +void handleSmartPortTelemetry(timeUs_t currentTimeUs) { - if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL && smartPortSerialPort) { - bool clearToSend = false; - smartPortPayload_t *payload = NULL; - const uint32_t requestTimeout = millis() + SMARTPORT_SERVICE_TIMEOUT_MS; - while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) { - uint8_t c = serialRead(smartPortSerialPort); - payload = smartPortDataReceive(c, &clearToSend, serialCheckQueueEmpty, true); - } + if (telemetryState != TELEMETRY_STATE_UNINITIALIZED) { + + telemetryScheduleUpdate(currentTimeUs); + + if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL && smartPortSerialPort) { + const uint32_t requestTimeout = millis() + SMARTPORT_SERVICE_TIMEOUT_MS; - processSmartPortTelemetry(payload, &clearToSend, &requestTimeout); + bool clearToSend = false; + smartPortPayload_t *payload = NULL; + + while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) { + uint8_t c = serialRead(smartPortSerialPort); + payload = smartPortDataReceive(c, &clearToSend, serialCheckQueueEmpty, true); + } + + processSmartPortTelemetry(payload, &clearToSend, &requestTimeout); + + if (clearToSend) { + const smartPortPayload_t emptySmartPortFrame = { 0, }; + smartPortWriteFrame(&emptySmartPortFrame); + } + } } } #endif diff --git a/src/main/telemetry/smartport.h b/src/main/telemetry/smartport.h index 0a682b2165c..d794b5ad8da 100644 --- a/src/main/telemetry/smartport.h +++ b/src/main/telemetry/smartport.h @@ -1,9 +1,3 @@ -/* - * smartport.h - * - * Created on: 25 October 2014 - * Author: Frank26080115 - */ #pragma once @@ -49,7 +43,7 @@ bool initSmartPortTelemetry(void); void checkSmartPortTelemetryState(void); bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameExternal); -void handleSmartPortTelemetry(void); +void handleSmartPortTelemetry(timeUs_t currentTimeUs); void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *hasRequest, const uint32_t *requestTimeout); smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool withChecksum); diff --git a/src/main/telemetry/smartport_legacy.c b/src/main/telemetry/smartport_legacy.c new file mode 100644 index 00000000000..59c44725e70 --- /dev/null +++ b/src/main/telemetry/smartport_legacy.c @@ -0,0 +1,399 @@ +/************************************************************** +************************************************************** +************************************************************** +************************************************************** +** ** +** ** +** REMOVE IN INAV 11 ** +** ** +** ** +************************************************************** +************************************************************** +************************************************************** +**************************************************************/ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ +#include +#include +#include +#include +#include + +#include "platform.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT) + +#include "smartport_legacy.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "config/parameter_group_ids.h" + +#include "drivers/time.h" + +#include "sensors/battery.h" + +#include "io/gps.h" +#include "io/serial.h" + +#include "rx/frsky_crc.h" + +#include "telemetry/telemetry.h" +#include "telemetry/smartport.h" +#include "telemetry/smartport_legacy.h" +#include "telemetry/msp_shared.h" + +#include "common/axis.h" +#include "common/color.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "drivers/accgyro/accgyro.h" +#include "drivers/compass/compass.h" +#include "drivers/sensor.h" +#include "drivers/time.h" + +#include "fc/config.h" +#include "fc/rc_controls.h" +#include "fc/rc_modes.h" +#include "fc/runtime_config.h" + +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/pid.h" + +#include "io/beeper.h" +#include "io/gps.h" +#include "io/serial.h" + +#include "navigation/navigation.h" + +#include "rx/frsky_crc.h" + +#include "sensors/boardalignment.h" +#include "sensors/sensors.h" +#include "sensors/battery.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/compass.h" +#include "sensors/gyro.h" +#include "sensors/pitotmeter.h" + +#include "rx/rx.h" + +#include "telemetry/telemetry.h" +#include "telemetry/smartport.h" +#include "telemetry/msp_shared.h" + +////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////// +static uint32_t frskyGetFlightMode(void) +{ + uint32_t tmpi = 0; + + // ones column (G) + if (!isArmingDisabled()) + tmpi += 1; + else + tmpi += 2; + if (ARMING_FLAG(ARMED)) + tmpi += 4; + + // tens column (F) + if (FLIGHT_MODE(ANGLE_MODE)) + tmpi += 10; + if (FLIGHT_MODE(HORIZON_MODE)) + tmpi += 20; + if (FLIGHT_MODE(MANUAL_MODE)) + tmpi += 40; + + // hundreds column (E) + if (FLIGHT_MODE(HEADING_MODE)) + tmpi += 100; + if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) + tmpi += 200; + if (FLIGHT_MODE(NAV_POSHOLD_MODE) && !STATE(AIRPLANE)) + tmpi += 400; + + // thousands column (D) + if (FLIGHT_MODE(NAV_RTH_MODE) && !isWaypointMissionRTHActive()) + tmpi += 1000; + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) // intentionally out of order and 'else-ifs' to prevent column overflow + tmpi += 8000; + else if (FLIGHT_MODE(NAV_WP_MODE)) + tmpi += 2000; + else if (FLIGHT_MODE(HEADFREE_MODE)) + tmpi += 4000; + + // ten thousands column (C) + if (FLIGHT_MODE(FLAPERON)) + tmpi += 10000; + if (FLIGHT_MODE(FAILSAFE_MODE)) + tmpi += 40000; + else if (FLIGHT_MODE(AUTO_TUNE)) // intentionally reverse order and 'else-if' to prevent 16-bit overflow + tmpi += 20000; + + // hundred thousands column (B) + if (FLIGHT_MODE(NAV_FW_AUTOLAND)) + tmpi += 100000; + if (FLIGHT_MODE(TURTLE_MODE)) + tmpi += 200000; + else if (FLIGHT_MODE(NAV_POSHOLD_MODE) && STATE(AIRPLANE)) + tmpi += 800000; + if (FLIGHT_MODE(NAV_SEND_TO)) + tmpi += 400000; + + // million column (A) + if (FLIGHT_MODE(NAV_RTH_MODE) && isWaypointMissionRTHActive()) + tmpi += 1000000; + if (FLIGHT_MODE(ANGLEHOLD_MODE)) + tmpi += 2000000; + + return tmpi; +} + +static uint16_t frskyGetGPSState(void) +{ + uint16_t tmpi = 0; + + // ones and tens columns (# of satellites 0 - 99) + tmpi += constrain(gpsSol.numSat, 0, 99); + + // hundreds column (satellite accuracy HDOP: 0 = worst [HDOP > 5.5], 9 = best [HDOP <= 1.0]) + tmpi += (9 - constrain((gpsSol.hdop - 51) / 50, 0, 9)) * 100; + + // thousands column (GPS fix status) + if (STATE(GPS_FIX)) + tmpi += 1000; + if (STATE(GPS_FIX_HOME)) + tmpi += 2000; + if (ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXHOMERESET) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE)) + tmpi += 4000; + + return tmpi; +} + +static void smartPortSensorEncodeINT(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) { + payload->data = sensor->value; +} + + +static void smartPortSensorEncodeVFAS(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + payload->data = telemetryConfig()->report_cell_voltage ? getBatteryAverageCellVoltage() : getBatteryVoltage();; +} + +static void smartPortSensorEncodeCurrent(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + payload->data = getAmperage() / 10; +} + +static void smartPortSensorEncodeAltitude(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + payload->data = (uint32_t)getEstimatedActualPosition(Z); +} + +static void smartPortSensorEncodeFuel(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + if (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_PERCENT) { + payload->data = calculateBatteryPercentage(); // Show remaining battery % if smartport_fuel_percent=ON + } else if (isAmperageConfigured()) { + payload->data = telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_MAH ? getMAhDrawn() : getMWhDrawn(); + } +} + +static void smartPortSensorEncodeVario(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + payload->data = lrintf(getEstimatedActualVelocity(Z)); +} + +static void smartPortSensorEncodeHeading(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + payload->data = attitude.values.yaw * 10; +} + +static void smartPortSensorEncodePitch(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + payload->data = attitude.values.pitch; +} + +static void smartPortSensorEncodeRoll(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + payload->data = attitude.values.roll; +} + +static void smartPortSensorEncodeACCX(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + payload->data = lrintf(100 * acc.accADCf[X]); +} + +static void smartPortSensorEncodeACCY(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + payload->data = lrintf(100 * acc.accADCf[Y]); +} + +static void smartPortSensorEncodeACCZ(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + payload->data = lrintf(100 * acc.accADCf[Z]); +} + +static void smartPortSensorEncodeModes(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + payload->data = frskyGetFlightMode(); +} + +#ifdef USE_GPS +static void smartPortSensorEncodeGNSS(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + payload->data = frskyGetGPSState(); +} + +static void smartPortSensorEncodeSpeed(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + //convert to knots: 1cm/s = 0.0194384449 knots + //Speed should be sent in knots/1000 (GPS speed is in cm/s) + payload->data = gpsSol.groundSpeed * 1944 / 100; +} + +static void smartPortSensorEncodeLat(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + uint32_t tmpui = abs(gpsSol.llh.lon); // now we have unsigned value and one bit to spare + tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast + if (gpsSol.llh.lon < 0) tmpui |= 0x40000000; + + payload->data = tmpui; +} + +static void smartPortSensorEncodeLon(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + uint32_t tmpui = abs(gpsSol.llh.lat); // now we have unsigned value and one bit to spare + tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast + if (gpsSol.llh.lat < 0) tmpui |= 0x40000000; + + payload->data = tmpui; +} + +static void smartPortSensorEncodeHomeDist(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + payload->data = GPS_distanceToHome; +} + +static void smartPortSensorEncodeGpsAlt(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + payload->data = gpsSol.llh.alt; +} + +static void smartPortSensorEncodeFpv(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + payload->data = gpsSol.groundCourse; +} + +static void smartPortSensorEncodeAzimuth(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + int16_t h = GPS_directionToHome; + if (h < 0) { + h += 360; + } + if(h >= 180) + h = h - 180; + else + h = h + 180; + + payload->data = h; +} +#endif +static void smartPortSensorEncodeA4(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + payload->data = getBatteryAverageCellVoltage(); +} + +#ifdef USE_PITOT +static void smartPortSensorEncodeASPD(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) +{ + payload->data = (uint32_t)(getAirspeedEstimate() * 0.194384449f); +} +#endif + + +////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////// +#define TLM_SENSOR(NAME, APPID, FAST, SLOW, WF, WS, DENOM, ENC) \ +{ \ + .sensor_id = TELEM_##NAME, \ + .app_id = (APPID), \ + .fast_interval = (FAST), \ + .slow_interval = (SLOW), \ + .fast_weight = (WF), \ + .slow_weight = (WS), \ + .ratio_num = 1, \ + .ratio_den = (DENOM), \ + .value = 0, \ + .bucket = 0, \ + .update = 0, \ + .active = false, \ + .encode = (telemetryEncode_f)smartPortSensorEncode##ENC, \ +} + +static telemetrySensor_t smartportTelemetrySensorsLegacy[] = +{ + TLM_SENSOR(HEARTBEAT , 0x5100, 1000, 1000, 0, 0, 0, INT), + TLM_SENSOR(LEGACY_VFAS , 0x0210, 200, 200, 0, 0, 0, VFAS), + TLM_SENSOR(LEGACY_CURRENT , 0x0200, 200, 200, 0, 0, 0, Current), + TLM_SENSOR(LEGACY_ALTITUDE , 0x0100, 200, 200, 0, 0, 0, Altitude), + TLM_SENSOR(LEGACY_FUEL , 0x0600, 200, 200, 0, 0, 0, Fuel), + TLM_SENSOR(LEGACY_VARIO , 0x0110, 200, 200, 0, 0, 0, Vario), + TLM_SENSOR(LEGACY_HEADING , 0x0840, 200, 200, 0, 0, 0, Heading), + TLM_SENSOR(LEGACY_PITCH , 0x0430, 200, 200, 0, 0, 0, Pitch), + TLM_SENSOR(LEGACY_ROLL , 0x0440, 200, 200, 0, 0, 0, Roll), + TLM_SENSOR(LEGACY_ACCX , 0x0700, 200, 200, 0, 0, 0, ACCX), + TLM_SENSOR(LEGACY_ACCY , 0x0710, 200, 200, 0, 0, 0, ACCY), + TLM_SENSOR(LEGACY_ACCZ , 0x0720, 200, 200, 0, 0, 0, ACCZ), + TLM_SENSOR(LEGACY_MODES , 0x0470, 200, 200, 0, 0, 0, Modes), +#ifdef USE_GPS + TLM_SENSOR(LEGACY_GNSS , 0x0480, 200, 200, 0, 0, 0, GNSS), + TLM_SENSOR(LEGACY_SPEED , 0x0830, 200, 200, 0, 0, 0, Speed), + TLM_SENSOR(LEGACY_LAT , 0x0800, 200, 200, 0, 0, 0, Lat), + TLM_SENSOR(LEGACY_LON , 0x0800, 200, 200, 0, 0, 0, Lon), + TLM_SENSOR(LEGACY_HOME_DIST , 0x0420, 200, 200, 0, 0, 0, HomeDist), + TLM_SENSOR(LEGACY_GPS_ALT , 0x0820, 200, 200, 0, 0, 0, GpsAlt), + TLM_SENSOR(LEGACY_FPV , 0x0450, 200, 200, 0, 0, 0, Fpv), + TLM_SENSOR(LEGACY_AZIMUTH , 0x0460, 200, 200, 0, 0, 0, Azimuth), +#endif + TLM_SENSOR(LEGACY_A4 , 0x0910, 200, 200, 0, 0, 0, A4), +#ifdef USE_PITOT + TLM_SENSOR(LEGACY_ASPD , 0x0A00, 200, 200, 0, 0, 0, ASPD), +#endif +}; + +void initSmartPortSensorsLegacy(void) { + telemetryScheduleInit(smartportTelemetrySensorsLegacy, ARRAYLEN(smartportTelemetrySensorsLegacy)); + + for(size_t i = 0; i < ARRAYLEN(smartportTelemetrySensorsLegacy); i++) { + if(telemetrySensorAllowed(smartportTelemetrySensorsLegacy[i].sensor_id)) { + telemetryScheduleAdd(&smartportTelemetrySensorsLegacy[i]); + } + } + +} + + +#endif \ No newline at end of file diff --git a/src/main/telemetry/smartport_legacy.h b/src/main/telemetry/smartport_legacy.h new file mode 100644 index 00000000000..992417381dd --- /dev/null +++ b/src/main/telemetry/smartport_legacy.h @@ -0,0 +1,44 @@ +/************************************************************** +************************************************************** +************************************************************** +************************************************************** +** ** +** ** +** REMOVE IN INAV 11 ** +** ** +** ** +************************************************************** +************************************************************** +************************************************************** +**************************************************************/ + +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + + +#pragma once + +#include "platform.h" + +#include +#include + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT) + +void initSmartPortSensorsLegacy(void); + +#endif \ No newline at end of file diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 2c896945843..002376c119a 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -17,7 +17,7 @@ #include #include -#include +#include #include "platform.h" @@ -26,6 +26,7 @@ #include "build/debug.h" #include "common/utils.h" +#include "common/maths.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" @@ -51,18 +52,18 @@ #include "telemetry/ibus.h" #include "telemetry/crsf.h" #include "telemetry/srxl.h" -#include "telemetry/sbus2.h" #include "telemetry/sim.h" #include "telemetry/ghst.h" +#include "telemetry/sbus2.h" + -PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 8); +PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, /*version*/9); PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT, .telemetry_inverted = SETTING_TELEMETRY_INVERTED_DEFAULT, .frsky_pitch_roll = SETTING_FRSKY_PITCH_ROLL_DEFAULT, - .frsky_use_legacy_gps_mode_sensor_ids = SETTING_FRSKY_USE_LEGACY_GPS_MODE_SENSOR_IDS_DEFAULT, .report_cell_voltage = SETTING_REPORT_CELL_VOLTAGE_DEFAULT, .hottAlarmSoundInterval = SETTING_HOTT_ALARM_SOUND_INTERVAL_DEFAULT, .halfDuplex = SETTING_TELEMETRY_HALFDUPLEX_DEFAULT, @@ -98,7 +99,25 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT, .radio_type = SETTING_MAVLINK_RADIO_TYPE_DEFAULT, .sysid = SETTING_MAVLINK_SYSID_DEFAULT - } + }, +#ifdef USE_TELEMETRY_CRSF + .crsf_telemetry_link_rate = SETTING_CRSF_TELEMETRY_LINK_RATE_DEFAULT, + .crsf_telemetry_link_ratio = SETTING_CRSF_TELEMETRY_LINK_RATIO_DEFAULT, +#ifdef USE_CUSTOM_TELEMETRY + .crsf_telemetry_mode = SETTING_CRSF_TELEMETRY_MODE_DEFAULT, +#endif //USE_CUSTOM_TELEMETRY +#endif //USE_TELEMETRY_CRSF + +#if defined(USE_TELEMETRY_SMARTPORT) && defined(USE_CUSTOM_TELEMETRY) +#if !defined(SETTING_SMARTPORT_TELEMETRY_MODE_DEFAULT) // SITL + .smartport_telemetry_mode = 0, +#else + .smartport_telemetry_mode = SETTING_SMARTPORT_TELEMETRY_MODE_DEFAULT, +#endif +#endif // USE_TELEMETRY_SMARTPORT USE_CUSTOM_TELEMETRY +#ifdef USE_CUSTOM_TELEMETRY + .telemetry_sensors = { 0x0, }, // all sensors enabled by default +#endif ); void telemetryInit(void) @@ -220,7 +239,7 @@ void telemetryProcess(timeUs_t currentTimeUs) #endif #if defined(USE_TELEMETRY_SMARTPORT) - handleSmartPortTelemetry(); + handleSmartPortTelemetry(currentTimeUs); #endif #if defined(USE_TELEMETRY_LTM) @@ -259,4 +278,84 @@ void telemetryProcess(timeUs_t currentTimeUs) #endif } + +/** Telemetry scheduling framework **/ +static telemetryScheduler_t sch = { 0, }; + +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) { + int value = telemetrySensorValue(sensor->sensor_id); + if (sensor->ratio_den) + value = value * sensor->ratio_num / sensor->ratio_den; + sensor->update |= (value != sensor->value); + sensor->value = value; + + const int interval = (sensor->update) ? sensor->fast_interval : sensor->slow_interval; + sensor->bucket += delta * 1000 / interval; + sensor->bucket = constrain(sensor->bucket, sch.min_level, sch.max_level); + } + } + + 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) + return sensor; + } + + return NULL; +} + +void telemetryScheduleAdd(telemetrySensor_t * sensor) +{ + if (sensor) { + sensor->bucket = 0; + sensor->value = 0; + sensor->update = true; + sensor->active = true; + } +} + +void telemetryScheduleCommit(telemetrySensor_t * sensor) +{ + if (sensor) { + sensor->bucket = constrain(sensor->bucket - sch.quanta, sch.min_level, sch.max_level); + sensor->update = false; + + sch.start_index = sensor->index; + } +} + +void telemetryScheduleInit(telemetrySensor_t * sensors, size_t count) +{ + sch.sensors = sensors; + sch.sensor_count = count; + + sch.update_time = 0; + sch.start_index = 0; + + sch.quanta = 1000000; + sch.max_level = 500000; + sch.min_level = -1500000; + + for (unsigned int i = 0; i < count; i++) { + telemetrySensor_t * sensor = &sch.sensors[i]; + sensor->index = i; + } +} + #endif + + diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 7fb26781c11..d4def39c2c6 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -25,10 +25,11 @@ #pragma once #include "common/time.h" - #include "config/parameter_group.h" - #include "io/serial.h" +#include "telemetry/sensors.h" + +#define TELEM_SENSOR_SLOT_COUNT 30 typedef enum { LTM_RATE_NORMAL, @@ -53,11 +54,22 @@ typedef enum { SMARTPORT_FUEL_UNIT_MWH } smartportFuelUnit_e; +typedef enum { + CRSFR_TELEMETRY_STATE_OFF = 0, + CRSFR_TELEMETRY_STATE_NATIVE, + CRSFR_TELEMETRY_STATE_CUSTOM, + CRSFR_TELEMETRY_STATE_POPULATE, +} crsfTelemetryMode_e; + +typedef enum { + SMARTPORT_TELEMETRY_STATE_LEGACY = 0, + SMARTPORT_CRSFR_TELEMETRY_STATE_STANDARD, +} smartportTelemetryMode_e; + typedef struct telemetryConfig_s { uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed. uint8_t telemetry_inverted; // Flip the default inversion of the protocol - Same as serialrx_inverted in rx.c, but for telemetry. uint8_t frsky_pitch_roll; - bool frsky_use_legacy_gps_mode_sensor_ids; uint8_t report_cell_voltage; uint8_t hottAlarmSoundInterval; uint8_t halfDuplex; @@ -89,8 +101,33 @@ typedef struct telemetryConfig_s { uint8_t radio_type; uint8_t sysid; } mavlink; +#ifdef USE_TELEMETRY_CRSF + uint16_t crsf_telemetry_link_rate; + uint16_t crsf_telemetry_link_ratio; +#ifdef USE_CUSTOM_TELEMETRY + uint8_t crsf_telemetry_mode; +#endif +#endif + +#if defined(USE_TELEMETRY_SMARTPORT) && defined(USE_CUSTOM_TELEMETRY) + uint8_t smartport_telemetry_mode; +#endif + +#ifdef USE_CUSTOM_TELEMETRY + uint16_t telemetry_sensors[TELEM_SENSOR_SLOT_COUNT]; +#endif } telemetryConfig_t; +typedef struct { + telemetrySensor_t * sensors; + uint16_t sensor_count; + uint16_t start_index; + timeUs_t update_time; + int32_t max_level; + int32_t min_level; + int32_t quanta; +} telemetryScheduler_t; + PG_DECLARE(telemetryConfig_t, telemetryConfig); #define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_IBUS) @@ -103,3 +140,8 @@ void telemetryCheckState(void); void telemetryProcess(timeUs_t currentTimeUs); bool telemetryDetermineEnabledState(portSharing_e portSharing); +void telemetryScheduleAdd(telemetrySensor_t * sensor); +void telemetryScheduleUpdate(timeUs_t currentTime); +void telemetryScheduleCommit(telemetrySensor_t * sensor); +void telemetryScheduleInit(telemetrySensor_t * sensors, size_t count); +telemetrySensor_t * telemetryScheduleNext(void); From 4caa885c29663472048839b0e967c71e83647d25 Mon Sep 17 00:00:00 2001 From: Error414 Date: Sun, 7 Dec 2025 16:22:29 +0100 Subject: [PATCH 2/5] add profile to extended telemetry --- src/main/telemetry/crsf.c | 1 + src/main/telemetry/sensors.c | 7 ++++- src/main/telemetry/sensors.h | 57 +++++++++++++++++----------------- src/main/telemetry/smartport.c | 1 + 4 files changed, 37 insertions(+), 29 deletions(-) diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index a85497ef16c..9b458081a1d 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -460,6 +460,7 @@ static telemetrySensor_t crsfCustomTelemetrySensors[] = TLM_SENSOR(CPU_LOAD, 0x1150, 500, 3000, 10, U8), TLM_SENSOR(FLIGHT_MODE, 0x1251, 200, 3000, 0, U16), TLM_SENSOR(ARMING_FLAGS, 0x1252, 200, 3000, 0, U8), + TLM_SENSOR(PROFILES, 0x1253, 200, 3000, 0, U16), }; telemetrySensor_t * crsfGetCustomSensor(sensor_id_e id) diff --git a/src/main/telemetry/sensors.c b/src/main/telemetry/sensors.c index ae8ddecde80..ae340402b33 100644 --- a/src/main/telemetry/sensors.c +++ b/src/main/telemetry/sensors.c @@ -166,6 +166,10 @@ int telemetrySensorValue(sensor_id_e id) ////// SYSTEM ////////////////////////// case TELEM_FLIGHT_MODE: return (int)flightModeFlags; + case TELEM_PROFILES: + return ((getConfigBatteryProfile() & 0xF) << 8) | + ((getConfigMixerProfile() & 0xF) << 4) | + ((getConfigProfile() & 0xF)); case TELEM_ARMING_FLAGS: return (int)armingFlags; case TELEM_CPU_LOAD: @@ -376,11 +380,12 @@ bool telemetrySensorAllowed(sensor_id_e id) ////// SYSTEM ////////////////////////// case TELEM_CPU_LOAD: return true; - case TELEM_FLIGHT_MODE: return true; case TELEM_ARMING_FLAGS: return true; + case TELEM_PROFILES: + return true; //////////////////////////////////////// ////// LEGACY SMARTPORT //////////////// diff --git a/src/main/telemetry/sensors.h b/src/main/telemetry/sensors.h index 391704b880b..4cdcd6e2510 100644 --- a/src/main/telemetry/sensors.h +++ b/src/main/telemetry/sensors.h @@ -92,34 +92,35 @@ typedef enum ////// SYSTEM ////////////////////////// TELEM_CPU_LOAD = 42, TELEM_FLIGHT_MODE = 43, - TELEM_ARMING_FLAGS = 44, - - ///////////////////////////////////////// - ////// LEGACY SMARTPORT //////////////// - TELEM_LEGACY_VFAS = 45, - TELEM_LEGACY_CURRENT = 46, - TELEM_LEGACY_ALTITUDE = 47, - TELEM_LEGACY_FUEL = 48, - TELEM_LEGACY_VARIO = 49, - TELEM_LEGACY_HEADING = 50, - TELEM_LEGACY_PITCH = 51, - TELEM_LEGACY_ROLL = 52, - TELEM_LEGACY_ACCX = 53, - TELEM_LEGACY_ACCY = 54, - TELEM_LEGACY_ACCZ = 55, - TELEM_LEGACY_MODES = 56, - - TELEM_LEGACY_GNSS = 57, - TELEM_LEGACY_SPEED = 58, - TELEM_LEGACY_LAT = 59, - TELEM_LEGACY_LON = 60, - TELEM_LEGACY_HOME_DIST = 61, - TELEM_LEGACY_GPS_ALT = 62, - TELEM_LEGACY_FPV = 63, - TELEM_LEGACY_AZIMUTH = 64, - - TELEM_LEGACY_A4 = 65, - TELEM_LEGACY_ASPD = 66, + TELEM_PROFILES = 44, + TELEM_ARMING_FLAGS = 45, + +///////////////////////////////////////// +////// LEGACY SMARTPORT //////////////// + TELEM_LEGACY_VFAS = 46, + TELEM_LEGACY_CURRENT = 47, + TELEM_LEGACY_ALTITUDE = 48, + TELEM_LEGACY_FUEL = 49, + TELEM_LEGACY_VARIO = 50, + TELEM_LEGACY_HEADING = 51, + TELEM_LEGACY_PITCH = 52, + TELEM_LEGACY_ROLL = 53, + TELEM_LEGACY_ACCX = 54, + TELEM_LEGACY_ACCY = 55, + TELEM_LEGACY_ACCZ = 56, + TELEM_LEGACY_MODES = 57, + + TELEM_LEGACY_GNSS = 58, + TELEM_LEGACY_SPEED = 59, + TELEM_LEGACY_LAT = 60, + TELEM_LEGACY_LON = 61, + TELEM_LEGACY_HOME_DIST = 62, + TELEM_LEGACY_GPS_ALT = 63, + TELEM_LEGACY_FPV = 64, + TELEM_LEGACY_AZIMUTH = 65, + + TELEM_LEGACY_A4 = 66, + TELEM_LEGACY_ASPD = 67, TELEM_SENSOR_COUNT diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 323ef590a3a..3df339d460e 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -240,6 +240,7 @@ static telemetrySensor_t smartportTelemetrySensors[] = TLM_SENSOR(CPU_LOAD, 0x51D0, 200, 3000, 1, 10, 10, INT), TLM_SENSOR(FLIGHT_MODE, 0x5121, 100, 3000, 1, 1, 0, INT), TLM_SENSOR(ARMING_FLAGS, 0x5122, 100, 3000, 1, 1, 0, INT), + TLM_SENSOR(PROFILES, 0x5123, 100, 3000, 1, 1, 0, INT), #ifdef USE_ESC_SENSOR TLM_SENSOR(ESC1_RPM, 0x5130, 100, 3000, 1, 10, 0, INT), From 93833d74f3594bc75ad5e40df19da1fa410ced45 Mon Sep 17 00:00:00 2001 From: Error414 Date: Sun, 7 Dec 2025 21:50:19 +0100 Subject: [PATCH 3/5] change MPS api version --- src/main/msp/msp_protocol.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 5a3af115f9c..b2a5923b41a 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -59,7 +59,7 @@ #define MSP_PROTOCOL_VERSION 0 // Same version over MSPv1 & MSPv2 - message format didn't change and it backward compatible #define API_VERSION_MAJOR 2 // increment when major changes are made -#define API_VERSION_MINOR 5 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR +#define API_VERSION_MINOR 6 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 From 646bf7eb1bd9e0dba44115d24c5d33a4a389162e Mon Sep 17 00:00:00 2001 From: Error414 Date: Fri, 19 Dec 2025 17:37:07 +0100 Subject: [PATCH 4/5] fix CRSF tESC temp --- src/main/telemetry/crsf.c | 23 +++-------------------- 1 file changed, 3 insertions(+), 20 deletions(-) diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 9b458081a1d..8bbc1044b61 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -302,8 +302,7 @@ void crsfSensorEncodeEscRpm(telemetrySensor_t *sensor, sbuf_t *buf) } } -void crsfSensorEncodeEscTemp(telemetrySensor_t *sensor, sbuf_t *buf) -{ +void crsfSensorEncodeEscTemp(telemetrySensor_t *sensor, sbuf_t *buf) { UNUSED(sensor); uint8_t motorCount = MAX(getMotorCount(), 1); //must send at least one motor, to avoid CRSF frame shifting motorCount = MIN(getMotorCount(), CRSF_PAYLOAD_SIZE_MAX / 3); // 3 bytes per RPM value @@ -311,26 +310,10 @@ void crsfSensorEncodeEscTemp(telemetrySensor_t *sensor, sbuf_t *buf) for (uint8_t i = 0; i < motorCount; i++) { const escSensorData_t *escState = getEscTelemetry(i); - crsfSerialize16BE(buf, escState->temperature & 0xFFFF); + uint32_t temp = (escState) ? (escState->temperature * 10) & 0xFFFFFF : TEMPERATURE_INVALID_VALUE; + crsfSerialize24BE(buf, temp); } } - -void crsfSensorEncodeEscTemperature(telemetrySensor_t *sensor, sbuf_t *buf) -{ - UNUSED(sensor); - - uint8_t motorCount = MAX(getMotorCount(), 1); //must send at least one motor, to avoid CRSF frame shifting - motorCount = MIN(getMotorCount(), CRSF_PAYLOAD_SIZE_MAX / 3); // 3 bytes per RPM value - motorCount = MIN(motorCount, MAX_SUPPORTED_MOTORS); // ensure we don't exceed available ESC telemetry data - - for (uint8_t i = 0; i < motorCount; i++) { - const escSensorData_t *escState = getEscTelemetry(i); - uint32_t rpm = (escState) ? (escState->temperature * 10) & 0xFFFFFF : TEMPERATURE_INVALID_VALUE; - crsfSerialize24BE(buf, rpm); - } - -} - #endif // USE_CUSTOM_TELEMETRY /////////////////////////////////////////////////////////////////////////////////////////// From 1688c68e6d834ea47897389621733093fc2e643d Mon Sep 17 00:00:00 2001 From: Error414 Date: Mon, 22 Dec 2025 11:13:54 +0100 Subject: [PATCH 5/5] make only one setting for custom telemetry --- docs/Settings.md | 30 ++++++++++-------------------- src/main/fc/settings.yaml | 21 ++++++--------------- src/main/telemetry/crsf.c | 14 +++++++------- src/main/telemetry/smartport.c | 2 +- src/main/telemetry/telemetry.c | 14 +++----------- src/main/telemetry/telemetry.h | 27 +++++++++++---------------- 6 files changed, 38 insertions(+), 70 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index f40165cbbc9..4e9d009bb14 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -602,16 +602,6 @@ CRSF telemetry link ratio --- -### crsf_telemetry_mode - -Use extended custom or native telemetry sensors for CRSF - -| Default | Min | Max | -| --- | --- | --- | -| NATIVE | | | - ---- - ### cruise_power Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit @@ -6222,16 +6212,6 @@ _// TODO_ --- -### smartport_telemetry_mode - -Use legacy or standard telemetry sensors for SmartPort - -| Default | Min | Max | -| --- | --- | --- | -| LEGACY | | | - ---- - ### smith_predictor_delay Expected delay of the gyro signal. In milliseconds @@ -6382,6 +6362,16 @@ Determines if the telemetry protocol default signal inversion is reversed. This --- +### telemetry_mode + +Use extended custom or native telemetry sensors for CRSF or SmartPort + +| Default | Min | Max | +| --- | --- | --- | +| LEGACY | | | + +--- + ### telemetry_switch Which aux channel to use to change serial output & baud rate (MSP / Telemetry). It disables automatic switching to Telemetry when armed. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a56c6470b45..7cca5b47eb2 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -103,12 +103,9 @@ tables: - name: bat_voltage_source values: ["RAW", "SAG_COMP"] enum: batVoltageSource_e - - name: crsf_telemetry_modes - values: [ "OFF", "NATIVE", "CUSTOM" ] - enum: crsfTelemetryMode_e - - name: smartport_telemetry_modes - values: [ "LEGACY", "STANDARD" ] - enum: smartportTelemetryMode_e + - name: telemetry_modes + values: [ "LEGACY", "CUSTOM" ] + enum: telemetryMode_e - name: smartport_fuel_unit values: ["PERCENT", "MAH", "MWH"] enum: smartportFuelUnit_e @@ -3123,17 +3120,11 @@ groups: description: "S.Port telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data" default_value: OFF type: bool - - name: crsf_telemetry_mode - description: "Use extended custom or native telemetry sensors for CRSF" - default_value: NATIVE - condition: USE_CUSTOM_TELEMETRY - table: crsf_telemetry_modes - type: uint8_t - - name: smartport_telemetry_mode - description: "Use legacy or standard telemetry sensors for SmartPort" + - name: telemetry_mode + description: "Use extended custom or native telemetry sensors for CRSF or SmartPort" default_value: LEGACY condition: USE_CUSTOM_TELEMETRY - table: smartport_telemetry_modes + table: telemetry_modes type: uint8_t - name: crsf_telemetry_link_rate description: "CRSF telemetry link rate" diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 8bbc1044b61..132278a7b6e 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -739,7 +739,7 @@ void crsfSendMspResponse(uint8_t *payload, const uint8_t payloadSize) static bool crsfSendNativeTelemetry(void) { - if (crsfTelemetryState != CRSFR_TELEMETRY_STATE_NATIVE) + if (crsfTelemetryState != TELEMETRY_STATE_LEGACY) { return false; } @@ -799,7 +799,7 @@ static bool crsfSendNativeTelemetry(void) #ifdef USE_CUSTOM_TELEMETRY static bool crsfSendCustomTelemetry(void) { - if (crsfTelemetryState == CRSFR_TELEMETRY_STATE_CUSTOM) + if (crsfTelemetryState == TELEMETRY_STATE_CUSTOM) { size_t sensor_count = 0; sbuf_t *dst = crsfInitializeSbuf(); // prepare buffer @@ -841,7 +841,7 @@ static bool crsfSendCustomTelemetry(void) #ifdef USE_CUSTOM_TELEMETRY static bool crsfPopulateCustomTelemetry(void) { - if (crsfTelemetryState == CRSFR_TELEMETRY_STATE_POPULATE) + if (crsfTelemetryState == TELEMETRY_STATE_POPULATE) { static int slot = -10; @@ -883,7 +883,7 @@ static bool crsfPopulateCustomTelemetry(void) } } - crsfTelemetryState = CRSFR_TELEMETRY_STATE_CUSTOM; + crsfTelemetryState = TELEMETRY_STATE_CUSTOM; } return false; @@ -985,9 +985,9 @@ void initCrsfTelemetry(void) // and feature is enabled, if so, set CRSF telemetry enabled #ifdef USE_CUSTOM_TELEMETRY - crsfTelemetryState = !crsfRxIsActive() ? CRSFR_TELEMETRY_STATE_OFF : (telemetryConfig()->crsf_telemetry_mode == CRSFR_TELEMETRY_STATE_NATIVE ? CRSFR_TELEMETRY_STATE_NATIVE : CRSFR_TELEMETRY_STATE_POPULATE); + crsfTelemetryState = !crsfRxIsActive() ? TELEMETRY_STATE_OFF : (telemetryConfig()->telemetry_mode == TELEMETRY_MODE_LEGACY ? TELEMETRY_STATE_LEGACY : TELEMETRY_STATE_POPULATE); #else - crsfTelemetryState = !crsfRxIsActive() ? CRSFR_TELEMETRY_STATE_OFF : CRSFR_TELEMETRY_STATE_NATIVE; + crsfTelemetryState = !crsfRxIsActive() ? TELEMETRY_STATE_OFF : TELEMETRY_STATE_LEGACY; #endif if(crsfTelemetryState) { @@ -1004,7 +1004,7 @@ void initCrsfTelemetry(void) mspReplyPending = false; #endif #ifdef USE_CUSTOM_TELEMETRY - if (crsfTelemetryState == CRSFR_TELEMETRY_STATE_NATIVE) { + if (crsfTelemetryState == TELEMETRY_STATE_LEGACY) { initCrsfNativeSensors(); } else { initCrsfCustomSensors(); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 3df339d460e..01b50eaa092 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -287,7 +287,7 @@ static void smartPortWriteFrameInternal(const smartPortPayload_t *payload) static void initSmartPortSensors(void) { #ifdef USE_CUSTOM_TELEMETRY - if(telemetryConfig()->smartport_telemetry_mode == SMARTPORT_TELEMETRY_STATE_LEGACY) { + if(telemetryConfig()->telemetry_mode == TELEMETRY_MODE_LEGACY) { initSmartPortSensorsLegacy(); } else { telemetryScheduleInit(smartportTelemetrySensors, ARRAYLEN(smartportTelemetrySensors)); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 002376c119a..8794e9d3ec1 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -103,21 +103,13 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, #ifdef USE_TELEMETRY_CRSF .crsf_telemetry_link_rate = SETTING_CRSF_TELEMETRY_LINK_RATE_DEFAULT, .crsf_telemetry_link_ratio = SETTING_CRSF_TELEMETRY_LINK_RATIO_DEFAULT, -#ifdef USE_CUSTOM_TELEMETRY - .crsf_telemetry_mode = SETTING_CRSF_TELEMETRY_MODE_DEFAULT, -#endif //USE_CUSTOM_TELEMETRY #endif //USE_TELEMETRY_CRSF -#if defined(USE_TELEMETRY_SMARTPORT) && defined(USE_CUSTOM_TELEMETRY) -#if !defined(SETTING_SMARTPORT_TELEMETRY_MODE_DEFAULT) // SITL - .smartport_telemetry_mode = 0, -#else - .smartport_telemetry_mode = SETTING_SMARTPORT_TELEMETRY_MODE_DEFAULT, -#endif -#endif // USE_TELEMETRY_SMARTPORT USE_CUSTOM_TELEMETRY -#ifdef USE_CUSTOM_TELEMETRY +#if defined(USE_CUSTOM_TELEMETRY) + .telemetry_mode = SETTING_TELEMETRY_MODE_DEFAULT, .telemetry_sensors = { 0x0, }, // all sensors enabled by default #endif + ); void telemetryInit(void) diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index d4def39c2c6..373e6e1e914 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -55,16 +55,16 @@ typedef enum { } smartportFuelUnit_e; typedef enum { - CRSFR_TELEMETRY_STATE_OFF = 0, - CRSFR_TELEMETRY_STATE_NATIVE, - CRSFR_TELEMETRY_STATE_CUSTOM, - CRSFR_TELEMETRY_STATE_POPULATE, -} crsfTelemetryMode_e; + TELEMETRY_STATE_OFF = 0, + TELEMETRY_STATE_LEGACY, + TELEMETRY_STATE_CUSTOM, + TELEMETRY_STATE_POPULATE, +} telemetryState_e; typedef enum { - SMARTPORT_TELEMETRY_STATE_LEGACY = 0, - SMARTPORT_CRSFR_TELEMETRY_STATE_STANDARD, -} smartportTelemetryMode_e; + TELEMETRY_MODE_LEGACY, + TELEMETRY_MODE_CUSTOM, +} telemetryMode_e; typedef struct telemetryConfig_s { uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed. @@ -104,18 +104,13 @@ typedef struct telemetryConfig_s { #ifdef USE_TELEMETRY_CRSF uint16_t crsf_telemetry_link_rate; uint16_t crsf_telemetry_link_ratio; -#ifdef USE_CUSTOM_TELEMETRY - uint8_t crsf_telemetry_mode; -#endif -#endif - -#if defined(USE_TELEMETRY_SMARTPORT) && defined(USE_CUSTOM_TELEMETRY) - uint8_t smartport_telemetry_mode; #endif -#ifdef USE_CUSTOM_TELEMETRY +#if defined(USE_CUSTOM_TELEMETRY) + uint8_t telemetry_mode; uint16_t telemetry_sensors[TELEM_SENSOR_SLOT_COUNT]; #endif + } telemetryConfig_t; typedef struct {