diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 342e028efc..d1aac0f490 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -101,6 +101,7 @@ #include "pg/beeper.h" #include "pg/board.h" +#include "pg/gyrodev.h" #include "pg/pg.h" #include "pg/pg_ids.h" #include "pg/rx.h" @@ -1279,11 +1280,41 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle); break; - case MSP_SENSOR_ALIGNMENT: - sbufWriteU8(dst, 0); // gyro_align - sbufWriteU8(dst, 0); // acc_align + case MSP_SENSOR_ALIGNMENT: { + uint8_t gyroAlignment; +#ifdef USE_MULTI_GYRO + switch (gyroConfig()->gyro_to_use) { + case GYRO_CONFIG_USE_GYRO_2: + gyroAlignment = gyroDeviceConfig(1)->align; + break; + case GYRO_CONFIG_USE_GYRO_BOTH: + // for dual-gyro in "BOTH" mode we only read/write gyro 0 + default: + gyroAlignment = gyroDeviceConfig(0)->align; + break; + } +#else + gyroAlignment = gyroDeviceConfig(0)->align; +#endif + sbufWriteU8(dst, gyroAlignment); + sbufWriteU8(dst, gyroAlignment); // Starting with 4.0 gyro and acc alignment are the same sbufWriteU8(dst, compassConfig()->mag_align); + + // API 1.41 - Add multi-gyro indicator, selected gyro, and support for separate gyro 1 & 2 alignment +#ifdef USE_MULTI_GYRO + sbufWriteU8(dst, 1); // USE_MULTI_GYRO + sbufWriteU8(dst, gyroConfig()->gyro_to_use); + sbufWriteU8(dst, gyroDeviceConfig(0)->align); + sbufWriteU8(dst, gyroDeviceConfig(1)->align); +#else + sbufWriteU8(dst, 0); + sbufWriteU8(dst, GYRO_CONFIG_USE_GYRO_1); + sbufWriteU8(dst, gyroDeviceConfig(0)->align); + sbufWriteU8(dst, ALIGN_DEFAULT); +#endif + break; + } case MSP_ADVANCED_CONFIG: sbufWriteU8(dst, gyroConfig()->gyro_sync_denom); @@ -1837,11 +1868,43 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_RESET_CURR_PID: resetPidProfile(currentPidProfile); break; - case MSP_SET_SENSOR_ALIGNMENT: - sbufReadU8(src); // gyro_align - sbufReadU8(src); // acc_align + case MSP_SET_SENSOR_ALIGNMENT: { + // maintain backwards compatibility for API < 1.41 + const uint8_t gyroAlignment = sbufReadU8(src); + sbufReadU8(src); // discard deprecated acc_align compassConfigMutable()->mag_align = sbufReadU8(src); + + if (sbufBytesRemaining(src) >= 3) { + // API >= 1.41 - support the gyro_to_use and alignment for gyros 1 & 2 +#ifdef USE_MULTI_GYRO + gyroConfigMutable()->gyro_to_use = sbufReadU8(src); + gyroDeviceConfigMutable(0)->align = sbufReadU8(src); + gyroDeviceConfigMutable(1)->align = sbufReadU8(src); +#else + sbufReadU8(src); // unused gyro_to_use + gyroDeviceConfigMutable(0)->align = sbufReadU8(src); + sbufReadU8(src); // unused gyro_2_sensor_align +#endif + } else { + // maintain backwards compatibility for API < 1.41 +#ifdef USE_MULTI_GYRO + switch (gyroConfig()->gyro_to_use) { + case GYRO_CONFIG_USE_GYRO_2: + gyroDeviceConfigMutable(1)->align = gyroAlignment; + break; + case GYRO_CONFIG_USE_GYRO_BOTH: + // For dual-gyro in "BOTH" mode we'll only update gyro 0 + default: + gyroDeviceConfigMutable(0)->align = gyroAlignment; + break; + } +#else + gyroDeviceConfigMutable(0)->align = gyroAlignment; +#endif + + } break; + } case MSP_SET_ADVANCED_CONFIG: gyroConfigMutable()->gyro_sync_denom = sbufReadU8(src);