Merge pull request #10388 from jeffhendrix/new_msp

Add MSP command to get ESC telemetry
iNavFlight · Oct 18, 2024 · 115dc5c · 115dc5c
2 parents 07b8bf9 + e92af8f
commit 115dc5c
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