diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 041a01c5f..7c75a05bc 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -200,6 +200,9 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_CF_SERIAL_CONFIG 54 #define MSP_SET_CF_SERIAL_CONFIG 55 +#define MSP_VOLTAGE_METER_CONFIG 56 +#define MSP_SET_VOLTAGE_METER_CONFIG 57 + // // Baseflight MSP commands (if enabled they exist in Cleanflight) // @@ -1030,10 +1033,20 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(masterConfig.boardAlignment.yawDegrees); break; - case MSP_CURRENT_METER_CONFIG: + case MSP_VOLTAGE_METER_CONFIG: headSerialReply(4); + serialize8(masterConfig.batteryConfig.vbatscale); + serialize8(masterConfig.batteryConfig.vbatmincellvoltage); + serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage); + serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage); + break; + + case MSP_CURRENT_METER_CONFIG: + headSerialReply(7); serialize16(masterConfig.batteryConfig.currentMeterScale); serialize16(masterConfig.batteryConfig.currentMeterOffset); + serialize8(0); // current meter type + serialize16(masterConfig.batteryConfig.batteryCapacity); break; case MSP_MIXER: @@ -1370,9 +1383,18 @@ static bool processInCommand(void) masterConfig.boardAlignment.yawDegrees = read16(); break; + case MSP_SET_VOLTAGE_METER_CONFIG: + masterConfig.batteryConfig.vbatscale = read8(); // actual vbatscale as intended + masterConfig.batteryConfig.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI + masterConfig.batteryConfig.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI + masterConfig.batteryConfig.vbatwarningcellvoltage = read8(); // vbatlevel when buzzer starts to alert + break; + case MSP_SET_CURRENT_METER_CONFIG: masterConfig.batteryConfig.currentMeterScale = read16(); masterConfig.batteryConfig.currentMeterOffset = read16(); + read(8); // current meter type + masterConfig.batteryConfig.batteryCapacity = read16(); break; #ifndef USE_QUAD_MIXER_ONLY