Activated SW2 on RC

Src/util.c

@@ -1368,14 +1368,20 @@ void sideboardSensors(uint8_t sensors) {
     // Override in case the Sideboard control is Active
     #if defined(DUAL_INPUTS) && defined(SIDEBOARD_SERIAL_USART2)
     if (inIdx == SIDEBOARD_SERIAL_USART2) {
+      sensor1_index = 3 + ((Sideboard_L.sensors & SW2_SET) >> 9); // SW2 on RC transmitter is used to change Control Type
+      if (sensor1_index == 3) {                                   // FOC control Type
         sensor1_index = (Sideboard_L.sensors & SW3_SET) >> 11;    // SW3 on RC transmitter is used to change Control Mode
-      sensor1_trig  = sensor1_index != sensor1_prev;            // rising or falling edge detection
+      }
+      sensor1_trig  = sensor1_index != sensor1_prev;              // rising or falling edge change detection
       sensor1_prev  = sensor1_index;
     }
     #endif
     #if defined(DUAL_INPUTS) && defined(SIDEBOARD_SERIAL_USART3)
     if (inIdx == SIDEBOARD_SERIAL_USART3) {
+      sensor1_index = 3 + ((Sideboard_R.sensors & SW2_SET) >> 9); // SW2 on RC transmitter is used to change Control Type
+      if (sensor1_index == 3) {                                   // FOC control Type
         sensor1_index = (Sideboard_R.sensors & SW3_SET) >> 11;    // SW3 on RC transmitter is used to change Control Mode
+      }
       sensor1_trig  = sensor1_index != sensor1_prev;              // rising or falling edge change detection
       sensor1_prev  = sensor1_index;
     }
@@ -1385,23 +1391,22 @@ void sideboardSensors(uint8_t sensors) {
     if (sensor1_trig) {
       switch (sensor1_index) {
         case 0:     // FOC VOLTAGE
-          rtP_Left.z_ctrlTypSel  = FOC_CTRL;
-          rtP_Right.z_ctrlTypSel = FOC_CTRL;
+          rtP_Left.z_ctrlTypSel = rtP_Right.z_ctrlTypSel = FOC_CTRL;
           ctrlModReqRaw         = VLT_MODE;
           break;
         case 1:     // FOC SPEED
+          rtP_Left.z_ctrlTypSel = rtP_Right.z_ctrlTypSel = FOC_CTRL;
           ctrlModReqRaw         = SPD_MODE;
           break;
         case 2:     // FOC TORQUE
+          rtP_Left.z_ctrlTypSel = rtP_Right.z_ctrlTypSel = FOC_CTRL;
           ctrlModReqRaw         = TRQ_MODE;
           break;
         case 3:     // SINUSOIDAL
-          rtP_Left.z_ctrlTypSel  = SIN_CTRL;
-          rtP_Right.z_ctrlTypSel = SIN_CTRL;
+          rtP_Left.z_ctrlTypSel = rtP_Right.z_ctrlTypSel = SIN_CTRL;
           break;
         case 4:     // COMMUTATION
-          rtP_Left.z_ctrlTypSel  = COM_CTRL;
-          rtP_Right.z_ctrlTypSel = COM_CTRL;
+          rtP_Left.z_ctrlTypSel = rtP_Right.z_ctrlTypSel = COM_CTRL;
           break;
       }
       beepShortMany(sensor1_index + 1, 1);