From c29208051e87cc708a517c9e4d75508d881cfe58 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 29 Mar 2024 19:34:50 +0100 Subject: [PATCH] MSP2_INAV_SERVO_CONFIG frame --- src/main/fc/fc_msp.c | 16 +++++----------- src/main/msp/msp_protocol_v2_inav.h | 2 ++ 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index d23537a75b..4097617f89 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -492,18 +492,16 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP_SERVO: sbufWriteData(dst, &servo, MAX_SUPPORTED_SERVOS * 2); break; - case MSP_SERVO_CONFIGURATIONS: + + case MSP2_INAV_SERVO_CONFIG: for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { sbufWriteU16(dst, servoParams(i)->min); sbufWriteU16(dst, servoParams(i)->max); sbufWriteU16(dst, servoParams(i)->middle); sbufWriteU8(dst, servoParams(i)->rate); - sbufWriteU8(dst, 0); - sbufWriteU8(dst, 0); - sbufWriteU8(dst, 255); // used to be forwardFromChannel, not used anymore, send 0xff for compatibility reasons - sbufWriteU32(dst, 0); //Input reversing is not required since it can be done on mixer level } break; + case MSP2_INAV_SERVO_MIXER: for (int i = 0; i < MAX_SERVO_RULES; i++) { sbufWriteU8(dst, customServoMixers(i)->targetChannel); @@ -2105,8 +2103,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) return MSP_RESULT_ERROR; break; - case MSP_SET_SERVO_CONFIGURATION: - if (dataSize != (1 + 14)) { + case MSP2_INAV_SET_SERVO_CONFIG: + if (dataSize != 8) { return MSP_RESULT_ERROR; } tmp_u8 = sbufReadU8(src); @@ -2117,10 +2115,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) servoParamsMutable(tmp_u8)->max = sbufReadU16(src); servoParamsMutable(tmp_u8)->middle = sbufReadU16(src); servoParamsMutable(tmp_u8)->rate = sbufReadU8(src); - sbufReadU8(src); - sbufReadU8(src); - sbufReadU8(src); // used to be forwardFromChannel, ignored - sbufReadU32(src); // used to be reversedSources servoComputeScalingFactors(tmp_u8); } break; diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 66e883ab8c..3312e5bc25 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -108,3 +108,5 @@ #define MSP2_INAV_CUSTOM_OSD_ELEMENTS 0x2100 #define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS 0x2101 +#define MSP2_INAV_SERVO_CONFIG 0x2200 +#define MSP2_INAV_SET_SERVO_CONFIG 0x2201 \ No newline at end of file