From 9b189865d1d192a15f201b880218331dcd13abb7 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Sat, 15 Jul 2017 08:25:39 +0200 Subject: [PATCH 1/3] MSP changes for the new Power & Battery tab in configurator --- src/main/fc/fc_msp.c | 93 +++++++++++++++++++++++++++++--------------- 1 file changed, 61 insertions(+), 32 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 666319f9c3..bb836d1cf4 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -706,7 +706,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce case MSP_VOLTAGE_METERS: // write out id and voltage meter values, once for each meter we support - for (int i = 0; i < supportedVoltageMeterCount; i++) { + for (int i = 0; i < supportedVoltageMeterCount - (12-getMotorCount()); i++) { voltageMeter_t meter; uint8_t id = (uint8_t)voltageMeterIds[i]; @@ -719,7 +719,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce case MSP_CURRENT_METERS: // write out id and current meter values, once for each meter we support - for (int i = 0; i < supportedCurrentMeterCount; i++) { + for (int i = 0; i < supportedCurrentMeterCount - (12-getMotorCount()); i++) { currentMeter_t meter; uint8_t id = (uint8_t)currentMeterIds[i]; @@ -2131,46 +2131,75 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #endif case MSP_SET_VOLTAGE_METER_CONFIG: { - int id = sbufReadU8(src); + int8_t config_count = sbufReadU8(src); - // - // find and configure an ADC voltage sensor - // - int voltageSensorADCIndex; - for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) { - if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) { - break; + while (config_count > 0) { + + int8_t subframe_length = sbufReadU8(src); + if (subframe_length > 4) { + for (int8_t j = 0; j < subframe_length; j++) { + sbufReadU8(src); + } + } else { + int8_t id = sbufReadU8(src); + + // + // find and configure an ADC voltage sensor + // + int8_t voltageSensorADCIndex; + for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) { + if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) { + break; + } + } + + if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) { + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src); + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src); + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src); + } else { + // if we had any other types of voltage sensor to configure, this is where we'd do it. + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); + } } - } - - if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) { - voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src); - voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src); - voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src); - } else { - // if we had any other types of voltage sensor to configure, this is where we'd do it. - return -1; + config_count--; } break; } case MSP_SET_CURRENT_METER_CONFIG: { - int id = sbufReadU8(src); + int8_t config_count = sbufReadU8(src); - switch (id) { - case CURRENT_METER_ID_BATTERY_1: - currentSensorADCConfigMutable()->scale = sbufReadU16(src); - currentSensorADCConfigMutable()->offset = sbufReadU16(src); - break; + while (config_count > 0) { + + int8_t subframe_length = sbufReadU8(src); + if (subframe_length > 5) { + for (int8_t j = 0; j < subframe_length; j++) { + sbufReadU8(src); + } + } else { + int id = sbufReadU8(src); + + switch (id) { + case CURRENT_METER_ID_BATTERY_1: + currentSensorADCConfigMutable()->scale = sbufReadU16(src); + currentSensorADCConfigMutable()->offset = sbufReadU16(src); + break; #ifdef USE_VIRTUAL_CURRENT_METER - case CURRENT_METER_ID_VIRTUAL_1: - currentSensorVirtualConfigMutable()->scale = sbufReadU16(src); - currentSensorVirtualConfigMutable()->offset = sbufReadU16(src); - break; + case CURRENT_METER_ID_VIRTUAL_1: + currentSensorVirtualConfigMutable()->scale = sbufReadU16(src); + currentSensorVirtualConfigMutable()->offset = sbufReadU16(src); + break; #endif - - default: - return -1; + default: + sbufReadU16(src); + sbufReadU16(src); + break; + } + } + config_count--; } break; From f61bb3aac8759b6120585017f5a9e8b3955bb274 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Sun, 16 Jul 2017 21:17:21 +0200 Subject: [PATCH 2/3] Replaced magic number with define VOLTAGE_METER_ID_ESC_COUNT --- src/main/fc/fc_msp.c | 4 ++-- src/main/sensors/voltage.h | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index bb836d1cf4..5b2c0facb1 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -706,7 +706,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce case MSP_VOLTAGE_METERS: // write out id and voltage meter values, once for each meter we support - for (int i = 0; i < supportedVoltageMeterCount - (12-getMotorCount()); i++) { + for (int i = 0; i < supportedVoltageMeterCount - (VOLTAGE_METER_ID_ESC_COUNT - getMotorCount()); i++) { voltageMeter_t meter; uint8_t id = (uint8_t)voltageMeterIds[i]; @@ -719,7 +719,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce case MSP_CURRENT_METERS: // write out id and current meter values, once for each meter we support - for (int i = 0; i < supportedCurrentMeterCount - (12-getMotorCount()); i++) { + for (int i = 0; i < supportedCurrentMeterCount - (VOLTAGE_METER_ID_ESC_COUNT - getMotorCount()); i++) { currentMeter_t meter; uint8_t id = (uint8_t)currentMeterIds[i]; diff --git a/src/main/sensors/voltage.h b/src/main/sensors/voltage.h index 4011e67141..0c8d3d6823 100644 --- a/src/main/sensors/voltage.h +++ b/src/main/sensors/voltage.h @@ -60,6 +60,8 @@ typedef enum { #define MAX_VOLTAGE_SENSOR_ADC 1 // VBAT - some boards have external, 12V, 9V and 5V meters. #endif +#define VOLTAGE_METER_ID_ESC_COUNT 12 + typedef enum { VOLTAGE_SENSOR_ADC_VBAT = 0, VOLTAGE_SENSOR_ADC_12V = 1, From 3c917794bffdeaf2cb2151ee2c077479a928bb09 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Tue, 18 Jul 2017 22:20:52 +0200 Subject: [PATCH 3/3] Changes according review comments & fixed linker error SPRACINGF3OSD --- src/main/fc/fc_msp.c | 112 +++++++++++++++++++------------------------ 1 file changed, 48 insertions(+), 64 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 5b2c0facb1..7799082fb7 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -704,9 +704,14 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce break; } - case MSP_VOLTAGE_METERS: + case MSP_VOLTAGE_METERS: { // write out id and voltage meter values, once for each meter we support - for (int i = 0; i < supportedVoltageMeterCount - (VOLTAGE_METER_ID_ESC_COUNT - getMotorCount()); i++) { + uint8_t count = supportedVoltageMeterCount; +#ifndef USE_OSD_SLAVE + count = supportedVoltageMeterCount - (VOLTAGE_METER_ID_ESC_COUNT - getMotorCount()); +#endif + + for (int i = 0; i < count; i++) { voltageMeter_t meter; uint8_t id = (uint8_t)voltageMeterIds[i]; @@ -716,10 +721,15 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce sbufWriteU8(dst, (uint8_t)constrain(meter.filtered, 0, 255)); } break; + } - case MSP_CURRENT_METERS: + case MSP_CURRENT_METERS: { // write out id and current meter values, once for each meter we support - for (int i = 0; i < supportedCurrentMeterCount - (VOLTAGE_METER_ID_ESC_COUNT - getMotorCount()); i++) { + uint8_t count = supportedVoltageMeterCount; +#ifndef USE_OSD_SLAVE + count = supportedVoltageMeterCount - (VOLTAGE_METER_ID_ESC_COUNT - getMotorCount()); +#endif + for (int i = 0; i < count; i++) { currentMeter_t meter; uint8_t id = (uint8_t)currentMeterIds[i]; @@ -730,6 +740,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce sbufWriteU16(dst, (uint16_t)constrain(meter.amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps (mA). Negative range is truncated to zero } break; + } case MSP_VOLTAGE_METER_CONFIG: // by using a sensor type and a sub-frame length it's possible to configure any type of voltage meter, @@ -2131,77 +2142,50 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #endif case MSP_SET_VOLTAGE_METER_CONFIG: { - int8_t config_count = sbufReadU8(src); + int8_t id = sbufReadU8(src); - while (config_count > 0) { - - int8_t subframe_length = sbufReadU8(src); - if (subframe_length > 4) { - for (int8_t j = 0; j < subframe_length; j++) { - sbufReadU8(src); - } - } else { - int8_t id = sbufReadU8(src); - - // - // find and configure an ADC voltage sensor - // - int8_t voltageSensorADCIndex; - for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) { - if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) { - break; - } - } - - if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) { - voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src); - voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src); - voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src); - } else { - // if we had any other types of voltage sensor to configure, this is where we'd do it. - sbufReadU8(src); - sbufReadU8(src); - sbufReadU8(src); - } + // + // find and configure an ADC voltage sensor + // + int8_t voltageSensorADCIndex; + for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) { + if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) { + break; } - config_count--; + } + + if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) { + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src); + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src); + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src); + } else { + // if we had any other types of voltage sensor to configure, this is where we'd do it. + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); } break; } case MSP_SET_CURRENT_METER_CONFIG: { - int8_t config_count = sbufReadU8(src); + int id = sbufReadU8(src); - while (config_count > 0) { - - int8_t subframe_length = sbufReadU8(src); - if (subframe_length > 5) { - for (int8_t j = 0; j < subframe_length; j++) { - sbufReadU8(src); - } - } else { - int id = sbufReadU8(src); - - switch (id) { - case CURRENT_METER_ID_BATTERY_1: - currentSensorADCConfigMutable()->scale = sbufReadU16(src); - currentSensorADCConfigMutable()->offset = sbufReadU16(src); - break; + switch (id) { + case CURRENT_METER_ID_BATTERY_1: + currentSensorADCConfigMutable()->scale = sbufReadU16(src); + currentSensorADCConfigMutable()->offset = sbufReadU16(src); + break; #ifdef USE_VIRTUAL_CURRENT_METER - case CURRENT_METER_ID_VIRTUAL_1: - currentSensorVirtualConfigMutable()->scale = sbufReadU16(src); - currentSensorVirtualConfigMutable()->offset = sbufReadU16(src); - break; + case CURRENT_METER_ID_VIRTUAL_1: + currentSensorVirtualConfigMutable()->scale = sbufReadU16(src); + currentSensorVirtualConfigMutable()->offset = sbufReadU16(src); + break; #endif - default: - sbufReadU16(src); - sbufReadU16(src); - break; - } - } - config_count--; + default: + sbufReadU16(src); + sbufReadU16(src); + break; } - break; }