Merge pull request #6625 from iNavFlight/dzikuvx-return-pid-value-via…

…-msp Add ability to read Programming PID status via MSP
iNavFlight · Feb 21, 2021 · ea756aa · ea756aa
2 parents 15321e9 + 67572fe
commit ea756aa
Show file tree

Hide file tree

Showing 4 changed files with 8 additions and 3 deletions.
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
@@ -567,6 +567,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             sbufWriteU16(dst, programmingPids(i)->gains.FF);
         }
         break;
+    case MSP2_INAV_PROGRAMMING_PID_STATUS:
+        for (int i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+            sbufWriteU32(dst, programmingPidGetOutput(i));
+        }
+        break;
 #endif
     case MSP2_COMMON_MOTOR_MIXER:
         for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {

diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
@@ -63,6 +63,7 @@
 #define MSP2_INAV_GVAR_STATUS                   0x2027
 #define MSP2_INAV_PROGRAMMING_PID               0x2028
 #define MSP2_INAV_SET_PROGRAMMING_PID           0x2029
+#define MSP2_INAV_PROGRAMMING_PID_STATUS        0x202A
 
 #define MSP2_PID                                0x2030
 #define MSP2_SET_PID                            0x2031
@@ -79,4 +80,3 @@
 #define MSP2_INAV_SET_SAFEHOME                  0x2039
 
 #define MSP2_INAV_MISC2                         0x203A
-
diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c
@@ -110,7 +110,7 @@ void programmingPidInit(void)
     }
 }
 
-int programmingPidGetOutput(uint8_t i) {
+int32_t programmingPidGetOutput(uint8_t i) {
     return programmingPidState[constrain(i, 0, MAX_PROGRAMMING_PID_COUNT)].output;
 }
 

diff --git a/src/main/programming/pid.h b/src/main/programming/pid.h
@@ -51,4 +51,4 @@ typedef struct programmingPidState_s {
 void programmingPidUpdateTask(timeUs_t currentTimeUs);
 void programmingPidInit(void);
 void programmingPidReset(void);
-int programmingPidGetOutput(uint8_t i);
+int32_t programmingPidGetOutput(uint8_t i);