From 571a7cfbddd16e2fe3fd88536000e66c75146e87 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Tue, 30 Apr 2024 13:02:09 -0500 Subject: [PATCH] MSP 1.54 (#991) * MSP 1.54 * MSP 1.54 - move sampleRate location * MSP 1.54 - cleanup * MSP 1.54 support - add vtx_low_power_disarm --- src/main/drivers/accgyro/gyro_sync.c | 5 ++ src/main/interface/msp.c | 71 ++++++++++++++++++---------- src/main/interface/msp_protocol.h | 2 +- src/main/sensors/gyro.h | 4 ++ 4 files changed, 55 insertions(+), 27 deletions(-) diff --git a/src/main/drivers/accgyro/gyro_sync.c b/src/main/drivers/accgyro/gyro_sync.c index 3653c24d43..91ddc1cec6 100644 --- a/src/main/drivers/accgyro/gyro_sync.c +++ b/src/main/drivers/accgyro/gyro_sync.c @@ -53,21 +53,26 @@ uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenomin } gyro->mpuDividerDrops = gyroSyncDenominator - 1; gyro->gyroRateKHz = lpfNoneOr256 ? GYRO_RATE_8_kHz : GYRO_RATE_1_kHz; + gyroConfigMutable()->gyroSampleRateHz = lpfNoneOr256 ? 8000 : 1000; switch (gyro->mpuDetectionResult.sensor) { case ICM_20649_SPI: //20649 is a weird gyro gyro->gyroRateKHz = lpfNoneOr256 ? GYRO_RATE_9_kHz : GYRO_RATE_1100_Hz; + gyroConfigMutable()->gyroSampleRateHz = lpfNoneOr256 ? 9000 : 1100; break; case BMI_160_SPI: //brainFPV is also a weird gyro if (lpfNoneOr256) { gyro->gyroRateKHz = GYRO_RATE_3200_Hz; } + gyroConfigMutable()->gyroSampleRateHz = 3200; break; case BMI_270_SPI: //bmi270 gyro->gyroRateKHz = GYRO_RATE_3200_Hz; + gyroConfigMutable()->gyroSampleRateHz = 3200; break; default: if (gyro_use_32khz) { //use full 32k gyro->gyroRateKHz = GYRO_RATE_32_kHz; + gyroConfigMutable()->gyroSampleRateHz = 32000; } } diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index bdffb85b36..f81d0c71f4 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -458,6 +458,9 @@ bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr // Signature sbufWriteData(dst, getSignature(), SIGNATURE_LENGTH); #endif + //MSP 1.54 + sbufWriteU16(dst, gyroConfig()->gyroSampleRateHz); //For Configurator PID/Gyro loop selection. + //End MSP 1.54 #endif // USE_BOARD_INFO break; } @@ -1169,6 +1172,9 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) { sbufWriteU16(dst, motorConfig()->dev.motorPwmRate); sbufWriteU16(dst, motorConfig()->digitalIdleOffsetValue); sbufWriteU8(dst, gyroConfig()->gyro_use_32khz); + //MSP 1.54 - insert here to avoid new unnecessary Configurator code + sbufWriteU8(dst, motorConfig()->motorPoleCount); + //End MSP 1.54 sbufWriteU8(dst, motorConfig()->dev.motorPwmInversion); sbufWriteU8(dst, gyroConfig()->gyro_to_use); sbufWriteU8(dst, gyroConfig()->gyro_high_fsr); @@ -1358,6 +1364,9 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) { sbufWriteU8(dst, vtxSettingsConfig()->power); sbufWriteU8(dst, pitmode); sbufWriteU16(dst, vtxSettingsConfig()->freq); + //MSP 1.54 + sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm); + //END MSP 1.54 // future extensions here... } else { sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX detected @@ -1792,6 +1801,11 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) { if (sbufBytesRemaining(src)) { gyroConfigMutable()->gyro_use_32khz = sbufReadU8(src); } + //MSP 1.54 - insert here to avoid new unnecessary Configurator code + if (sbufBytesRemaining(src) >= 1) { + motorConfigMutable()->motorPoleCount = sbufReadU8(src); + } + //End MSP 1.54 if (sbufBytesRemaining(src)) { motorConfigMutable()->dev.motorPwmInversion = sbufReadU8(src); } @@ -2016,32 +2030,37 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) { #endif #ifdef USE_VTX_COMMON case MSP_SET_VTX_CONFIG: { - vtxDevice_t *vtxDevice = vtxCommonDevice(); - if (vtxDevice) { - if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) { - uint16_t newFrequency = sbufReadU16(src); - if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { //value is band and channel - const uint8_t newBand = (newFrequency / 8) + 1; - const uint8_t newChannel = (newFrequency % 8) + 1; - vtxSettingsConfigMutable()->band = newBand; - vtxSettingsConfigMutable()->channel = newChannel; - vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(newBand, newChannel); - } else { //value is frequency in MHz - vtxSettingsConfigMutable()->band = 0; - vtxSettingsConfigMutable()->freq = newFrequency; - } - if (sbufBytesRemaining(src) > 1) { - vtxSettingsConfigMutable()->power = sbufReadU8(src); - // Delegate pitmode to vtx directly - const uint8_t newPitmode = sbufReadU8(src); - uint8_t currentPitmode = 0; - vtxCommonGetPitMode(vtxDevice, ¤tPitmode); - if (currentPitmode != newPitmode) { - vtxCommonSetPitMode(vtxDevice, newPitmode); - } - } - } - } + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice) { + if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) { + uint16_t newFrequency = sbufReadU16(src); + if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { //value is band and channel + const uint8_t newBand = (newFrequency / 8) + 1; + const uint8_t newChannel = (newFrequency % 8) + 1; + vtxSettingsConfigMutable()->band = newBand; + vtxSettingsConfigMutable()->channel = newChannel; + vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(newBand, newChannel); + } else { //value is frequency in MHz + vtxSettingsConfigMutable()->band = 0; + vtxSettingsConfigMutable()->freq = newFrequency; + } + if (sbufBytesRemaining(src) > 1) { + vtxSettingsConfigMutable()->power = sbufReadU8(src); + // Delegate pitmode to vtx directly + const uint8_t newPitmode = sbufReadU8(src); + uint8_t currentPitmode = 0; + vtxCommonGetPitMode(vtxDevice, ¤tPitmode); + if (currentPitmode != newPitmode) { + vtxCommonSetPitMode(vtxDevice, newPitmode); + } + } + // MSP 1.54 + if (sbufBytesRemaining(src) >= 1) { + vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src); + } + // END MSP 1.54 + } + } } break; #endif diff --git a/src/main/interface/msp_protocol.h b/src/main/interface/msp_protocol.h index 8716c30f0c..2d37b51717 100644 --- a/src/main/interface/msp_protocol.h +++ b/src/main/interface/msp_protocol.h @@ -64,7 +64,7 @@ // API VERSION #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 53 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release) +#define API_VERSION_MINOR 54 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release) #define API_VERSION_LENGTH 2 diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 671d7b7cbd..e333d77410 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -175,6 +175,10 @@ typedef struct gyroConfig_s { uint8_t smithPredictorStrength; uint8_t smithPredictorDelay; uint8_t smithPredictorFilterHz; + + //MSP 1.54 + uint16_t gyroSampleRateHz; + //End MSP 1.54 } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig);