diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 6bf04f123..feb5a7ab0 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -923,26 +923,33 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, (uint8_t)getBatteryState()); break; } -/* case MSP_VOLTAGE_METERS: - // TODO write out voltage, once for each meter id we support - for (int i = 0; i < MAX_VOLTAGE_METERS; i++) { + // write out id and voltage meter values, once for each meter we support + for (int i = 0; i < supportedVoltageMeterCount; i++) { - uint16_t voltage = getVoltageMeter(i)->vbat; - sbufWriteU8(dst, (uint8_t)constrain(voltage, 0, 255)); + voltageMeter_t meter; + uint8_t id = (uint8_t)voltageMeterIds[i]; + voltageMeterRead(id, &meter); + + sbufWriteU8(dst, id); + sbufWriteU8(dst, (uint8_t)constrain(meter.filtered, 0, 255)); } break; case MSP_CURRENT_METERS: - // TODO write out amperage meter for each meter id we support - for (int i = 0; i < MAX_AMPERAGE_METERS; i++) { - amperageMeter_t *meter = getAmperageMeter(i); - // write out amperage, once for each current meter. - sbufWriteU16(dst, (uint16_t)constrain(meter->amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero - sbufWriteU32(dst, meter->mAhDrawn); + // write out id and current meter values, once for each meter we support + for (int i = 0; i < supportedCurrentMeterCount; i++) { + + currentMeter_t meter; + uint8_t id = (uint8_t)currentMeterIds[i]; + currentMeterRead(id, &meter); + + sbufWriteU8(dst, id); + sbufWriteU16(dst, (uint16_t)constrain(meter.mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery + 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, // e.g. an i2c/spi/can sensor or any sensor not built directly into the FC such as ESC/RX/SPort/SBus that has diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 8ba3ad970..6b81591f9 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -279,12 +279,20 @@ void batteryInit(void) // current // consumptionState = BATTERY_OK; - resetCurrentMeterState(¤tMeter); + currentMeterReset(¤tMeter); switch(batteryConfig()->currentMeterSource) { case CURRENT_METER_ADC: currentMeterADCInit(); break; + case CURRENT_METER_VIRTUAL: + currentMeterVirtualInit(); + break; + + case CURRENT_METER_ESC: + currentMeterESCInit(); + break; + default: break; } @@ -309,13 +317,14 @@ static void batteryUpdateConsumptionState(void) void batteryUpdateCurrentMeter(int32_t lastUpdateAt, bool armed) { if (batteryCellCount == 0) { - resetCurrentMeterState(¤tMeter); + currentMeterReset(¤tMeter); return; } switch(batteryConfig()->currentMeterSource) { case CURRENT_METER_ADC: - currentUpdateADCMeter(¤tMeter, lastUpdateAt); + currentMeterADCRefresh(lastUpdateAt); + currentMeterADCRead(¤tMeter); break; case CURRENT_METER_VIRTUAL: { @@ -323,21 +332,23 @@ void batteryUpdateCurrentMeter(int32_t lastUpdateAt, bool armed) bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)); int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; - currentUpdateVirtualMeter(¤tMeter, lastUpdateAt, armed, throttleLowAndMotorStop, throttleOffset); + currentMeterVirtualRefresh(lastUpdateAt, armed, throttleLowAndMotorStop, throttleOffset); + currentMeterVirtualRead(¤tMeter); break; } case CURRENT_METER_ESC: #ifdef USE_ESC_SENSOR if (feature(FEATURE_ESC_SENSOR)) { - currentUpdateESCMeter(¤tMeter, lastUpdateAt); + currentMeterESCRefresh(lastUpdateAt); + currentMeterESCReadCombined(¤tMeter); } #endif break; default: -// case CURRENT_METER_NONE: -// resetCurrentMeterState(¤tMeter); + case CURRENT_METER_NONE: + currentMeterReset(¤tMeter); break; } } diff --git a/src/main/sensors/current.c b/src/main/sensors/current.c index d77303ef0..09e36c99b 100644 --- a/src/main/sensors/current.c +++ b/src/main/sensors/current.c @@ -36,6 +36,43 @@ #include "sensors/current.h" #include "sensors/esc_sensor.h" +const uint8_t currentMeterIds[] = { + CURRENT_METER_ID_VBAT_1, + CURRENT_METER_ID_VIRTUAL_1, +#ifdef USE_ESC_SENSOR + CURRENT_METER_ID_ESC_COMBINED_1, + CURRENT_METER_ID_ESC_MOTOR_1, + CURRENT_METER_ID_ESC_MOTOR_2, + CURRENT_METER_ID_ESC_MOTOR_3, + CURRENT_METER_ID_ESC_MOTOR_4, + CURRENT_METER_ID_ESC_MOTOR_5, + CURRENT_METER_ID_ESC_MOTOR_6, + CURRENT_METER_ID_ESC_MOTOR_7, + CURRENT_METER_ID_ESC_MOTOR_8, + CURRENT_METER_ID_ESC_MOTOR_9, + CURRENT_METER_ID_ESC_MOTOR_10, + CURRENT_METER_ID_ESC_MOTOR_11, + CURRENT_METER_ID_ESC_MOTOR_12, +#endif +}; + +const uint8_t supportedCurrentMeterCount = ARRAYLEN(currentMeterIds); + +// +// ADC/Virtual/ESC shared +// + +void currentMeterReset(currentMeter_t *meter) +{ + meter->amperage = 0; + meter->amperageLatest = 0; + meter->mAhDrawn = 0; +} + +// +// ADC/Virtual shared +// + #define ADCVREF 3300 // in mV #define IBAT_LPF_FREQ 0.4f @@ -75,60 +112,150 @@ static int32_t currentMeterADCToCentiamps(const uint16_t src) return (millivolts * 1000) / (int32_t)config->scale; // current in 0.01A steps } -void updateCurrentDrawn(currentMeter_t *state, int32_t lastUpdateAt) +static void updateCurrentmAhDrawnState(currentMeterMAhDrawnState_t *state, int32_t amperageLatest, int32_t lastUpdateAt) { - state->mAhDrawnF = state->mAhDrawnF + (state->amperageLatest * lastUpdateAt / (100.0f * 1000 * 3600)); + state->mAhDrawnF = state->mAhDrawnF + (amperageLatest * lastUpdateAt / (100.0f * 1000 * 3600)); state->mAhDrawn = state->mAhDrawnF; } -void currentUpdateADCMeter(currentMeter_t *state, int32_t lastUpdateAt) -{ - uint16_t iBatSample = adcGetChannel(ADC_CURRENT); - state->amperageLatest = currentMeterADCToCentiamps(iBatSample); - state->amperage = currentMeterADCToCentiamps(biquadFilterApply(&adciBatFilter, iBatSample)); +// +// ADC +// - updateCurrentDrawn(state, lastUpdateAt); +currentMeterADCState_t currentMeterADCState; + +void currentMeterADCInit(void) +{ + memset(¤tMeterADCState, 0, sizeof(currentMeterADCState_t)); + biquadFilterInitLPF(&adciBatFilter, IBAT_LPF_FREQ, 50000); //50HZ Update } -void currentUpdateVirtualMeter(currentMeter_t *state, int32_t lastUpdateAt, bool armed, bool throttleLowAndMotorStop, int32_t throttleOffset) +void currentMeterADCRefresh(int32_t lastUpdateAt) { - state->amperage = (int32_t)currentMeterADCOrVirtualConfig(CURRENT_SENSOR_VIRTUAL)->offset; + uint16_t iBatSample = adcGetChannel(ADC_CURRENT); + currentMeterADCState.amperageLatest = currentMeterADCToCentiamps(iBatSample); + currentMeterADCState.amperage = currentMeterADCToCentiamps(biquadFilterApply(&adciBatFilter, iBatSample)); + + updateCurrentmAhDrawnState(¤tMeterADCState.mahDrawnState, currentMeterADCState.amperageLatest, lastUpdateAt); +} + +void currentMeterADCRead(currentMeter_t *meter) +{ + meter->amperageLatest = currentMeterADCState.amperageLatest; + meter->amperage = currentMeterADCState.amperage; + meter->mAhDrawn = currentMeterADCState.mahDrawnState.mAhDrawn; +} + +// +// VIRTUAL +// + +currentMeterVirtualState_t currentMeterVirtualState; + +void currentMeterVirtualInit(void) +{ + memset(¤tMeterVirtualState, 0, sizeof(currentMeterVirtualState_t)); +} + +void currentMeterVirtualRefresh(int32_t lastUpdateAt, bool armed, bool throttleLowAndMotorStop, int32_t throttleOffset) +{ + currentMeterVirtualState.amperage = (int32_t)currentMeterADCOrVirtualConfig(CURRENT_SENSOR_VIRTUAL)->offset; if (armed) { if (throttleLowAndMotorStop) { throttleOffset = 0; } int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); // FIXME magic number 50, 50hz? - state->amperageLatest = state->amperage += throttleFactor * (int32_t)currentMeterADCOrVirtualConfig(CURRENT_SENSOR_VIRTUAL)->scale / 1000; + currentMeterVirtualState.amperage += throttleFactor * (int32_t)currentMeterADCOrVirtualConfig(CURRENT_SENSOR_VIRTUAL)->scale / 1000; } - updateCurrentDrawn(state, lastUpdateAt); + updateCurrentmAhDrawnState(¤tMeterVirtualState.mahDrawnState, currentMeterVirtualState.amperage, lastUpdateAt); } -void currentUpdateESCMeter(currentMeter_t *state, int32_t lastUpdateAt) +void currentMeterVirtualRead(currentMeter_t *meter) { - UNUSED(lastUpdateAt); -#ifndef USE_ESC_SENSOR - UNUSED(state); -#else - escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); - if (escData->dataAge <= ESC_BATTERY_AGE_MAX) { - state->amperageLatest = escData->current; - state->mAhDrawn = escData->consumption; - } else { - state->amperageLatest = 0; - state->mAhDrawn = 0; - } - state->amperage = state->amperageLatest; + meter->amperageLatest = currentMeterVirtualState.amperage; + meter->amperage = currentMeterVirtualState.amperage; + meter->mAhDrawn = currentMeterVirtualState.mahDrawnState.mAhDrawn; +} + +// +// ESC +// + +#ifdef USE_ESC_SENSOR +currentMeterESCState_t currentMeterESCState; +#endif + +void currentMeterESCInit(void) +{ +#ifdef USE_ESC_SENSOR + memset(¤tMeterESCState, 0, sizeof(currentMeterESCState_t)); #endif } -void resetCurrentMeterState(currentMeter_t *state) +void currentMeterESCRefresh(int32_t lastUpdateAt) { - state->amperage = 0; - state->amperageLatest = 0; + UNUSED(lastUpdateAt); +#ifdef USE_ESC_SENSOR + escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); + if (escData->dataAge <= ESC_BATTERY_AGE_MAX) { + currentMeterESCState.amperage = escData->current; + currentMeterESCState.mAhDrawn = escData->consumption; + } else { + currentMeterESCState.amperage = 0; + currentMeterESCState.mAhDrawn = 0; + } +#endif } -void currentMeterADCInit(void) +void currentMeterESCReadCombined(currentMeter_t *meter) { - biquadFilterInitLPF(&adciBatFilter, IBAT_LPF_FREQ, 50000); //50HZ Update +#ifdef USE_ESC_SENSOR + meter->amperageLatest = currentMeterESCState.amperage; + meter->amperage = currentMeterESCState.amperage; + meter->mAhDrawn = currentMeterESCState.mAhDrawn; +#else + currentMeterReset(meter); +#endif +} + +void currentMeterESCReadMotor(uint8_t motorNumber, currentMeter_t *meter) +{ +#ifndef USE_ESC_SENSOR + UNUSED(motorNumber); + currentMeterReset(meter); +#else + escSensorData_t *escData = getEscSensorData(motorNumber); + if (escData->dataAge <= ESC_BATTERY_AGE_MAX) { + meter->amperage = escData->current; + meter->amperageLatest = escData->current; + meter->mAhDrawn = escData->consumption; + return; + } +#endif +} + +// +// API for current meters using IDs +// + +void currentMeterRead(currentMeterId_e id, currentMeter_t *meter) +{ + if (id == CURRENT_METER_ID_VBAT_1) { + currentMeterADCRead(meter); + } else if (id == CURRENT_METER_ID_VIRTUAL_1) { + currentMeterVirtualRead(meter); + } +#ifdef USE_ESC_SENSOR + if (id == CURRENT_METER_ID_ESC_COMBINED_1) { + currentMeterESCReadCombined(meter); + } else + if (id >= CURRENT_METER_ID_ESC_MOTOR_1 && id <= CURRENT_METER_ID_ESC_MOTOR_20 ) { + int motor = id - CURRENT_METER_ID_ESC_MOTOR_1; + currentMeterESCReadMotor(motor, meter); + } else +#endif + { + currentMeterReset(meter); + } } diff --git a/src/main/sensors/current.h b/src/main/sensors/current.h index 7ed293087..50b887523 100644 --- a/src/main/sensors/current.h +++ b/src/main/sensors/current.h @@ -17,6 +17,7 @@ #pragma once +#include "current_ids.h" typedef enum { CURRENT_METER_NONE = 0, @@ -26,54 +27,10 @@ typedef enum { CURRENT_METER_MAX = CURRENT_METER_ESC } currentMeterSource_e; -// -// fixed ids, current can be measured at many different places, these identifiers are the ones we support or would consider supporting. -// - -typedef enum { - CURRENT_METER_ID_NONE = 0, - - CURRENT_METER_ID_VBAT_1 = 10, // 10-19 for battery meters - CURRENT_METER_ID_VBAT_2, - //.. - CURRENT_METER_ID_VBAT_10 = 19, - - CURRENT_METER_ID_5V_1 = 20, // 20-29 for 5V meters - CURRENT_METER_ID_5V_2, - //.. - CURRENT_METER_ID_5V_10 = 29, - - CURRENT_METER_ID_9V_1 = 30, // 30-39 for 9V meters - CURRENT_METER_ID_9V_2, - //.. - CURRENT_METER_ID_9V_10 = 39, - - CURRENT_METER_ID_12V_1 = 40, // 40-49 for 12V meters - CURRENT_METER_ID_12V_2, - //.. - CURRENT_METER_ID_12V_10 = 49, - - CURRENT_METER_ID_ESC_COMBINED_1 = 50, // 50-59 for ESC combined (it's doubtful an FC would ever expose 51-59 however) - // ... - CURRENT_METER_ID_ESC_COMBINED_10 = 59, - - CURRENT_METER_ID_ESC_MOTOR_1 = 60, // 60-79 for ESC motors (20 motors) - CURRENT_METER_ID_ESC_MOTOR_2, - CURRENT_METER_ID_ESC_MOTOR_3, - CURRENT_METER_ID_ESC_MOTOR_4, - CURRENT_METER_ID_ESC_MOTOR_5, - CURRENT_METER_ID_ESC_MOTOR_6, - CURRENT_METER_ID_ESC_MOTOR_7, - CURRENT_METER_ID_ESC_MOTOR_8, - //... - CURRENT_METER_ID_ESC_MOTOR_20 = 79, -} currentMeterId_e; - typedef struct currentMeter_s { int32_t amperage; // current read by current sensor in centiampere (1/100th A) int32_t amperageLatest; // current read by current sensor in centiampere (1/100th A) (unfiltered) int32_t mAhDrawn; // milliampere hours drawn from the battery since start - float mAhDrawnF; } currentMeter_t; // NOTE: currentMeterConfig is only used by physical and virtual current meters, not ESC based current meters. @@ -86,6 +43,27 @@ typedef enum { // WARNING - do not mix usage of CURRENT_SENSOR_* and CURRENT_METER_*, they are separate concerns. +typedef struct currentMeterMAhDrawnState_s { + int32_t mAhDrawn; // milliampere hours drawn from the battery since start + float mAhDrawnF; +} currentMeterMAhDrawnState_t; + +typedef struct currentMeterADCState_s { + currentMeterMAhDrawnState_t mahDrawnState; + int32_t amperage; // current read by current sensor in centiampere (1/100th A) + int32_t amperageLatest; // current read by current sensor in centiampere (1/100th A) (unfiltered) +} currentMeterADCState_t; + +typedef struct currentMeterVirtualState_s { + currentMeterMAhDrawnState_t mahDrawnState; + int32_t amperage; // current read by current sensor in centiampere (1/100th A) +} currentMeterVirtualState_t; + +typedef struct currentMeterESCState_s { + int32_t mAhDrawn; // milliampere hours drawn from the battery since start + int32_t amperage; // current read by current sensor in centiampere (1/100th A) +} currentMeterESCState_t; + typedef struct currentMeterADCOrVirtualConfig_s { int16_t scale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A uint16_t offset; // offset of the current sensor in millivolt steps @@ -93,10 +71,28 @@ typedef struct currentMeterADCOrVirtualConfig_s { PG_DECLARE_ARRAY(currentMeterADCOrVirtualConfig_t, MAX_ADC_OR_VIRTUAL_CURRENT_METERS, currentMeterADCOrVirtualConfig); +// +// Main API +// + +void currentMeterReset(currentMeter_t *meter); + void currentMeterADCInit(void); +void currentMeterADCRefresh(int32_t lastUpdateAt); +void currentMeterADCRead(currentMeter_t *meter); -void currentUpdateADCMeter(currentMeter_t *state, int32_t lastUpdateAt); -void currentUpdateESCMeter(currentMeter_t *state, int32_t lastUpdateAt); -void currentUpdateVirtualMeter(currentMeter_t *state, int32_t lastUpdateAt, bool armed, bool throttleLowAndMotorStop, int32_t throttleOffset); +void currentMeterVirtualInit(void); +void currentMeterVirtualRefresh(int32_t lastUpdateAt, bool armed, bool throttleLowAndMotorStop, int32_t throttleOffset); +void currentMeterVirtualRead(currentMeter_t *meter); -void resetCurrentMeterState(currentMeter_t *state); +void currentMeterESCInit(void); +void currentMeterESCRefresh(int32_t lastUpdateAt); +void currentMeterESCReadCombined(currentMeter_t *meter); +void currentMeterESCReadMotor(uint8_t motorNumber, currentMeter_t *meter); + +// +// API for reading current meters by id. +// +extern const uint8_t supportedCurrentMeterCount; +extern const uint8_t currentMeterIds[]; +void currentMeterRead(currentMeterId_e id, currentMeter_t *currentMeter); diff --git a/src/main/sensors/current_ids.h b/src/main/sensors/current_ids.h new file mode 100644 index 000000000..f61028e6c --- /dev/null +++ b/src/main/sensors/current_ids.h @@ -0,0 +1,69 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +// +// fixed ids, current can be measured at many different places, these identifiers are the ones we support or would consider supporting. +// + +typedef enum { + CURRENT_METER_ID_NONE = 0, + + CURRENT_METER_ID_VBAT_1 = 10, // 10-19 for battery meters + CURRENT_METER_ID_VBAT_2, + //.. + CURRENT_METER_ID_VBAT_10 = 19, + + CURRENT_METER_ID_5V_1 = 20, // 20-29 for 5V meters + CURRENT_METER_ID_5V_2, + //.. + CURRENT_METER_ID_5V_10 = 29, + + CURRENT_METER_ID_9V_1 = 30, // 30-39 for 9V meters + CURRENT_METER_ID_9V_2, + //.. + CURRENT_METER_ID_9V_10 = 39, + + CURRENT_METER_ID_12V_1 = 40, // 40-49 for 12V meters + CURRENT_METER_ID_12V_2, + //.. + CURRENT_METER_ID_12V_10 = 49, + + CURRENT_METER_ID_ESC_COMBINED_1 = 50, // 50-59 for ESC combined (it's doubtful an FC would ever expose 51-59 however) + // ... + CURRENT_METER_ID_ESC_COMBINED_10 = 59, + + CURRENT_METER_ID_ESC_MOTOR_1 = 60, // 60-79 for ESC motors (20 motors) + CURRENT_METER_ID_ESC_MOTOR_2, + CURRENT_METER_ID_ESC_MOTOR_3, + CURRENT_METER_ID_ESC_MOTOR_4, + CURRENT_METER_ID_ESC_MOTOR_5, + CURRENT_METER_ID_ESC_MOTOR_6, + CURRENT_METER_ID_ESC_MOTOR_7, + CURRENT_METER_ID_ESC_MOTOR_8, + CURRENT_METER_ID_ESC_MOTOR_9, + CURRENT_METER_ID_ESC_MOTOR_10, + CURRENT_METER_ID_ESC_MOTOR_11, + CURRENT_METER_ID_ESC_MOTOR_12, + //... + CURRENT_METER_ID_ESC_MOTOR_20 = 79, + + CURRENT_METER_ID_VIRTUAL_1 = 80, // 80-89 for virtual meters + CURRENT_METER_ID_VIRTUAL_2, + +} currentMeterId_e; diff --git a/src/main/sensors/voltage.c b/src/main/sensors/voltage.c index 035b02518..88a3fe1fa 100644 --- a/src/main/sensors/voltage.c +++ b/src/main/sensors/voltage.c @@ -37,6 +37,49 @@ #include "sensors/voltage.h" #include "sensors/esc_sensor.h" +const uint8_t voltageMeterIds[] = { + VOLTAGE_METER_ID_VBAT_1, +#ifdef ADC_POWER_12V + VOLTAGE_METER_ID_12V_1, +#endif +#ifdef ADC_POWER_9V + VOLTAGE_METER_ID_9V_1, +#endif +#ifdef ADC_POWER_5V + VOLTAGE_METER_ID_5V_1, +#endif +#ifdef USE_ESC_SENSOR + VOLTAGE_METER_ID_ESC_COMBINED_1, + VOLTAGE_METER_ID_ESC_MOTOR_1, + VOLTAGE_METER_ID_ESC_MOTOR_2, + VOLTAGE_METER_ID_ESC_MOTOR_3, + VOLTAGE_METER_ID_ESC_MOTOR_4, + VOLTAGE_METER_ID_ESC_MOTOR_5, + VOLTAGE_METER_ID_ESC_MOTOR_6, + VOLTAGE_METER_ID_ESC_MOTOR_7, + VOLTAGE_METER_ID_ESC_MOTOR_8, + VOLTAGE_METER_ID_ESC_MOTOR_9, + VOLTAGE_METER_ID_ESC_MOTOR_10, + VOLTAGE_METER_ID_ESC_MOTOR_11, + VOLTAGE_METER_ID_ESC_MOTOR_12, +#endif +}; + +const uint8_t supportedVoltageMeterCount = ARRAYLEN(voltageMeterIds); + +// +// ADC/ESC shared +// + +void voltageMeterReset(voltageMeter_t *meter) +{ + meter->filtered = 0; + meter->unfiltered = 0; +} +// +// ADC +// + #ifndef VBAT_SCALE_DEFAULT #define VBAT_SCALE_DEFAULT 110 #endif @@ -49,16 +92,6 @@ #define VBAT_RESDIVMULTIPLIER_DEFAULT 1 #endif -#ifdef USE_ESC_SENSOR -typedef struct voltageMeterESCState_s { - uint16_t voltageFiltered; // battery voltage in 0.1V steps (filtered) - uint16_t voltageUnfiltered; // battery voltage in 0.1V steps (unfiltered) - biquadFilter_t filter; -} voltageMeterESCState_t; - -static voltageMeterESCState_t voltageMeterESCState; -#endif - typedef struct voltageMeterADCState_s { uint16_t voltageFiltered; // battery voltage in 0.1V steps (filtered) uint16_t voltageUnfiltered; // battery voltage in 0.1V steps (unfiltered) @@ -67,7 +100,6 @@ typedef struct voltageMeterADCState_s { extern voltageMeterADCState_t voltageMeterADCStates[MAX_VOLTAGE_SENSOR_ADC]; - voltageMeterADCState_t voltageMeterADCStates[MAX_VOLTAGE_SENSOR_ADC]; voltageMeterADCState_t *getVoltageMeterADC(uint8_t index) @@ -148,11 +180,19 @@ void voltageMeterADCInit(void) } } -void voltageMeterReset(voltageMeter_t *meter) -{ - meter->filtered = 0; - meter->unfiltered = 0; -} +// +// ESC +// + +#ifdef USE_ESC_SENSOR +typedef struct voltageMeterESCState_s { + uint16_t voltageFiltered; // battery voltage in 0.1V steps (filtered) + uint16_t voltageUnfiltered; // battery voltage in 0.1V steps (unfiltered) + biquadFilter_t filter; +} voltageMeterESCState_t; + +static voltageMeterESCState_t voltageMeterESCState; +#endif #define VBAT_LPF_FREQ 0.4f @@ -196,36 +236,40 @@ void voltageMeterESCReadCombined(voltageMeter_t *voltageMeter) #endif } -void voltageReadMeter(voltageMeterId_e id, voltageMeter_t *voltageMeter) +// +// API for voltage meters using IDs +// + +void voltageMeterRead(voltageMeterId_e id, voltageMeter_t *meter) { if (id == VOLTAGE_METER_ID_VBAT_1) { - voltageMeterADCRead(VOLTAGE_SENSOR_ADC_VBAT, voltageMeter); + voltageMeterADCRead(VOLTAGE_SENSOR_ADC_VBAT, meter); } else #ifdef ADC_POWER_12V if (id == VOLTAGE_METER_ID_12V_1) { - voltageMeterADCRead(VOLTAGE_SENSOR_ADC_12V, voltageMeter); + voltageMeterADCRead(VOLTAGE_SENSOR_ADC_12V, meter); } else #endif #ifdef ADC_POWER_9V if (id == VOLTAGE_METER_ID_9V_1) { - voltageMeterADCRead(VOLTAGE_SENSOR_ADC_9V, voltageMeter); + voltageMeterADCRead(VOLTAGE_SENSOR_ADC_9V, meter); } else #endif #ifdef ADC_POWER_5V if (id == VOLTAGE_METER_ID_5V_1) { - voltageMeterADCRead(VOLTAGE_SENSOR_ADC_5V, voltageMeter); + voltageMeterADCRead(VOLTAGE_SENSOR_ADC_5V, meter); } else #endif #ifdef USE_ESC_SENSOR if (id == VOLTAGE_METER_ID_ESC_COMBINED_1) { - voltageMeterESCReadCombined(voltageMeter); + voltageMeterESCReadCombined(meter); } else if (id >= VOLTAGE_METER_ID_ESC_MOTOR_1 && id <= VOLTAGE_METER_ID_ESC_MOTOR_20 ) { int motor = id - VOLTAGE_METER_ID_ESC_MOTOR_1; - voltageMeterESCReadMotor(motor, voltageMeter); + voltageMeterESCReadMotor(motor, meter); } else #endif { - voltageMeterReset(voltageMeter); + voltageMeterReset(meter); } } diff --git a/src/main/sensors/voltage.h b/src/main/sensors/voltage.h index be5451af8..c8e6aff3d 100644 --- a/src/main/sensors/voltage.h +++ b/src/main/sensors/voltage.h @@ -17,6 +17,8 @@ #pragma once +#include "voltage_ids.h" + // // meters // @@ -32,56 +34,6 @@ typedef struct voltageMeter_s { uint16_t unfiltered; // voltage in 0.1V steps } voltageMeter_t; -// -// fixed ids, voltage can be measured at many different places, these identifiers are the ones we support or would consider supporting. -// - -typedef enum { - VOLTAGE_METER_ID_NONE = 0, - - VOLTAGE_METER_ID_VBAT_1 = 10, // 10-19 for battery meters - VOLTAGE_METER_ID_VBAT_2, - //.. - VOLTAGE_METER_ID_VBAT_10 = 19, - - VOLTAGE_METER_ID_5V_1 = 20, // 20-29 for 5V meters - VOLTAGE_METER_ID_5V_2, - //.. - VOLTAGE_METER_ID_5V_10 = 29, - - VOLTAGE_METER_ID_9V_1 = 30, // 30-39 for 9V meters - VOLTAGE_METER_ID_9V_2, - //.. - VOLTAGE_METER_ID_9V_10 = 39, - - VOLTAGE_METER_ID_12V_1 = 40, // 40-49 for 12V meters - VOLTAGE_METER_ID_12V_2, - //.. - VOLTAGE_METER_ID_12V_10 = 49, - - VOLTAGE_METER_ID_ESC_COMBINED_1 = 50, // 50-59 for ESC combined (it's doubtful an FC would ever expose 51-59 however) - // ... - VOLTAGE_METER_ID_ESC_COMBINED_10 = 59, - - VOLTAGE_METER_ID_ESC_MOTOR_1 = 60, // 60-79 for ESC motors (20 motors) - VOLTAGE_METER_ID_ESC_MOTOR_2, - VOLTAGE_METER_ID_ESC_MOTOR_3, - VOLTAGE_METER_ID_ESC_MOTOR_4, - VOLTAGE_METER_ID_ESC_MOTOR_5, - VOLTAGE_METER_ID_ESC_MOTOR_6, - VOLTAGE_METER_ID_ESC_MOTOR_7, - VOLTAGE_METER_ID_ESC_MOTOR_8, - //... - VOLTAGE_METER_ID_ESC_MOTOR_20 = 79, - - VOLTAGE_METER_ID_CELL_1 = 80, // 80-119 for cell meters (40 cells) - VOLTAGE_METER_ID_CELL_2, - //... - VOLTAGE_METER_ID_CELL_40 = 119, - -} voltageMeterId_e; - - // // sensors // @@ -121,6 +73,11 @@ typedef struct voltageSensorADCConfig_s { PG_DECLARE_ARRAY(voltageSensorADCConfig_t, MAX_VOLTAGE_SENSOR_ADC, voltageSensorADCConfig); +// +// Main API +// +void voltageMeterReset(voltageMeter_t *voltageMeter); + void voltageMeterADCInit(void); void voltageMeterADCRefresh(void); void voltageMeterADCRead(voltageSensorADC_e adcChannel, voltageMeter_t *voltageMeter); @@ -130,6 +87,10 @@ void voltageMeterESCRefresh(void); void voltageMeterESCReadCombined(voltageMeter_t *voltageMeter); void voltageMeterESCReadMotor(uint8_t motor, voltageMeter_t *voltageMeter); -void voltageMeterReset(voltageMeter_t *voltageMeter); -void voltageReadMeter(voltageMeterId_e id, voltageMeter_t *voltageMeter); +// +// API for reading current meters by id. +// +extern const uint8_t supportedVoltageMeterCount; +extern const uint8_t voltageMeterIds[]; +void voltageMeterRead(voltageMeterId_e id, voltageMeter_t *voltageMeter); diff --git a/src/main/sensors/voltage_ids.h b/src/main/sensors/voltage_ids.h new file mode 100644 index 000000000..ba3344fc2 --- /dev/null +++ b/src/main/sensors/voltage_ids.h @@ -0,0 +1,71 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +// +// fixed ids, voltage can be measured at many different places, these identifiers are the ones we support or would consider supporting. +// + +typedef enum { + VOLTAGE_METER_ID_NONE = 0, + + VOLTAGE_METER_ID_VBAT_1 = 10, // 10-19 for battery meters + VOLTAGE_METER_ID_VBAT_2, + //.. + VOLTAGE_METER_ID_VBAT_10 = 19, + + VOLTAGE_METER_ID_5V_1 = 20, // 20-29 for 5V meters + VOLTAGE_METER_ID_5V_2, + //.. + VOLTAGE_METER_ID_5V_10 = 29, + + VOLTAGE_METER_ID_9V_1 = 30, // 30-39 for 9V meters + VOLTAGE_METER_ID_9V_2, + //.. + VOLTAGE_METER_ID_9V_10 = 39, + + VOLTAGE_METER_ID_12V_1 = 40, // 40-49 for 12V meters + VOLTAGE_METER_ID_12V_2, + //.. + VOLTAGE_METER_ID_12V_10 = 49, + + VOLTAGE_METER_ID_ESC_COMBINED_1 = 50, // 50-59 for ESC combined (it's doubtful an FC would ever expose 51-59 however) + // ... + VOLTAGE_METER_ID_ESC_COMBINED_10 = 59, + + VOLTAGE_METER_ID_ESC_MOTOR_1 = 60, // 60-79 for ESC motors (20 motors) + VOLTAGE_METER_ID_ESC_MOTOR_2, + VOLTAGE_METER_ID_ESC_MOTOR_3, + VOLTAGE_METER_ID_ESC_MOTOR_4, + VOLTAGE_METER_ID_ESC_MOTOR_5, + VOLTAGE_METER_ID_ESC_MOTOR_6, + VOLTAGE_METER_ID_ESC_MOTOR_7, + VOLTAGE_METER_ID_ESC_MOTOR_8, + VOLTAGE_METER_ID_ESC_MOTOR_9, + VOLTAGE_METER_ID_ESC_MOTOR_10, + VOLTAGE_METER_ID_ESC_MOTOR_11, + VOLTAGE_METER_ID_ESC_MOTOR_12, + //... + VOLTAGE_METER_ID_ESC_MOTOR_20 = 79, + + VOLTAGE_METER_ID_CELL_1 = 80, // 80-119 for cell meters (40 cells) + VOLTAGE_METER_ID_CELL_2, + //... + VOLTAGE_METER_ID_CELL_40 = 119, + +} voltageMeterId_e;