From 8f5ac3e51f8cb239d77efc9abde9c4a3d2f83773 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 21 Feb 2021 10:38:39 +0100 Subject: [PATCH 1/2] Refactor PID value getter --- src/main/programming/pid.c | 2 +- src/main/programming/pid.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c index 09916fbb1b3..8b284087ebd 100644 --- 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 index 8645de8f130..0d55b952c67 100644 --- 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); \ No newline at end of file +int32_t programmingPidGetOutput(uint8_t i); \ No newline at end of file From 67572fed9d776eff58f781d4bedd29f8f3aab8d5 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 21 Feb 2021 11:20:07 +0100 Subject: [PATCH 2/2] Add ability to read Programming PID status via MSP --- src/main/fc/fc_msp.c | 5 +++++ src/main/msp/msp_protocol_v2_inav.h | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index a32961dabae..0e47385044d 100644 --- 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 index 70e394ccb3f..d3e7d14585a 100755 --- 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 -