Add MSP command to get ESC telemetry

iNavFlight · Sep 27, 2024 · e92af8f · e92af8f
1 parent 5260d88
commit e92af8f
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 0 deletions.
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
@@ -1674,6 +1674,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             }
         }
         break;
+
+    case MSP2_INAV_ESC_TELEM:
+        {
+            uint8_t motorCount = getMotorCount();
+            sbufWriteU8(dst, motorCount);
+
+            for (uint8_t i = 0; i < motorCount; i++){
+                const escSensorData_t *escState = getEscTelemetry(i); //Get ESC telemetry
+                sbufWriteDataSafe(dst, escState, sizeof(escSensorData_t));
+            }
+        }
+        break;
 #endif
 
 #ifdef USE_EZ_TUNE

diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
@@ -89,6 +89,7 @@
 #define MSP2_INAV_LOGIC_CONDITIONS_SINGLE       0x203B
 
 #define MSP2_INAV_ESC_RPM                       0x2040
+#define MSP2_INAV_ESC_TELEM                     0x2041
 
 #define MSP2_INAV_LED_STRIP_CONFIG_EX           0x2048
 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX       0x2049