Refactor CRSF and SmartPort telemetry

docs/Settings.md

@@ -582,6 +582,26 @@ 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 |
+
+---
+
 ### cruise_power
 
 Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit
@@ -1192,16 +1212,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`.
@@ -6352,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.

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)

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;
 }

src/main/common/crc.h

@@ -19,6 +19,8 @@
 
 #include <stdint.h>
 
+#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);
+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);
+

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", "<id> [baud] [mode] [options]: passthrough to serial", cliSerialPassthrough),
 #endif

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()) {

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

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);

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)

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;

src/main/fc/settings.yaml

@@ -103,6 +103,9 @@ tables:
   - name: bat_voltage_source
     values: ["RAW", "SAG_COMP"]
     enum: batVoltageSource_e
+  - name: telemetry_modes
+    values: [ "LEGACY", "CUSTOM" ]
+    enum: telemetryMode_e
   - name: smartport_fuel_unit
     values: ["PERCENT", "MAH", "MWH"]
     enum: smartportFuelUnit_e
@@ -3117,10 +3120,26 @@ 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: telemetry_mode
+        description: "Use extended custom or native telemetry sensors for CRSF or SmartPort"
+        default_value: LEGACY
+        condition: USE_CUSTOM_TELEMETRY
+        table: 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

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:

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:

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
 

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