add FETtec ESC passthrough to the existing serial passthrough cli fun…

…ction
betaflight · Jun 28, 2019 · 85f00d4 · 85f00d4
1 parent 3695b12
commit 85f00d4
Showing 1 changed file with 33 additions and 2 deletions.
diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c
@@ -1264,18 +1264,25 @@ static void cliSerialPassthrough(char *cmdline)
         return;
     }
 
-    serialPassthroughPort_t ports[2] = { {SERIAL_PORT_NONE, 0, 0, NULL}, { SERIAL_PORT_USB_VCP, 0, 0, cliPort} };
+    serialPassthroughPort_t ports[2] = { {SERIAL_PORT_NONE, 0, 0, NULL}, {cliPort->identifier, 0, 0, cliPort} };
     bool enableBaudCb = false;
     int port1PinioDtr = 0;
     bool port1ResetOnDtr = false;
+    bool escSensorPassthrough = false;
     char *saveptr;
     char* tok = strtok_r(cmdline, " ", &saveptr);
     int index = 0;
 
     while (tok != NULL) {
         switch (index) {
         case 0:
-            ports[0].id = atoi(tok);
+            if (strcasestr(tok, "esc_sensor")) {
+                escSensorPassthrough = true;
+                const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_ESC_SENSOR);
+                ports[0].id = portConfig->identifier;
+            } else {
+                ports[0].id = atoi(tok);
+            }
             break;
         case 1:
             ports[0].baud = atoi(tok);
@@ -1415,6 +1422,30 @@ static void cliSerialPassthrough(char *cmdline)
         serialSetCtrlLineStateCb(ports[0].port, cbCtrlLine, (void *)(intptr_t)(port1PinioDtr));
     }
 
+#ifdef USE_PWM_OUTPUT
+    if (escSensorPassthrough) {
+        pwmDisableMotors();
+        delay(5);
+        unsigned motorsCount = getMotorCount();
+        for (unsigned i = 0; i < motorsCount; i++) {
+            const ioTag_t tag = motorConfig()->dev.ioTags[i];
+            if (tag) {
+                const timerHardware_t *timerHardware = timerGetByTag(tag);
+                if (timerHardware) {
+                    IO_t io = IOGetByTag(tag);
+                    IOInit(io, OWNER_MOTOR, 0);
+                    IOConfigGPIO(io, IOCFG_OUT_PP);
+                    if (timerHardware->output & TIMER_OUTPUT_INVERTED) {
+                        IOLo(io);
+                    } else {
+                        IOHi(io);
+                    }
+                }
+            }
+        }
+    }
+#endif
+
     serialPassthrough(ports[0].port, ports[1].port, NULL, NULL);
 }
 #endif