Refactor telemetry sensors for SmartPort and Crsf

* add new scheduler for telemetry sensors
* used new frsky sensors Not used temp sensor for roll etc... anymore
* add custome telemetry sensors for CRSF

Author: shota3527

docs/Settings.md

@@ -592,6 +592,36 @@ Control rate profile to switch to when the battery profile is selected, 0 to dis
 
 ---
 
+### 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 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,26 +1222,6 @@ The tilt angle of the FPV camera in degrees, used by the FPV ANGLE MIX mode
 
 ---
 
-### frsky_pitch_roll
-
-S.Port telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data
-
-| Default | Min | Max |
-| --- | --- | --- |
-| OFF | OFF | ON |
-
----
-
-### 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`.

src/main/CMakeLists.txt

@@ -626,6 +626,8 @@ main_sources(COMMON_SRC
     telemetry/sim.h
     telemetry/telemetry.c
     telemetry/telemetry.h
+    telemetry/sensors.c
+    telemetry/sensors.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/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/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: crsf_telemetry_modes
+    values: [ "OFF", "NATIVE", "CUSTOM" ]
+    enum: crsfTelemetryMode_e
   - name: smartport_fuel_unit
     values: ["PERCENT", "MAH", "MWH"]
     enum: smartportFuelUnit_e
@@ -3113,14 +3116,24 @@ groups:
         description: "Determines if the telemetry protocol default signal inversion is reversed. This should be OFF in most cases unless a custom or hacked RX is used."
         default_value: OFF
         type: bool
-      - name: frsky_pitch_roll
-        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 telemetry sensors for CRSF"
+        default_value: NATIVE
+        condition: USE_TELEMETRY_LTM
+        table: crsf_telemetry_modes
+        type: uint8_t
+      - name: crsf_telemetry_link_rate
+        description: "CRSF telemetry link rate"
+        default_value: 250
+        type: uint16_t
+        min: 0
+        max: 50000
+      - name: crsf_telemetry_link_ratio
+        description: "CRSF telemetry link ratio"
+        default_value: 8
+        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: