From 999f0ce002995aef00f24696415e89ba68815d48 Mon Sep 17 00:00:00 2001 From: tracernz Date: Fri, 16 Jan 2015 22:00:45 +1300 Subject: [PATCH 1/3] Add virtual current sensor support Virtual current sensor calculates an estimate of current based on throttle position, current_meter_scale, and current_meter_offset. Documentation to follow later. --- src/main/config/config.c | 2 +- src/main/io/serial_cli.c | 2 +- src/main/sensors/battery.c | 31 ++++++++++++++++++++++++++----- src/main/sensors/battery.h | 10 ++++++++++ 4 files changed, 38 insertions(+), 7 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index f4b0dca420..457bf30212 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -108,7 +108,7 @@ profile_t *currentProfile; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 88; +static const uint8_t EEPROM_CONF_VERSION = 89; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 428496c159..7a952e7ff0 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -305,7 +305,7 @@ const clivalue_t valueTable[] = { { "current_meter_scale", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, 1, 10000 }, { "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, 0, 1650 }, { "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, 0, 1 }, - + { "current_meter_type", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterType, 0, CURRENT_SENSOR_MAX }, { "align_gyro", VAR_UINT8 | MASTER_VALUE, &masterConfig.sensorAlignmentConfig.gyro_align, 0, 8 }, { "align_acc", VAR_UINT8 | MASTER_VALUE, &masterConfig.sensorAlignmentConfig.acc_align, 0, 8 }, diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 9766d40130..f3c3e80288 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -20,9 +20,14 @@ #include "common/maths.h" +#include "config/runtime_config.h" + #include "drivers/adc.h" #include "drivers/system.h" +#include "rx/rx.h" +#include "io/rc_controls.h" + #include "sensors/battery.h" // Battery monitoring stuff @@ -112,13 +117,29 @@ void updateCurrentMeter(int32_t lastUpdateAt) { static int32_t amperageRaw = 0; static int64_t mAhdrawnRaw = 0; + int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; + int32_t throttleFactor = 0; - amperageRaw -= amperageRaw / 8; - amperageRaw += adcGetChannel(ADC_CURRENT); - amperage = currentSensorToCentiamps(amperageRaw / 8); + switch(batteryConfig->currentMeterType) { + case CURRENT_SENSOR_ADC: + amperageRaw -= amperageRaw / 8; + amperageRaw += adcGetChannel(ADC_CURRENT); + amperage = currentSensorToCentiamps(amperageRaw / 8); + break; + case CURRENT_SENSOR_VIRTUAL: + amperage = (int32_t)batteryConfig->currentMeterOffset; + if(ARMING_FLAG(ARMED)) { + throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); + amperage += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000; + } + break; + case CURRENT_SENSOR_NONE: + amperage = 0; + break; + } - mAhdrawnRaw += (amperage * lastUpdateAt) / 1000; - mAhDrawn = mAhdrawnRaw / (3600 * 100); + mAhdrawnRaw += (amperage * lastUpdateAt) / 1000; + mAhDrawn = mAhdrawnRaw / (3600 * 100); } uint8_t calculateBatteryPercentage(void) diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index f90395f5fc..eaf989347f 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -20,6 +20,15 @@ #define VBAT_SCALE_DEFAULT 110 #define VBAT_SCALE_MIN 0 #define VBAT_SCALE_MAX 255 +#define VIRTUAL_CURRENT_MIN 0 +#define VIRTUAL_CURRENT_MAX 0xffff + +typedef enum { + CURRENT_SENSOR_NONE = 0, + CURRENT_SENSOR_ADC, + CURRENT_SENSOR_VIRTUAL, + CURRENT_SENSOR_MAX = CURRENT_SENSOR_VIRTUAL +} currentSensor_e; typedef struct batteryConfig_s { uint8_t vbatscale; // adjust this to match battery voltage to reported value @@ -29,6 +38,7 @@ typedef struct batteryConfig_s { uint16_t currentMeterScale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A uint16_t currentMeterOffset; // offset of the current sensor in millivolt steps + currentSensor_e currentMeterType; // type of current meter used, either ADC or virtual // FIXME this doesn't belong in here since it's a concern of MSP, not of the battery code. uint8_t multiwiiCurrentMeterOutput; // if set to 1 output the amperage in milliamp steps instead of 0.01A steps via msp From a81fad9cc2bdd148cf6a8377b3bb5bfafde87222 Mon Sep 17 00:00:00 2001 From: tracernz Date: Fri, 16 Jan 2015 22:47:10 +1300 Subject: [PATCH 2/3] Add virtual current sensor documentation ..and remove a couple of defines I didn't use. --- docs/Battery.md | 32 +++++++++++++++++++++++++++++++- src/main/sensors/battery.h | 2 -- 2 files changed, 31 insertions(+), 3 deletions(-) diff --git a/docs/Battery.md b/docs/Battery.md index c233c11947..aac57415d4 100644 --- a/docs/Battery.md +++ b/docs/Battery.md @@ -74,8 +74,19 @@ Enable current monitoring using the CLI command feature CURRENT_METER ``` +Configure the current meter type using the `current_meter_type` settings as per the following table. + +| Value | Sensor Type | +| ----- | ---------------------- | +| 0 | None | +| 1 | ADC/hardware sensor | +| 2 | Virtual sensor | + Configure capacity using the `battery_capacity` setting, which takes a value in mAh. +If you're using an OSD that expects the multiwii current meter output value, then set `multiwii_current_meter_output` to `1` (this multiplies amperage sent to MSP by 10). + +### ADC Sensor The current meter may need to be configured so that the value read at the ADC input matches actual current draw. Just like you need a voltmeter to correctly calibrate your voltage reading you also need an ammeter to calibrate your current sensor. Use the following settings to adjust calibration. @@ -83,4 +94,23 @@ Use the following settings to adjust calibration. `current_meter_scale` `current_meter_offset` -If you're using an OSD that expects the multiwii current meter output value, then set `multiwii_current_meter_output` to `1`. +### Virtual Sensor +The virtual sensor uses the throttle position to calculate as estimated current value. This is useful when a real sensor is not available. The following settings adjust the calibration. + +| Setting | Description | +| ----------------------------- | -------------------------------------------------------- | +| `current_meter_scale` | The throttle scaling factor [centiamps, i.e. 1/100th A] | +| `current_meter_offset` | The current at zero throttle [centiamps, i.e. 1/100th A] | + +If you know your current at zero throttle (Imin) and maximum throttle(Imax) you can calculate the scaling factors using the following formulas where Tmax is maximum throttle offset (i.e. for `max_throttle` = 1850, Tmax = 1850 - 1000 = 850): +``` +current_meter_scale = (Imax - Imin) * 100000 / (Tmax + (Tmax * Tmax / 50)) +current_meter_offset = Imin * 100 +``` +e.g. For a maximum current of 34.2 A and minimum current of 2.8 A with `max_throttle` = 1850 +``` +current_meter_scale = (Imax - Imin) * 100000 / (Tmax + (Tmax * Tmax / 50)) + = (34.2 - 2.8) * 100000 / (850 + (850 * 850 / 50)) + = 205 +current_meter_offset = Imin * 100 = 280 +``` diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index eaf989347f..38d2dd17d7 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -20,8 +20,6 @@ #define VBAT_SCALE_DEFAULT 110 #define VBAT_SCALE_MIN 0 #define VBAT_SCALE_MAX 255 -#define VIRTUAL_CURRENT_MIN 0 -#define VIRTUAL_CURRENT_MAX 0xffff typedef enum { CURRENT_SENSOR_NONE = 0, From 6868999d636def7428845cc671746d3a7c248224 Mon Sep 17 00:00:00 2001 From: tracernz Date: Sun, 25 Jan 2015 19:27:58 +1300 Subject: [PATCH 3/3] Set default current meter type Current meter type added to resetBatteryConfig in config.c so it defaults to ADC. PWM mapping skips the current meter ADC pin only if used. --- src/main/config/config.c | 2 +- src/main/main.c | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 58f3a5ea87..ea8d512673 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -225,7 +225,7 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig) batteryConfig->currentMeterOffset = 0; batteryConfig->currentMeterScale = 400; // for Allegro ACS758LCB-100U (40mV/A) batteryConfig->batteryCapacity = 0; - + batteryConfig->currentMeterType = CURRENT_SENSOR_ADC; } void resetSerialConfig(serialConfig_t *serialConfig) diff --git a/src/main/main.c b/src/main/main.c index 4a9656b8df..bcbd18412e 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -287,7 +287,8 @@ void init(void) pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); - pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER); + pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER) + && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC; pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useOneshot = feature(FEATURE_ONESHOT125);