diff --git a/.travis.sh b/.travis.sh old mode 100755 new mode 100644 diff --git a/docs/Battery.md b/docs/Battery.md index c233c1194..aac57415d 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/config/config.c b/src/main/config/config.c index d3b7db24c..6ec55de3d 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -227,7 +227,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/io/serial_cli.c b/src/main/io/serial_cli.c index 72338bc42..12185fd4e 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_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, -10000, 10000 }, { "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, 0, 3300 }, { "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/main.c b/src/main/main.c index 08c1e6469..3e300b815 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -290,7 +290,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); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 86539cb3f..a367cb8f8 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 @@ -113,13 +118,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 += (amperageLatestADC = adcGetChannel(ADC_CURRENT)); - amperage = currentSensorToCentiamps(amperageRaw / 8); + switch(batteryConfig->currentMeterType) { + case CURRENT_SENSOR_ADC: + amperageRaw -= amperageRaw / 8; + amperageRaw += (amperageLatestADC = 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 aa5878ad9..60404b0f5 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -21,6 +21,13 @@ #define VBAT_SCALE_MIN 0 #define VBAT_SCALE_MAX 255 +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 uint8_t vbatmaxcellvoltage; // maximum voltage per cell, used for auto-detecting battery voltage in 0.1V units, default is 43 (4.3V) @@ -29,6 +36,7 @@ typedef struct batteryConfig_s { int16_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