fix legacy sensors

Author: error414

src/main/fc/settings.yaml

@@ -3119,6 +3119,10 @@ 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: crsf_telemetry_mode
         description: "Use extended custom or native telemetry sensors for CRSF"
         default_value: NATIVE

src/main/telemetry/crsf.c

@@ -974,7 +974,9 @@ static void initCrsfNativeSensors(void)
     telemetryScheduleInit(crsfNativeTelemetrySensors, ARRAYLEN(crsfNativeTelemetrySensors));
 
     for(size_t i = 0; i < ARRAYLEN(crsfNativeTelemetrySensors); i++) {
-        telemetryScheduleAdd(&crsfNativeTelemetrySensors[i]);
+        if(telemetrySensorAllowed(crsfNativeTelemetrySensors[i].sensor_id)) {
+            telemetryScheduleAdd(&crsfNativeTelemetrySensors[i]);
+        }
     }
 }
 

src/main/telemetry/sensors.c

@@ -385,7 +385,7 @@ bool telemetrySensorAllowed(sensor_id_e id)
         case TELEM_LEGACY_HEADING:
             return true;
         case TELEM_LEGACY_PITCH:
-            return telemetryConfig()->frsky_pitch_roll;
+            FALLTHROUGH;
         case TELEM_LEGACY_ROLL:
             return telemetryConfig()->frsky_pitch_roll;
         case TELEM_LEGACY_ACCX:

src/main/telemetry/smartport_legacy.c

@@ -388,7 +388,7 @@ void initSmartPortSensorsLegacy(void) {
     telemetryScheduleInit(smartportTelemetrySensorsLegacy, ARRAYLEN(smartportTelemetrySensorsLegacy));
 
     for(size_t i = 0; i < ARRAYLEN(smartportTelemetrySensorsLegacy); i++) {
-        if(telemetrySensorAllowed(smartportTelemetrySensorsLegacy[i].index)) {
+        if(telemetrySensorAllowed(smartportTelemetrySensorsLegacy[i].sensor_id)) {
             telemetryScheduleAdd(&smartportTelemetrySensorsLegacy[i]);
         }
     }

src/main/telemetry/telemetry.c

@@ -63,6 +63,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY
 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,
     .report_cell_voltage = SETTING_REPORT_CELL_VOLTAGE_DEFAULT,
     .hottAlarmSoundInterval = SETTING_HOTT_ALARM_SOUND_INTERVAL_DEFAULT,
     .halfDuplex = SETTING_TELEMETRY_HALFDUPLEX_DEFAULT,